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)