def transform_demo(reg, demo, left=True, right=True, cloud_xyz=False, object_clouds=False, l_offset = None, r_offset = None): """ reg: NonrigidRegistration object demo: array with the following fields: r_gripper_xyzs, l_gripper_xyzs, r_gripper_quats, l_gripper_quats, cloud_xyz (todo: replace 'cloud_xyz' with 'object_points') """ warped_demo = group_to_dict(demo) if left: if l_offset is None: l_tool_xyzs = demo["l_gripper_xyzs"] else: l_tool_xyzs = np.array([xyz + np.dot(rot[:3,:3], l_offset) for (xyz, rot) in zip(demo["l_gripper_xyzs"], quats2mats(demo["l_gripper_quats"]))]) l_tool_xyzs_warped, rot_l_warped = reg.transform_frames(l_tool_xyzs, quats2mats(demo["l_gripper_quats"])) l_gripper_quats_warped = mats2quats(rot_l_warped) if l_offset is None: l_gripper_xyzs_warped = l_tool_xyzs_warped else: l_gripper_xyzs_warped = np.array([xyz - np.dot(rot[:3,:3], l_offset) for (xyz, rot) in zip(l_tool_xyzs_warped, rot_l_warped)]) warped_demo["l_gripper_xyzs"] = l_gripper_xyzs_warped warped_demo["l_gripper_quats"] = l_gripper_quats_warped if right: if r_offset is None: r_tool_xyzs = demo["r_gripper_xyzs"] else: r_tool_xyzs = np.array([xyz + np.dot(rot[:3,:3], r_offset) for (xyz, rot) in zip(demo["r_gripper_xyzs"], quats2mats(demo["r_gripper_quats"]))]) r_tool_xyzs_warped, rot_r_warped = reg.transform_frames(r_tool_xyzs, quats2mats(demo["r_gripper_quats"])) r_gripper_quats_warped = mats2quats(rot_r_warped) if r_offset is None: r_gripper_xyzs_warped = r_tool_xyzs_warped else: r_gripper_xyzs_warped = np.array([xyz - np.dot(rot[:3,:3], r_offset) for (xyz, rot) in zip(r_tool_xyzs_warped, rot_r_warped)]) warped_demo["r_gripper_xyzs"] = r_gripper_xyzs_warped warped_demo["r_gripper_quats"] = r_gripper_quats_warped if cloud_xyz: old_cloud_xyz_pts = np.asarray(demo["cloud_xyz"]).reshape(-1,3) new_cloud_xyz_pts = reg.transform_points(old_cloud_xyz_pts) warped_demo["cloud_xyz"] = new_cloud_xyz_pts.reshape(demo["cloud_xyz"].shape) if object_clouds: for key in sorted(demo["object_clouds"].keys()): old_cloud_xyz_pts = np.asarray(demo["object_clouds"][key]).reshape(-1,3) new_cloud_xyz_pts = reg.transform_points(old_cloud_xyz_pts) warped_demo["object_clouds"][key] = new_cloud_xyz_pts return warped_demo
def callback(request): global HANDLES HANDLES = [] xyzs, _ = pc2xyzrgb(request.point_cloud) new_mins = xyzs.reshape(-1, 3).min(axis=0) new_maxes = xyzs.reshape(-1, 3).max(axis=0) new_obj_xyz = (new_mins + new_maxes) / 2 new_obj_dim = new_maxes - new_mins f.fit(np.atleast_2d(obj_xyz), np.atleast_2d(new_obj_xyz), 0.001, 0.001) new_xyzs, new_mats = f.transform_frames(old_xyzs, conversions.quats2mats(old_quats)) new_quats = conversions.mats2quats(new_mats) show_object_and_trajectory(new_xyzs, new_obj_xyz, obj_quat, new_obj_dim, NEW_ID) show_object_and_trajectory(xyzquat[:, :3], obj_xyz, obj_quat, obj_dim, OLD_ID) Globals.pr2.update_rave() joint_positions, inds = trajectories.make_joint_traj( new_xyzs, new_quats, Globals.pr2.rarm.manip, "base_link", "r_wrist_roll_link", filter_options=18 ) response = PushResponse() if joint_positions is not None: Globals.pr2.rarm.goto_joint_positions(joint_positions[0]) # current_joints = Globals.pr2.rarm.get_joint_positions()[None,:] joint_positions, joint_velocities, times = retiming.make_traj_with_limits( joint_positions, Globals.pr2.rarm.vel_limits, Globals.pr2.rarm.acc_limits, smooth=True ) # joint_velocities = kinematics_utils.get_velocities(joint_positions, times, tol=.001) Globals.pr2.rarm.follow_timed_joint_trajectory(joint_positions, joint_velocities, times) response.success = True else: response.success = False return response
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