# 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
def set_transparency(body, transparency):
    for geometry in get_geometries(body):
        geometry.SetTransparency(transparency)


def get_box_dimensions(box):
    [link] = box.GetLinks()
    [geometry] = link.GetGeometries()
    assert geometry.GetType() == GeometryType.Box
    return geometry.GetBoxExtents()


def box_body(env, name, dx, dy, dz, color=None, transparency=None):
    body = RaveCreateKinBody(env, '')
    body.InitFromBoxes(
        np.array([[0, 0, .5 * dz, .5 * dx, .5 * dy, .5 * dz]]), draw=True)
    body.SetName(name)
    if color is not None:
        set_color(body, color)
    if transparency is not None:
        set_transparency(body, transparency)
    return body


def mirror_arm_config(robot, config):
    return config * np.array([1 if left_min == right_min else -1 for left_min, right_min in
                              zip(robot.GetDOFLimits(robot.GetManipulator('leftarm').GetArmIndices())[0],
                                  robot.GetDOFLimits(robot.GetManipulator('rightarm').GetArmIndices())[0])])



# 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

def set_transparency(body, transparency):
    for geometry in get_geometries(body):
        geometry.SetTransparency(transparency)


def get_box_dimensions(box):
    [link] = box.GetLinks()
    [geometry] = link.GetGeometries()
    assert geometry.GetType() == GeometryType.Box
    return geometry.GetBoxExtents()


def box_body(env, name, dx, dy, dz, color=None, transparency=None):
    body = RaveCreateKinBody(env, '')
    body.InitFromBoxes(
        np.array([[0, 0, .5 * dz, .5 * dx, .5 * dy, .5 * dz]]), draw=True)
    body.SetName(name)
    if color is not None:
        set_color(body, color)
    if transparency is not None:
        set_transparency(body, transparency)
    return body


def mirror_arm_config(robot, config):
    return config * np.array([1 if left_min == right_min else -1 for left_min, right_min in
                              zip(robot.GetDOFLimits(robot.GetManipulator('leftarm').GetArmIndices())[0],
                                  robot.GetDOFLimits(robot.GetManipulator('rightarm').GetArmIndices())[0])])



def set_manipulator_conf(manipulator, values):