# 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
                traj = base_manip.MoveHandStraight(direction, minsteps=10 * steps, maxsteps=steps, steplength=step_length,
                                                   ignorefirstcollision=None, starteematrix=None, greedysearch=True, execute=False, outputtraj=None, maxdeviationangle=None,
                                                   planner=None, outputtrajobj=True)
                return list(sample_manipulator_trajectory(manipulator, traj))
            except planning_error:
                return None


def aabb_min(aabb): return aabb.pos() - aabb.extents()


def aabb_max(aabb): return aabb.pos() + aabb.extents()


class AASurface(object):

    def __init__(self, name, xy_min, xy_max, z, color=(0, 0, 0, .25)):
        self.xy_min = xy_min
        self.xy_max = xy_max
        self.z = z
        self.name = name
        self.color = color

    def supports(self, body):
        aabb = body.ComputeAABB()
        return np.all(self.xy_min <= aabb_min(aabb)[:2]) and np.all(aabb_max(aabb)[:2] <= self.xy_max) and abs(aabb_min(aabb)[2] - self.z) < 1e-6

    def sample_placement(self, body, max_attempts=10):
        with body.CreateKinBodyStateSaver():
            for _ in irange(0, max_attempts):

# 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

                traj = base_manip.MoveHandStraight(direction, minsteps=10 * steps, maxsteps=steps, steplength=step_length,
                                                   ignorefirstcollision=None, starteematrix=None, greedysearch=True, execute=False, outputtraj=None, maxdeviationangle=None,
                                                   planner=None, outputtrajobj=True)
                return list(sample_manipulator_trajectory(manipulator, traj))
            except planning_error:
                return None


def aabb_min(aabb): return aabb.pos() - aabb.extents()


def aabb_max(aabb): return aabb.pos() + aabb.extents()


class AASurface(object):

    def __init__(self, name, xy_min, xy_max, z, color=(0, 0, 0, .25)):
        self.xy_min = xy_min
        self.xy_max = xy_max
        self.z = z
        self.name = name
        self.color = color

    def supports(self, body):
        aabb = body.ComputeAABB()
        return np.all(self.xy_min <= aabb_min(aabb)[:2]) and np.all(aabb_max(aabb)[:2] <= self.xy_max) and abs(aabb_min(aabb)[2] - self.z) < 1e-6

    def sample_placement(self, body, max_attempts=10):
        with body.CreateKinBodyStateSaver():
            for _ in irange(0, max_attempts):

                quat = quat_from_axis_angle(0, 0, uniform(0, 2 * math.pi))