def sample_grasp_traj(pose, grasp, base_conf):
enable_all(False)
set_base_values(robot, base_conf.value)
body1.Enable(True)
set_pose(body1, pose.value)
manip_vector = get_manip_vector(pose, grasp)
grasp_config = inverse_kinematics_helper(env, robot, manip_vector.manip_trans) # NOTE - maybe need to find all IK solutions
if grasp_config is None: return
set_manipulator_values(manipulator, grasp_config)
grasp_traj = workspace_traj_helper(base_manip, manip_vector.approach_vector)
if grasp_config is None: return
yield [(Config(grasp_traj.end()), grasp_traj)]
def sample_grasp_traj(pose, grasp):
enable_all(False)
body1.Enable(True)
set_pose(body1, pose.value)
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
grasp_config = inverse_kinematics_helper(env, robot, manip_trans)
if grasp_config is None: return
set_manipulator_values(manipulator, grasp_config)
robot.Grab(body1)
grasp_traj = workspace_traj_helper(base_manip, approach_vector)
robot.Release(body1)
if grasp_traj is None: return
grasp_traj.grasp = grasp
yield [(Config(grasp_traj.end()), grasp_traj)]
def feasible_pick_place(env, robot, obj, grasps, approach_config,
do_motion_planning):
base_manip = interfaces.BaseManipulation(robot,
plannername=None,
maxvelmult=None)
pose = Pose(get_pose(obj))
for grasp in grasps:
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
set_config(robot, approach_config,
robot.GetActiveManipulator().GetArmIndices())
if env.CheckCollision(robot) or robot.CheckSelfCollision():
continue
grasp_config = inverse_kinematics_helper(
env, robot,
manip_trans) # NOTE - maybe need to find all IK solutions
#grasp_config = solve_inverse_kinematics_exhaustive(env, robot, manip_trans)
if grasp_config is None:
continue
set_config(robot, grasp_config, get_active_arm_indices(robot))
#approach_config = get_full_config(robot)
traj, arm_traj = None, None
if do_motion_planning:
#traj = vector_traj_helper(env, robot, approach_vector)
traj = workspace_traj_helper(base_manip, approach_vector)
if traj is None:
continue
set_config(robot, traj.end(), get_active_arm_indices(robot))
#vector_config = oracle.get_robot_config()
arm_traj = motion_plan(env,
CSpace.robot_arm(get_manipulator(robot)),
approach_config,
self_collisions=True)
if arm_traj is None:
continue
return grasp_config, traj, arm_traj
return None
def feasible_pick_place(env, robot, obj, grasps, approach_config, do_motion_planning):
t0 = time()
base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None)
#base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None)
print time() - t0
#t0 = time()
#task_manip = interfaces.TaskManipulation(robot, plannername=None, maxvelmult=None, graspername=None)
#task_manip = interfaces.TaskManipulation(robot, plannername=None, maxvelmult=None, graspername=None)
#print time() - t0
#print
# Slight overhead but it appears you can just keep creating them
#0.000910043716431
#0.000546932220459
pose = Pose(get_pose(obj))
#env.Remove(obj) # NOTE - I'm just testing this now
for grasp in grasps:
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
set_config(robot, approach_config, robot.GetActiveManipulator().GetArmIndices())
if env.CheckCollision(robot) or robot.CheckSelfCollision():
print 'Collision failure'
#rospy.loginfo("Failed collision")
continue
#DrawAxes(env, manip_trans) # TODO - OpenRAVE bug
#print grasp.grasp_trans
#handles = draw_axes(env, get_trans(obj))
#handles = draw_axes(env, manip_trans)
#raw_input('Continue?')
#grasp_config = inverse_kinematics_helper(env, robot, manip_trans) # NOTE - maybe need to find all IK solutions
grasp_config = solve_inverse_kinematics_exhaustive(env, robot, manip_trans)
if grasp_config is None:
print 'IK failure'
#rospy.loginfo("Failed IK")
continue
set_config(robot, grasp_config, get_active_arm_indices(robot))
#approach_config = get_full_config(robot)
traj, arm_traj = None, None
if do_motion_planning:
#traj = vector_traj_helper(env, robot, approach_vector)
traj = workspace_traj_helper(base_manip, approach_vector)
if traj is None:
print 'Vec traj failure'
continue
set_config(robot, traj.end(), get_active_arm_indices(robot))
#vector_config = oracle.get_robot_config()
arm_traj = motion_plan(env, CSpace.robot_arm(get_manipulator(robot)), approach_config, self_collisions=True)
#arm_traj = manip_traj_helper(base_manip, approach_config, max_iterations=25, max_tries=2)
if arm_traj is None:
print 'Approach traj failure'
continue
# TODO - need to do reverse trajectory
return grasp_config, traj, arm_traj
return None