def execute_traj(traj):
#for j, conf in enumerate(traj.path()):
#for j, conf in enumerate([traj.end()]):
path = list(sample_manipulator_trajectory(manipulator, traj.traj()))
for j, conf in enumerate(path):
set_manipulator_conf(manipulator, conf)
raw_input('%s/%s) Step?'%(j, len(path)))
def visualize_solution(env, problem, initial_conf, robot, manipulator, bodies, plan):
def execute_traj(traj):
#for j, conf in enumerate(traj.path()):
#for j, conf in enumerate([traj.end()]):
path = list(sample_manipulator_trajectory(manipulator, traj.traj()))
for j, conf in enumerate(path):
set_manipulator_conf(manipulator, conf)
raw_input('%s/%s) Step?'%(j, len(path)))
# Resets the initial state
set_manipulator_conf(manipulator, initial_conf.value)
for obj, pose in problem.initial_poses.iteritems():
set_pose(bodies[obj], pose.value)
for i, (action, args) in enumerate(plan):
raw_input('\n%s/%s) Next?'%(i, len(plan)))
if action.name == 'move':
_, _, traj = args
execute_traj(traj)
elif action.name == 'move_holding':
_, _, traj, _, _ = args
execute_traj(traj)
elif action.name == 'pick':
obj, _, _, _, traj = args
execute_traj(traj.reverse())
robot.Grab(bodies[obj])
execute_traj(traj)
elif action.name == 'place':
obj, _, _, _, traj = args
execute_traj(traj.reverse())
robot.Release(bodies[obj])
execute_traj(traj)
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
def _cfree_traj_pose(traj, pose): # Collision free test between a robot executing traj and an object at pose
enable_all(all_bodies, False)
body2.Enable(True)
set_pose(body2, pose.value)
for conf in traj.path():
set_manipulator_conf(manipulator, conf)
if env.CheckCollision(robot, body2):
return False
return True
def _cfree_traj_pose(traj, pose):
enable_all(all_bodies, False)
body2.Enable(True)
set_pose(body2, pose.value)
for conf in traj.path():
set_manipulator_conf(manipulator, conf)
if env.CheckCollision(robot, body2):
return False
return True
def sample_free_motion(conf1, conf2): # Sample motion while not holding
if DISABLE_TRAJECTORIES:
yield [(object(),)] # [(True,)]
return
enable_all(all_bodies, False)
set_manipulator_conf(manipulator, conf1.value)
#traj = motion_plan(env, cspace, conf2.value, self_collisions=True)
traj = cspace_traj_helper(base_manip, cspace, conf2.value, max_iterations=10)
if not traj: return
traj.grasp = None
yield [(traj,)]
def _cfree_traj_grasp_pose(traj, grasp, pose): # Collision free test between an object held at grasp while executing traj and an object at pose
enable_all(all_bodies, False)
body1.Enable(True)
body2.Enable(True)
set_pose(body2, pose.value)
for conf in traj.path():
set_manipulator_conf(manipulator, conf)
manip_trans = manipulator.GetTransform()
set_pose(body1, object_trans_from_manip_trans(manip_trans, grasp.grasp_trans))
if env.CheckCollision(body1, body2):
return False
return True
def sample_free_motion(conf1, conf2):
if DISABLE_TRAJECTORIES:
yield [(object(), )]
return
enable_all(all_bodies, False)
set_manipulator_conf(manipulator, conf1.value)
traj = cspace_traj_helper(base_manip,
cspace,
conf2.value,
max_iterations=10)
if not traj:
return
traj.grasp = None
yield [(traj, )]
def sample_holding_motion(conf1, conf2, grasp): # Sample motion while holding
if DISABLE_TRAJECTORIES:
yield [(object(),)] # [(True,)]
return
enable_all(all_bodies, False)
body1.Enable(True)
set_manipulator_conf(manipulator, conf1.value)
manip_trans = manipulator.GetTransform()
set_pose(body1, object_trans_from_manip_trans(manip_trans, grasp.grasp_trans))
robot.Grab(body1)
#traj = motion_plan(env, cspace, conf2.value, self_collisions=True)
traj = cspace_traj_helper(base_manip, cspace, conf2.value, max_iterations=10)
robot.Release(body1)
if not traj: return
traj.grasp = grasp
yield [(traj,)]
def sample_grasp_traj(pose, grasp): # Sample pregrasp config and motion plan that performs a grasp
enable_all(all_bodies, False)
body1.Enable(True)
set_pose(body1, pose.value)
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
grasp_conf = solve_inverse_kinematics(env, manipulator, manip_trans) # Grasp configuration
if grasp_conf is None: return
if DISABLE_TRAJECTORIES:
yield [(Conf(grasp_conf), object())]
return
set_manipulator_conf(manipulator, grasp_conf)
robot.Grab(body1)
grasp_traj = vector_traj_helper(env, robot, approach_vector) # Trajectory from grasp configuration to pregrasp
#grasp_traj = workspace_traj_helper(base_manip, approach_vector)
robot.Release(body1)
if grasp_traj is None: return
grasp_traj.grasp = grasp
pregrasp_conf = Conf(grasp_traj.end()) # Pregrasp configuration
yield [(pregrasp_conf, grasp_traj)]
def sample_grasp_traj(pose, grasp):
enable_all(all_bodies, False)
body1.Enable(True)
set_pose(body1, pose.value)
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
grasp_conf = solve_inverse_kinematics(env, manipulator, manip_trans)
if grasp_conf is None:
return
if DISABLE_TRAJECTORIES:
yield [(Conf(grasp_conf), object())]
return
set_manipulator_conf(manipulator, grasp_conf)
robot.Grab(body1)
grasp_traj = vector_traj_helper(env, robot, approach_vector)
robot.Release(body1)
if grasp_traj is None:
return
grasp_traj.grasp = grasp
pregrasp_conf = Conf(grasp_traj.end())
yield [(pregrasp_conf, grasp_traj)]