# 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 extract_base_values(robot, spec, data):
    return RaveGetAffineDOFValuesFromTransform(spec.ExtractTransform(np.identity(4), data, robot), AFFINE_MASK, ROTATION_AXIS)


def sample_base_trajectory(robot, traj):
    spec = traj.GetConfigurationSpecification()
    waypoints = [extract_base_values(robot, spec, traj.GetWaypoint(
        i)) for i in range(traj.GetNumWaypoints())]
    yield waypoints[0]
    for start, end in zip(waypoints, waypoints[1:]):
        for conf in linear_interpolation(robot, start, end):
            yield conf


def execute_viewer(env, execute):
    if sys.platform == 'darwin':
        SetViewerUserThread(env, 'qtcoin', execute)
    else:
        env.SetViewer('qtcoin')
        execute()


def initialize_openrave(env, manipulator_name, min_delta=MIN_DELTA, collision_checker='ode'):
    env.StopSimulation()
    for sensor in env.GetSensors():
        sensor.Configure(Sensor.ConfigureCommand.PowerOff)
        sensor.Configure(Sensor.ConfigureCommand.RenderDataOff)
        sensor.Configure(Sensor.ConfigureCommand.RenderGeometryOff)

# 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 extract_base_values(robot, spec, data):
    return RaveGetAffineDOFValuesFromTransform(spec.ExtractTransform(np.identity(4), data, robot), AFFINE_MASK, ROTATION_AXIS)


def sample_base_trajectory(robot, traj):
    spec = traj.GetConfigurationSpecification()
    waypoints = [extract_base_values(robot, spec, traj.GetWaypoint(
        i)) for i in range(traj.GetNumWaypoints())]
    yield waypoints[0]
    for start, end in zip(waypoints, waypoints[1:]):
        for conf in linear_interpolation(robot, start, end):
            yield conf


def execute_viewer(env, execute):
    if sys.platform == 'darwin':
        SetViewerUserThread(env, 'qtcoin', execute)
    else:
        env.SetViewer('qtcoin')
        execute()


def initialize_openrave(env, manipulator_name, min_delta=MIN_DELTA, collision_checker='ode'):
    env.StopSimulation()
    for sensor in env.GetSensors():
        sensor.Configure(Sensor.ConfigureCommand.PowerOff)
        sensor.Configure(Sensor.ConfigureCommand.RenderDataOff)
        sensor.Configure(Sensor.ConfigureCommand.RenderGeometryOff)

        env.Remove(sensor)