# Path: robotics/openrave/utils.py
# 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 set_manipulator_conf(manipulator, values):
#     manipulator.GetRobot().SetDOFValues(values, manipulator.GetArmIndices())
# 
# class Conf(Wrapper):
#     _ids = count(0)
# 
# class Traj(Wrapper):
#     _ids = count(0)
# 
# def manip_from_pose_grasp(pose, grasp):
#     return manip_trans_from_object_trans(trans_from_pose(pose), grasp)
# 
# 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 linear_motion_plan(robot, end_config):
#     env = robot.GetEnv()
#     with robot:
# 
#         with collision_saver(env, openravepy_int.CollisionOptions.ActiveDOFs):
#             start_config = get_active_config(robot)
#             path = [start_config] + \
#                 list(linear_interpolation(robot, start_config, end_config))
#             for conf in path:
#                 set_active_config(robot, conf)
#                 if env.CheckCollision(robot):
#                     return None
#             return path
# 
# def has_mp():
#     try:
#         import motion_planners
#     except ImportError:
#         return False
#     return True
# 
# def mp_birrt(robot, q1, q2):
#     from motion_planners.rrt_connect import birrt
#     return birrt(q1, q2, get_distance_fn(robot),
#                  get_sample_fn(robot.GetEnv(), robot),
#                  get_extend_fn(robot), get_collision_fn(robot.GetEnv(), robot))
# 
# def mp_straight_line(robot, q1, q2):
#     from motion_planners.rrt_connect import direct_path
#     return direct_path(q1, q2, get_extend_fn(robot),
#                        get_collision_fn(robot.GetEnv(), robot))
# 
# Path: robotics/openrave/transforms.py
# def set_pose(body, pose):
#     set_trans(body, trans_from_pose(pose))
# 
# def object_trans_from_manip_trans(manip_trans, grasp):
#     return np.dot(manip_trans, grasp)
# 
# def trans_from_point(x, y, z):
#     return trans_from_quat_point(unit_quat(), np.array([x, y, z]))


from time import sleep
from robotics.openrave.utils import solve_inverse_kinematics,  set_manipulator_conf, Conf, Traj, manip_from_pose_grasp,  manipulator_motion_plan, linear_motion_plan,  has_mp, mp_birrt, mp_straight_line
from robotics.openrave.transforms import set_pose,  object_trans_from_manip_trans, trans_from_point
import numpy as np

    def cfree_traj(traj, pose):
        if DISABLE_MOTION_COLLISIONS:
            return True
        if traj.pose is not None and traj.pose == pose:

            return True
        return _cfree_traj_pose(traj, pose) and (traj.grasp is None or _cfree_traj_grasp_pose(traj, traj.grasp, pose))
    return cfree_traj


def sample_grasp_traj_fn(env, manipulator, body1, all_bodies):
    robot = manipulator.GetRobot()

    def sample_grasp_traj(pose, grasp):
        enable_all(all_bodies, False)
        body1.Enable(True)
        set_pose(body1, pose.value)
        manip_trans = manip_from_pose_grasp(pose.value, grasp.value)
        grasp_conf = solve_inverse_kinematics(manipulator, manip_trans)
        if grasp_conf is None:
            return
        if DISABLE_MOTIONS:
            yield [(Conf(grasp_conf), Traj([]))]
            return

        set_manipulator_conf(manipulator, grasp_conf)
        robot.Grab(body1)

        pregrasp_trans = manip_trans.dot(trans_from_point(*APPROACH_VECTOR))

# Path: robotics/openrave/utils.py
# 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 set_manipulator_conf(manipulator, values):
#     manipulator.GetRobot().SetDOFValues(values, manipulator.GetArmIndices())
# 
# class Conf(Wrapper):
#     _ids = count(0)
# 
# class Traj(Wrapper):
#     _ids = count(0)
# 
# def manip_from_pose_grasp(pose, grasp):
#     return manip_trans_from_object_trans(trans_from_pose(pose), grasp)
# 
# 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 linear_motion_plan(robot, end_config):
#     env = robot.GetEnv()
#     with robot:
# 
#         with collision_saver(env, openravepy_int.CollisionOptions.ActiveDOFs):
#             start_config = get_active_config(robot)
#             path = [start_config] + \
#                 list(linear_interpolation(robot, start_config, end_config))
#             for conf in path:
#                 set_active_config(robot, conf)
#                 if env.CheckCollision(robot):
#                     return None
#             return path
# 
# def has_mp():
#     try:
#         import motion_planners
#     except ImportError:
#         return False
#     return True
# 
# def mp_birrt(robot, q1, q2):
#     from motion_planners.rrt_connect import birrt
#     return birrt(q1, q2, get_distance_fn(robot),
#                  get_sample_fn(robot.GetEnv(), robot),
#                  get_extend_fn(robot), get_collision_fn(robot.GetEnv(), robot))
# 
# def mp_straight_line(robot, q1, q2):
#     from motion_planners.rrt_connect import direct_path
#     return direct_path(q1, q2, get_extend_fn(robot),
#                        get_collision_fn(robot.GetEnv(), robot))
# 
# Path: robotics/openrave/transforms.py
# def set_pose(body, pose):
#     set_trans(body, trans_from_pose(pose))
# 
# def object_trans_from_manip_trans(manip_trans, grasp):
#     return np.dot(manip_trans, grasp)
# 
# def trans_from_point(x, y, z):
#     return trans_from_quat_point(unit_quat(), np.array([x, y, z]))


# Path: robotics/openrave/tamp_fixed_base.py
from time import sleep
from robotics.openrave.utils import solve_inverse_kinematics,  set_manipulator_conf, Conf, Traj, manip_from_pose_grasp,  manipulator_motion_plan, linear_motion_plan,  has_mp, mp_birrt, mp_straight_line
from robotics.openrave.transforms import set_pose,  object_trans_from_manip_trans, trans_from_point
import numpy as np


    def cfree_traj(traj, pose):
        if DISABLE_MOTION_COLLISIONS:
            return True
        if traj.pose is not None and traj.pose == pose:

            return True
        return _cfree_traj_pose(traj, pose) and (traj.grasp is None or _cfree_traj_grasp_pose(traj, traj.grasp, pose))
    return cfree_traj


def sample_grasp_traj_fn(env, manipulator, body1, all_bodies):
    robot = manipulator.GetRobot()

    def sample_grasp_traj(pose, grasp):
        enable_all(all_bodies, False)
        body1.Enable(True)
        set_pose(body1, pose.value)
        manip_trans = manip_from_pose_grasp(pose.value, grasp.value)
        grasp_conf = solve_inverse_kinematics(manipulator, manip_trans)
        if grasp_conf is None:
            return
        if DISABLE_MOTIONS:
            yield [(Conf(grasp_conf), Traj([]))]
            return

        set_manipulator_conf(manipulator, grasp_conf)
        robot.Grab(body1)

        pregrasp_trans = manip_trans.dot(trans_from_point(*APPROACH_VECTOR))

        pregrasp_conf = solve_inverse_kinematics(manipulator, pregrasp_trans)