def quat_from_axis_angle(x_angle, y_angle, z_angle):
    return quatFromAxisAngle(np.array((x_angle, y_angle, z_angle))) def trans_from_pose(pose):
    return matrixFromPose(pose) def manip_trans_from_object_trans(object_trans, grasp):

    return np.linalg.solve(grasp.T, object_trans.T).T def length(vector):
    return np.linalg.norm(vector) def normalize(vector):
    return 1. / length(vector) * vector def trans_from_point(x, y, z):
    return trans_from_quat_point(unit_quat(), np.array([x, y, z])) def get_point(body):
    return point_from_trans(get_trans(body)) def trans_from_quat(quat):
    return matrixFromQuat(quat) def trans_from_axis_angle(x_angle, y_angle, z_angle):
    return trans_from_quat(quat_from_axis_angle(x_angle, y_angle, z_angle)) def get_active_config(body):
    return body.GetActiveDOFValues() def set_active_config(body, config):
    body.SetActiveDOFValues(config) def set_quat(body, quat):
    set_pose(body, pose_from_quat_point(quat, get_point(body))) def pose_from_quat_point(quat, point):
    return np.concatenate([quat, point]) def irange(start, end, step=1):
    n = start
    while n < end:
        yield n
        n += step INF = float('inf')
from itertools import count
from random import uniform
from openravepy import rotationMatrixFromAxisAngle, Sensor, RaveCreateCollisionChecker, databases, interfaces,  IkParameterization, GeometryType, RaveCreateKinBody, planning_error, openravepy_int,  CollisionOptionsStateSaver, DOFAffine, RaveGetEnvironment, RaveGetAffineDOFValuesFromTransform
from openravepy.misc import SetViewerUserThread
from robotics.openrave.transforms import quat_from_axis_angle, trans_from_pose, manip_trans_from_object_trans,  length, normalize, trans_from_point, get_point,  trans_from_quat, trans_from_axis_angle, get_active_config, set_active_config, set_quat,  pose_from_quat_point
from stripstream.utils import irange, INF
    from motion_planners.rrt_connect import birrt
    from motion_planners.rrt_connect import direct_path
import math
import sys
import numpy as np
        import motion_planners

    cd_model = databases.convexdecomposition.ConvexDecompositionModel(robot)
    if not cd_model.load():
        print 'Generating convex decomposition model'
        cd_model.autogenerate()
    l_model = databases.linkstatistics.LinkStatisticsModel(robot)

    if not l_model.load():
        print 'Generating link statistics model'
        l_model.autogenerate()
    l_model.setRobotWeights()
    l_model.setRobotResolutions(xyzdelta=min_delta)

    robot.SetActiveManipulator(manipulator_name)
    manipulator = robot.GetManipulator(manipulator_name)
    robot.SetActiveDOFs(manipulator.GetArmIndices())
    ikmodel = databases.inversekinematics.InverseKinematicsModel(robot=robot, iktype=IkParameterization.Type.Transform6D,
                                                                 forceikfast=True, freeindices=None, freejoints=None, manip=None)
    if not ikmodel.load():
        print 'Generating inverse kinematics model'
        ikmodel.autogenerate()

    base_manip = interfaces.BaseManipulation(
        robot, plannername=None, maxvelmult=None)
    ir_model = databases.inversereachability.InverseReachabilityModel(robot)
    if ir_model.load():
        print 'Generating inverse reachability model'
        ir_model.autogenerate()

    return robot, manipulator, base_manip, ir_model


def collision_saver(env, options):
    return CollisionOptionsStateSaver(env.GetCollisionChecker(), options)


def has_mp():
    try:
    except ImportError:
        return False
    return True


def mp_birrt(robot, q1, q2):
    return birrt(q1, q2, get_distance_fn(robot),
                 get_sample_fn(robot.GetEnv(), robot),
                 get_extend_fn(robot), get_collision_fn(robot.GetEnv(), robot))


def mp_straight_line(robot, q1, q2):
    return direct_path(q1, q2, get_extend_fn(robot),
                       get_collision_fn(robot.GetEnv(), robot))


def linear_motion_plan(robot, end_config):
    env = robot.GetEnv()
    with robot:

        with collision_saver(env, openravepy_int.CollisionOptions.ActiveDOFs):
            start_config = get_active_config(robot)
            path = [start_config] + \
                list(linear_interpolation(robot, start_config, end_config))
            for conf in path:
                set_active_config(robot, conf)
                if env.CheckCollision(robot):
                    return None
            return path


def set_active(robot, indices=(), use_base=False):
    if use_base is False:
        robot.SetActiveDOFs(indices)
    robot.SetActiveDOFs(indices, AFFINE_MASK, ROTATION_AXIS)


def cspace_motion_plan(base_manip, indices, goal, step_length=MIN_DELTA, max_iterations=10, max_tries=1):
    with base_manip.robot:
        base_manip.robot.SetActiveDOFs(indices)
        with collision_saver(base_manip.robot.GetEnv(), openravepy_int.CollisionOptions.ActiveDOFs):
            try:
                traj = base_manip.MoveActiveJoints(goal=goal, steplength=step_length, maxiter=max_iterations, maxtries=max_tries,
                                                   execute=False, outputtraj=None, goals=None, outputtrajobj=True, jitter=None, releasegil=False, postprocessingplanner=None,
                                                   postprocessingparameters=None)

                return traj

            except planning_error:
                return None


def base_motion_plan(base_manip, goal, step_length=MIN_DELTA, max_iterations=10, max_tries=1):

    with base_manip.robot:
        set_active(base_manip.robot, use_base=True)
        with collision_saver(base_manip.robot.GetEnv(), openravepy_int.CollisionOptions.ActiveDOFs):
            try:
                traj = base_manip.MoveActiveJoints(goal=goal, steplength=step_length, maxiter=max_iterations, maxtries=max_tries,
                                                   execute=False, outputtraj=None, goals=None, outputtrajobj=True, jitter=None, releasegil=False, postprocessingplanner=None,
                                                   postprocessingparameters=None)

                return list(sample_base_trajectory(base_manip.robot, traj))
            except planning_error:
                return None


def manipulator_motion_plan(base_manip, manipulator, goal, step_length=MIN_DELTA, max_iterations=10, max_tries=1):
    with base_manip.robot:
        base_manip.robot.SetActiveManipulator(manipulator)
        base_manip.robot.SetActiveDOFs(manipulator.GetArmIndices())
        with collision_saver(base_manip.robot.GetEnv(), openravepy_int.CollisionOptions.ActiveDOFs):
            try:
                traj = base_manip.MoveManipulator(goal=goal,
                                                  maxiter=max_iterations, execute=False, outputtraj=None, maxtries=max_tries,
                                                  goals=None, steplength=step_length, outputtrajobj=True, jitter=None, releasegil=False)
                return list(sample_manipulator_trajectory(manipulator, traj))
            except planning_error:
                return None


def workspace_motion_plan(base_manip, manipulator, vector, steps=10):

    distance, direction = length(vector), normalize(vector)