l_arm_cur = pr2.larm.get_joint_positions() cmd_states["l_arm"] = mu.linspace2d(l_arm_cur, pr2.rarm.L_POSTURES["tucked"] - l_arm_cur, n_steps) r_grip_cur = pr2.rgrip.get_angle() cmd_states["r_gripper"] = np.linspace(r_grip_cur, .08 - r_grip_cur, n_steps) l_grip_cur = pr2.lgrip.get_angle() cmd_states["l_gripper"] = np.linspace(l_grip_cur, .08 - l_grip_cur, n_steps) base_cur = pr2.base.get_pose("/odom_combined") cmd_states["base"] = mu.linspace2d(base_cur, np.random.randn(3), n_steps) trajectories.follow_body_traj(pr2, cmd_states, times=cmd_times, r_arm=True, l_arm=True, r_gripper=True, l_gripper=True, base=True, wait=False) actual_times = [] actual_states = np.zeros(5000, trajectories.BodyState) t_start = time() size = 0 while time() - t_start < cmd_times[-1] + 1: #why doesn't is_moving() work??? pr2.update_rave() actual_times.append(time() - t_start) actual_states["r_arm"][size] = pr2.rarm.get_joint_positions() actual_states["l_arm"][size] = pr2.larm.get_joint_positions()
cmd_times = np.linspace(0,3,n_steps) cmd_states = np.zeros(n_steps, dtype=trajectories.BodyState) r_arm_cur = pr2.rarm.get_joint_positions() cmd_states["r_arm"] = mu.linspace2d(r_arm_cur, mirror_arm_joints(pr2.rarm.L_POSTURES["untucked"]) - r_arm_cur,n_steps) l_arm_cur = pr2.larm.get_joint_positions() cmd_states["l_arm"] = mu.linspace2d(l_arm_cur, pr2.rarm.L_POSTURES["tucked"] - l_arm_cur, n_steps) r_grip_cur = pr2.rgrip.get_angle() cmd_states["r_gripper"] = np.linspace(r_grip_cur, .08 - r_grip_cur, n_steps) l_grip_cur = pr2.lgrip.get_angle() cmd_states["l_gripper"] = np.linspace(l_grip_cur, .08 - l_grip_cur, n_steps) base_cur = pr2.base.get_pose("/odom_combined") cmd_states["base"] = mu.linspace2d(base_cur, np.random.randn(3), n_steps) trajectories.follow_body_traj(pr2, cmd_states, times=cmd_times, r_arm=True, l_arm=True, r_gripper=True, l_gripper=True, base=True, wait=False) actual_times = [] actual_states = np.zeros(5000, trajectories.BodyState) t_start = time() size=0 while time() - t_start < cmd_times[-1]+1: #why doesn't is_moving() work??? pr2.update_rave() actual_times.append(time() - t_start) actual_states["r_arm"][size] = pr2.rarm.get_joint_positions() actual_states["l_arm"][size] = pr2.larm.get_joint_positions() actual_states["r_gripper"][size] = pr2.rgrip.get_angle() actual_states["l_gripper"][size] = pr2.lgrip.get_angle() actual_states["base"][size] = pr2.base.get_pose("/odom_combined")
def callback(req): Globals.handles = [] traj = req.traj if traj.arms_used in "rl": lr = traj.arms_used manip_name = {"l": "leftarm", "r": "rightarm"}[lr] manip = Globals.pr2.robot.GetManipulator(manip_name) gripper0_xyzs, gripper0_quats = [], [] for pose in traj.gripper0_poses.poses: xyz, quat = conversions.pose_to_trans_rot(pose) gripper0_xyzs.append(xyz) gripper0_quats.append(quat) joint_positions, inds = trajectories.make_joint_traj( gripper0_xyzs, gripper0_quats, manip, "base_footprint", "%s_gripper_tool_frame" % lr, filter_options=1 ) feasible_frac = len(inds) / len(gripper0_xyzs) best_feasible_frac, best_lr, best_joint_positions = feasible_frac, lr, joint_positions if best_feasible_frac < 0.7: rospy.logerr("unacceptably low fraction of ik success: %.4f", best_feasible_frac) return ExecTrajectoryResponse(success=False) rospy.loginfo("using %s hand", {"l": "left", "r": "right"}[best_lr]) gripper_angles = np.array(traj.gripper0_angles) rospy.logwarn("warning! gripper angle hack") gripper_angles[gripper_angles < 0.04] = gripper_angles[gripper_angles < 0.04] - 0.02 body_traj = np.zeros(len(best_joint_positions), dtype=trajectories.BodyState) body_traj["%s_arm" % best_lr] = best_joint_positions body_traj["%s_gripper" % best_lr] = gripper_angles pose_array = gm.PoseArray() pose_array.header.frame_id = "base_footprint" pose_array.poses = traj.gripper0_poses.poses Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba=(1, 0, 0, 1))) trajectories.follow_body_traj( Globals.pr2, body_traj, times=None, r_arm=(best_lr == "r"), r_gripper=(best_lr == "r"), l_arm=(best_lr == "l"), l_gripper=(best_lr == "l"), ) elif traj.arms_used in "b": body_traj = np.zeros(len(traj.gripper0_poses.poses), dtype=trajectories.BodyState) for (lr, gripper_poses, gripper_angles) in zip( "lr", [traj.gripper0_poses.poses, traj.gripper1_poses.poses], [traj.gripper0_angles, traj.gripper1_angles] ): gripper_angles = np.array(gripper_angles) rospy.logwarn("warning! gripper angle hack") gripper_angles[gripper_angles < 0.04] = gripper_angles[gripper_angles < 0.04] - 0.02 manip_name = {"l": "leftarm", "r": "rightarm"}[lr] manip = Globals.pr2.robot.GetManipulator(manip_name) gripper_xyzs, gripper_quats = [], [] for pose in gripper_poses: xyz, quat = conversions.pose_to_trans_rot(pose) gripper_xyzs.append(xyz) gripper_quats.append(quat) joint_positions, inds = trajectories.make_joint_traj( gripper_xyzs, gripper_quats, manip, "base_footprint", "%s_gripper_tool_frame" % lr, filter_options=1 ) if len(inds) == 0: return ExecTrajectoryResponse(success=False) feasible_frac = len(inds) / len(gripper_xyzs) 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.gripper0_poses.poses Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba=(1, 0, 0, 1))) trajectories.follow_body_traj( Globals.pr2, body_traj, times=None, r_arm=True, r_gripper=True, l_arm=True, l_gripper=True ) else: raise NotImplementedError Globals.pr2.join_all() return ExecTrajectoryResponse(success=True)
def execute(self, userdata): Globals.pr2.update_rave() trajectories.follow_body_traj(Globals.pr2, userdata.trajectory, times=None, l_arm = userdata.left_used, r_arm = userdata.right_used, l_gripper = userdata.left_used, r_gripper = userdata.right_used) return "success"
def callback(request): Globals.pr2.rarm.goto_posture('side') Globals.pr2.larm.goto_posture('side') #Globals.rviz.remove_all_markers() #draw_table() new_cloud1, _ = pc2xyzrgb(request.object_clouds[0]) new_cloud2, _ = pc2xyzrgb(request.object_clouds[1]) new_cloud1, new_cloud2 = sorted([new_cloud1, new_cloud2], key = lambda x: np.squeeze(x).ptp(axis=0).prod()) new_cloud1 = new_cloud1.reshape(-1,3) new_cloud2 = new_cloud2.reshape(-1,3) new_xyz1 = (new_cloud1.max(axis=0) + new_cloud1.min(axis=0))/2 new_xyz2 = (new_cloud2.max(axis=0) + new_cloud2.min(axis=0))/2 f.fit(np.array([xyz1, xyz2]), np.array([new_xyz1, new_xyz2]), 1e6, 1e-3) new_gripper_xyzs, new_gripper_mats = f.transform_frames(old_gripper_xyzs, conversions.quats2mats(old_gripper_quats)) new_gripper_quats = conversions.mats2quats(new_gripper_mats) #print "warning: using old oreitnations" show_objects_and_trajectory(new_gripper_xyzs, np.array([new_xyz1, new_xyz2]), np.array([quat1, quat2]), obj_dims, (0,1,0,1)) show_objects_and_trajectory(old_gripper_xyzs, np.array([xyz1, xyz2]), np.array([quat1, quat2]), obj_dims, (0,0,1,1)) grid_handle = draw_grid(Globals.rviz, f.transform_points, old_gripper_xyzs.min(axis=0), old_gripper_xyzs.max(axis=0), "base_footprint") HANDLES.append(grid_handle) Globals.pr2.join_all() Globals.pr2.update_rave() best_traj_info, best_feasible_frac = None, 0 env = Globals.pr2.robot.GetEnv() Globals.pr2.update_rave() collision_env = create_obstacle_env(env) basemanip = openravepy.interfaces.BaseManipulation(collision_env.GetRobot("pr2"),plannername=None) rospy.sleep(.1) #collision_env.SetViewer("qtcoin") #raw_input("press enter to continue") for (lr, arm) in zip("lr",[Globals.pr2.larm,Globals.pr2.rarm]): name = arm.manip.GetName() manip = collision_env.GetRobot('pr2').GetManipulator(name) rospy.loginfo("trying to plan to initial position with %s",name) first_mat1 = np.eye(4) first_mat1[:3,:3] = new_gripper_mats[0] first_mat1[:3,3] = new_gripper_xyzs[0] first_mat = transform_relative_pose_for_ik(manip, first_mat1, "world", "%s_gripper_tool_frame"%lr) collision_env.GetRobot("pr2").SetActiveManipulator(name) trajobj = None try: trajobj = basemanip.MoveToHandPosition([first_mat],seedik=16,outputtrajobj=True,execute=0) rospy.loginfo("planning succeeded") except Exception: rospy.loginfo("planning failed") traceback.print_exc() print "initial ik result", manip.FindIKSolutions(first_mat,0) continue rospy.loginfo("trying ik") Globals.pr2.update_rave() joint_positions, inds = trajectories.make_joint_traj(new_gripper_xyzs, new_gripper_quats, manip, 'base_footprint', '%s_gripper_tool_frame'%lr, filter_options = 1+16) feasible_frac = len(inds)/len(new_gripper_xyzs) print inds if feasible_frac > best_feasible_frac: best_feasible_frac = feasible_frac best_traj_info = dict( feasible_frac = feasible_frac, lr = 'l' if name == 'leftarm' else 'r', manip = manip, arm = arm, initial_traj = trajobj, joint_positions = joint_positions) collision_env.Destroy() response = PourResponse() if best_feasible_frac > .75: if best_traj_info["initial_traj"] is not None: follow_rave_traj(best_traj_info["arm"], best_traj_info["initial_traj"]) else: print "no initial traj" #print "feasible inds", best_traj_info["inds"] body_traj = np.zeros(len(best_traj_info["joint_positions"]),dtype=trajectories.BodyState) lr = best_traj_info["lr"] body_traj["%s_arm"%lr] = best_traj_info["joint_positions"] body_traj["%s_gripper"%lr] = gripper_angles trajectories.follow_body_traj(Globals.pr2, body_traj, times, r_arm = (lr=='r'), r_gripper = (lr=='r'), l_arm = (lr=='l'), l_gripper= (lr=='l')) Globals.pr2.join_all() response.success = True else: rospy.logerr("could not execute trajectory because not enough points are reachable") response.success = False return response