# Path: robotics/openrave/transforms.py
# 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])
# 
# Path: stripstream/utils.py
# 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
    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)
    step_length = distance / steps

# Path: robotics/openrave/transforms.py
# 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])
# 
# Path: stripstream/utils.py
# def irange(start, end, step=1):
#     n = start
#     while n < end:
#         yield n
#         n += step
# 
# INF = float('inf')


# Path: robotics/openrave/utils.py
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

    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)
    step_length = distance / steps

    with base_manip.robot: