# 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

            yield base_trans
    except (ValueError, planning_error):
        raise StopIteration


def solve_inverse_kinematics(manipulator, manip_trans):
    robot = manipulator.GetRobot()
    env = robot.GetEnv()
    with robot:
        robot.SetActiveDOFs(manipulator.GetArmIndices())

        config = manipulator.FindIKSolution(manip_trans, 0)
        if config is None:
            return None
        robot.SetDOFValues(config, manipulator.GetArmIndices())
        if env.CheckCollision(robot) or robot.CheckSelfCollision():
            return None
        return config


def get_collision_fn(env, body, self_collisions=False):
    def fn(q):
        body.SetActiveDOFValues(q)
        return env.CheckCollision(body) or (self_collisions and body.CheckSelfCollision())
    return fn


def get_sample_fn(env, body, collisions=False, self_collisions=False):
    limits = body.GetActiveDOFLimits()

# 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


            yield base_trans
    except (ValueError, planning_error):
        raise StopIteration


def solve_inverse_kinematics(manipulator, manip_trans):
    robot = manipulator.GetRobot()
    env = robot.GetEnv()
    with robot:
        robot.SetActiveDOFs(manipulator.GetArmIndices())

        config = manipulator.FindIKSolution(manip_trans, 0)
        if config is None:
            return None
        robot.SetDOFValues(config, manipulator.GetArmIndices())
        if env.CheckCollision(robot) or robot.CheckSelfCollision():
            return None
        return config


def get_collision_fn(env, body, self_collisions=False):
    def fn(q):
        body.SetActiveDOFValues(q)
        return env.CheckCollision(body) or (self_collisions and body.CheckSelfCollision())
    return fn


def get_sample_fn(env, body, collisions=False, self_collisions=False):
    limits = body.GetActiveDOFLimits()

    collision = get_collision_fn(env, body, self_collisions=self_collisions)