def exec_traj_do_work(l_gripper_poses, l_gripper_angles, r_gripper_poses, r_gripper_angles, traj_ik_func, obj_cloud_xyz, obj_name, can_move_lower):
del Globals.handles[1:]
grab_obj_kinbody = setup_obj_rave(obj_cloud_xyz, obj_name) if obj_cloud_xyz is not None else None
body_traj = {}
for (lr, gripper_poses, gripper_angles) in zip("lr", [l_gripper_poses, r_gripper_poses], [l_gripper_angles, r_gripper_angles]):
if len(gripper_poses) == 0: continue
gripper_poses, gripper_angles = np.array(gripper_poses), np.array(gripper_angles)
gripper_angles_grabbing = process_gripper_angles_for_grabbing(lr, gripper_angles)
Globals.pr2.update_rave()
current_pos = Globals.pr2.robot.GetLink("%s_gripper_tool_frame"%lr).GetTransform()
if can_move_lower:
gripper_poses_remove, gripper_angles_remove = gripper_poses, gripper_angles_grabbing
else:
gripper_poses_remove, gripper_angles_remove = remove_lower_poses(current_pos, gripper_poses, gripper_angles_grabbing)
gripper_poses_prepend, gripper_angles_prepend = prepend_path_to_start(current_pos, gripper_poses_remove, gripper_angles_remove, 15)
final_gripper_poses, final_gripper_angles = gripper_poses_prepend, gripper_angles_prepend
#do ik
joint_positions = traj_ik_func(Globals.pr2, lr, final_gripper_poses)
if len(joint_positions) == 0:
return ExecTrajectoryResponse(success=False)
unwrapped_joint_positions = unwrap_angles(joint_positions)
final_joint_positions = unwrapped_joint_positions
body_traj["%s_arm"%lr] = final_joint_positions
body_traj["%s_gripper"%lr] = final_gripper_angles
yn = yes_or_no("continue?")
if yn:
lt.follow_trajectory_with_grabs(Globals.pr2, body_traj)
while not yes_or_no("continue to next stage?"):
lt.follow_trajectory_with_grabs(Globals.pr2, body_traj)
if grab_obj_kinbody is not None:
handle_grab_or_release_obj(grab_obj_kinbody, l_gripper_poses, l_gripper_angles, r_gripper_poses, r_gripper_angles)
raw_input("Press enter when done viewing trajectory")
Globals.pr2.join_all()
return ExecTrajectoryResponse(success=True)
else:
return ExecTrajectoryResponse(success=False)
def exec_traj(req, traj_ik_func=do_traj_ik_default):
assert isinstance(req, ExecTrajectoryRequest)
del Globals.handles[1:]
traj = req.traj
body_traj = {}
for (lr,gripper_poses, gripper_angles) in zip("lr",[traj.l_gripper_poses.poses,traj.r_gripper_poses.poses], [traj.l_gripper_angles,traj.r_gripper_angles]):
if len(gripper_poses) == 0: continue
gripper_angles = np.array(gripper_angles)
rospy.logwarn("warning! gripper angle hacks")
gripper_angles[gripper_angles < .04] = gripper_angles[gripper_angles < .04] - .02
gripper_angles = get_proper_gripper_angles(lr, gripper_angles)
plot_gripper_xyzs_from_poses(lr, gripper_poses)
#do ik
joint_positions = traj_ik_func(lr, gripper_poses)
if len(joint_positions) == 0:
return ExecTrajectoryResponse(success=False)
joint_positions = ku.smooth_positions(joint_positions, .15)
body_traj["%s_arm"%lr] = joint_positions
body_traj["%s_gripper"%lr] = gripper_angles
pose_array = gm.PoseArray()
pose_array.header.frame_id = "base_footprint"
pose_array.header.stamp = rospy.Time.now()
pose_array.poses = traj.l_gripper_poses.poses if lr == 'l' else traj.r_gripper_poses.poses
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (1,0,0,1)))
yn = yes_or_no("continue?")
if yn:
lt.go_to_start(Globals.pr2, body_traj)
lt.follow_trajectory_with_grabs(Globals.pr2, body_traj)
Globals.pr2.join_all()
return ExecTrajectoryResponse(success=True)
else:
return ExecTrajectoryResponse(success=False)
def execute(self, userdata):
#if not args.test: draw_table()
Globals.pr2.update_rave()
if yes_or_no('about to execute trajectory. save?'):
import pickle
fname = '/tmp/trajectory_' + str(np.random.randint(9999999999)) + '.pkl'
with open(fname, 'w') as f:
pickle.dump(userdata.trajectory, f)
print 'saved to', fname
success = lfd_traj.follow_trajectory_with_grabs(Globals.pr2, userdata.trajectory)
raw_input('done executing segment. press enter to continue')
if success:
if args.count_steps: Globals.stage += 1
return "success"
else: return "failure"
def execute(self, userdata):
#if not args.test: draw_table()
Globals.pr2.update_rave()
if yes_or_no('about to execute trajectory. save?'):
import pickle
fname = '/tmp/trajectory_' + str(
np.random.randint(9999999999)) + '.pkl'
with open(fname, 'w') as f:
pickle.dump(userdata.trajectory, f)
print 'saved to', fname
success = lfd_traj.follow_trajectory_with_grabs(
Globals.pr2, userdata.trajectory)
raw_input('done executing segment. press enter to continue')
if success:
if args.count_steps: Globals.stage += 1
return "success"
else:
return "failure"
import numpy as np
import rospy
if rospy.get_name() == "/unnamed": rospy.init_node("exec_demo")
pr2 = PR2.create()
F = h5py.File(osp.join(osp.dirname(lfd.__file__),"data/knot_segs.h5"))
demo = F["knot00.00"]
jpos = np.asarray(demo["joint_states"]["position"])
jname = list(demo["joint_states"]["name"])
r_arm_inds = [jname.index(name) for name in pr2.rarm.joint_names]
l_arm_inds = [jname.index(name) for name in pr2.larm.joint_names]
r_grip_inds= [jname.index(name) for name in pr2.rgrip.joint_names]
l_grip_inds= [jname.index(name) for name in pr2.lgrip.joint_names]
l_grab = jpos[:,l_grip_inds] < .07
r_grab = jpos[:,r_grip_inds] < .07
j_l_grip = jpos[:,l_grip_inds]
j_l_grip[l_grab] = 0
j_l_grip = np.fmin(j_l_grip, .08)
j_r_grip = jpos[:,r_grip_inds]
j_r_grip[r_grab] = 0
j_r_grip = np.fmin(j_r_grip, .08)
lt.follow_trajectory_with_grabs(pr2, {"l_gripper":j_l_grip,
"r_gripper":j_r_grip,
"l_arm":jpos[:,l_arm_inds],
"r_arm":jpos[:,r_arm_inds]},
l_grab, r_grab)
def exec_traj_do_work(l_gripper_poses,
l_gripper_angles,
r_gripper_poses,
r_gripper_angles,
traj_ik_func,
obj_cloud_xyz,
obj_name,
can_move_lower=True):
del Globals.handles[1:]
grab_obj_kinbody = setup_obj_rave(
obj_cloud_xyz, obj_name) if obj_cloud_xyz is not None else None
body_traj = {}
for (lr, gripper_poses,
gripper_angles) in zip("lr", [l_gripper_poses, r_gripper_poses],
[l_gripper_angles, r_gripper_angles]):
if len(gripper_poses) == 0: continue
gripper_poses, gripper_angles = np.array(gripper_poses), np.array(
gripper_angles)
gripper_angles_grabbing = process_gripper_angles_for_grabbing(
lr, gripper_angles)
Globals.pr2.update_rave()
current_pos = Globals.pr2.robot.GetLink("%s_gripper_tool_frame" %
lr).GetTransform()
if can_move_lower:
gripper_poses_remove, gripper_angles_remove = gripper_poses, gripper_angles_grabbing
else:
gripper_poses_remove, gripper_angles_remove = remove_lower_poses(
current_pos, gripper_poses, gripper_angles_grabbing)
gripper_poses_prepend, gripper_angles_prepend = prepend_path_to_start(
current_pos, gripper_poses_remove, gripper_angles_remove, 15)
final_gripper_poses, final_gripper_angles = gripper_poses_prepend, gripper_angles_prepend
#do ik
joint_positions = traj_ik_func(Globals.pr2, lr, final_gripper_poses)
if len(joint_positions) == 0:
return ExecTrajectoryResponse(success=False)
unwrapped_joint_positions = unwrap_angles(joint_positions)
final_joint_positions = unwrapped_joint_positions
body_traj["%s_arm" % lr] = final_joint_positions
body_traj["%s_gripper" % lr] = final_gripper_angles
yn = yes_or_no("continue?")
if yn:
lt.follow_trajectory_with_grabs(Globals.pr2, body_traj)
if grab_obj_kinbody is not None:
handle_grab_or_release_obj(grab_obj_kinbody, l_gripper_poses,
l_gripper_angles, r_gripper_poses,
r_gripper_angles)
raw_input("Press enter when done viewing trajectory")
Globals.pr2.join_all()
return ExecTrajectoryResponse(success=True)
else:
return ExecTrajectoryResponse(success=False)
