def compute_object_poses(self): for placement_id in range(len(self.samples_dict['placement_id'])): T = self.sampler.object.stable_placement_dict['T'][placement_id] object_pose = roshelper.pose_from_matrix(T, frame_id='yumi_body') object_poses_list = [] for i in range(len(self.samples_dict['sample_ids'][placement_id])): object_poses_list.append(object_pose) self.samples_dict['object_pose'].append(object_poses_list)
def tilt_gripper_poses(self, gripper_rotate, gripper_anchor, rotate_angle=0 * np.pi / 180, anchor_angle=0 * np.pi / 180): #1. convert to gripper frame gripper_rotate_gripper_frame = roshelper.convert_reference_frame( gripper_rotate, gripper_rotate, roshelper.unit_pose(), frame_id="gripper_rotate") gripper_anchor_gripper_frame = roshelper.convert_reference_frame( gripper_anchor, gripper_anchor, roshelper.unit_pose(), frame_id="gripper_anchor") #2. rotate rotate_angle = self.find_lever_angle_sign( gripper_rotate) * rotate_angle anchor_angle = self.find_lever_angle_sign( gripper_anchor) * anchor_angle pose_transform_rotate = roshelper.pose_from_matrix( tfm.euler_matrix(0, 0, rotate_angle, 'sxyz'), frame_id="proposals_base") pose_transform_rotation_anchor = roshelper.pose_from_matrix( tfm.euler_matrix(0, 0, anchor_angle, 'sxyz'), frame_id="proposals_base") gripper_rotate_tilded_gripper_frame = roshelper.transform_pose( gripper_rotate_gripper_frame, pose_transform_rotate) gripper_anchor_tilded_gripper_frame = roshelper.transform_pose( gripper_anchor_gripper_frame, pose_transform_rotation_anchor) #3. convert back to proposals base frame gripper_rotate_tilded_proposals_base = roshelper.convert_reference_frame( gripper_rotate_tilded_gripper_frame, roshelper.unit_pose(), gripper_rotate, frame_id="proposals_base") gripper_anchor_tilded_proposals_base = roshelper.convert_reference_frame( gripper_anchor_tilded_gripper_frame, roshelper.unit_pose(), gripper_anchor, frame_id="proposals_base") return gripper_rotate_tilded_proposals_base, gripper_anchor_tilded_proposals_base
def visualize_placement(self, id, proposals=None): T_object_world = self.samples_dict['T'][id][0][0][0] pose_object_world = roshelper.pose_from_matrix(T_object_world, type="list") translated_mesh = self.sampler.object.transform_object( pose_object_world) if proposals: proposals = proposals[id] self.sampler.object.plot_proposals(translated_mesh, proposals) else: self.sampler.object.plot_meshes([translated_mesh]) pass
def get3dpose_object(pose2d, object, stable_placement=0, frame_id="yumi_body"): mesh = object.stable_placement_dict['mesh'][stable_placement] volume, cog, inertia = mesh.get_mass_properties() # 1. get orientation pose of stable placement T_stable_placement = object.stable_placement_dict['T'][stable_placement] pose_stable_placement = roshelper.pose_from_matrix(T_stable_placement, frame_id=frame_id) #2. rotate by theta about the vertical axis T_pose_transform = tfm.euler_matrix(0, 0, pose2d[2], 'sxyz') pose_transform = roshelper.pose_from_matrix(T_pose_transform, frame_id=frame_id) pose_stable_placement_rotated = roshelper.transform_pose(pose_stable_placement, pose_transform) #3. convert to quaternion # quaternion = tfm.quaternion_from_euler(0, 0, pose2d[2]) msg = PoseStamped() msg.pose.position.x = pose2d[0] msg.pose.position.y = pose2d[1] msg.pose.position.z = cog[2] - mesh.min_[2] msg.pose.orientation = pose_stable_placement_rotated.pose.orientation msg.header.frame_id = frame_id return msg
def align_arm_poses(pose, grasp_dist): '''Set position of right arm with respect to left arm''' T_left_world = roshelper.matrix_from_pose(pose) pose_right_left = PoseStamped() pose_right_left.header.frame_id = pose.header.frame_id pose_right_left.pose.position.y = -grasp_dist quat = roshelper.rotate_quat_y(pose_right_left) pose_right_left.pose.orientation.x = quat[0] pose_right_left.pose.orientation.y = quat[1] pose_right_left.pose.orientation.z = quat[2] pose_right_left.pose.orientation.w = quat[3] T_right_left = roshelper.matrix_from_pose(pose_right_left) T_right_world = np.matmul(T_left_world, T_right_left) pose_right = roshelper.pose_from_matrix(T_right_world) return [pose, pose_right]
def grasp_from_proposal(point_list_new, normal_list_new, T, sampling_base_pose): '''Returns gripper pose in proposals base frame from sampled grasp points and normals''' #point_list: in free object frame (sampling base) #point_normal: in free object frame (sampling base) #T: transformation matrix from sampling base to object stable placement try: pose_list, grasp_width = gripper_from_proposal(point_list_new, normal_list_new) except ValueError as e: raise ValueError(e) pose_stamped_list = [] for pose in pose_list: # pose_stamped_world = self.manipulation_3d.planner.listener.transformPose("yumi_body", # pose) pose_stamped_world = roshelper.convert_reference_frame( pose, roshelper.unit_pose(), sampling_base_pose, "yumi_body") pose_stamped_list.append(pose_stamped_world) pose_stamped_list_sorted, is_flipped = sort_pose_list(pose_stamped_list) pose_stamped_list_aligned = align_arm_poses(pose_stamped_list_sorted[0], grasp_width) pose_new_list = [] T_pose_list = [] for gripper_pose_world in pose_stamped_list_aligned: #convert to local proposals frame # gripper_pose_body = self.manipulation_3d.planner.listener.transformPose("sampling_base", # gripper_pose_world) gripper_pose_body = roshelper.convert_reference_frame( gripper_pose_world, sampling_base_pose, roshelper.unit_pose(), "sampling_base") T_gripper_body = roshelper.matrix_from_pose(gripper_pose_body) #rotate pose by T (in sampling_base frame) #there is a sublelty here: #T convert from sampling base to proposals frame T_gripper_body_new = np.matmul(T, T_gripper_body) pose_new_list.append( roshelper.pose_from_matrix(T_gripper_body_new, frame_id="proposals_base")) T_pose_list.append(T_gripper_body) return pose_new_list, grasp_width, T_pose_list, is_flipped
def get2dpose_object(pose3d, object): #1. transform object to pose3d T_posed3d = roshelper.matrix_from_pose(pose3d) R = T_posed3d[0:3,0:3] #2. find placement with z=0 vector_rotated_list = [] for placement, vector in enumerate(object.stable_placement_dict['vector']): vector_rotated_list.append(np.matmul(R, vector)[2]) placement_id = np.argmin(np.array(vector_rotated_list)) #3. extract x and y of object x = pose3d.pose.position.x y = pose3d.pose.position.y #4. find rotation (about z axis) between stable_placement and posed3d orientation T_stable_placement = object.stable_placement_dict['T_rot'][placement_id] pose_rot_stable_placement = roshelper.pose_from_matrix(T_stable_placement) pose_transform = roshelper.get_transform(pose3d, pose_rot_stable_placement) T_pose_transform = roshelper.matrix_from_pose(pose_transform) euler = tfm.euler_from_matrix(T_pose_transform, 'rxyz') theta = euler[2] pose2d = np.array([x,y,theta]) return pose2d, placement_id
def initialize_stable_placements(self): """convert samples taken in nominal pose to all stable placements poses""" #samples are expressed in proposals_base base_pose = self.sampler.base_initial # angle_list = list(np.linspace(np.pi / 4, np.pi / 4 + 2* np.pi, 4)) angle_list = list(np.linspace(0, 2 * np.pi, 12)) for placement_id in range( len(self.sampler.object.stable_placement_dict['convex_face_3d'] )): print('generating grasps for placement id: ', placement_id) grasp_id = 0 #1. find transformation to stable placement from base (transformation includes rot + trans) T = self.sampler.object.stable_placement_dict['T'][placement_id] object_pose = roshelper.pose_from_matrix(T, frame_id="yumi_body") #2. rotate all grasp points, normals, grasp_poses grasp_id_list_new = [] grasp_point_list_new = [] grasp_normal_list_new = [] grasp_gripper_pose_list_new = [] grasp_width_list_new = [] grasp_T_list_new = [] grasp_T_pose_list_new = [] grasp_base_pose_list_new = [] grasp_object_pose_list_new = [] grasp_collisions_list = [] grasp_sample_id_list = [] #iterate over sample points for point_list, normal_list, sample_id in zip( self.dict_grasp['points'], self.dict_grasp['face_normals'], self.dict_grasp['sample_ids']): point_list_new = [] normal_list_new = [] gripper_pose_list_new = [] T_list_new = [] T_pose_list_new = [] for point, normal in zip(point_list, normal_list): point_new = helper.vector_transform(T, point) T_rot = deepcopy(T) T_rot[:, 3] = [0, 0, 0, 1] normal_new = helper.vector_transform(T_rot, normal) point_list_new.append(point_new) normal_list_new.append(normal_new) pose_list, grasp_width, T_gripper_body_list, is_flipped = grasp_from_proposal( point_list, normal_list, T, self.sampler.sampling_pose_initial) if is_flipped: tmp = normal_list_new[0] normal_list_new[0] = normal_list_new[1] normal_list_new[1] = tmp T_list_new.append([T, T]) T_pose_list_new.append(T_gripper_body_list) for angle in angle_list: #publish gripper reference frame # roshelper.handle_block_pose(pose_list[0], self.sampler.planner.br, 'proposals_base', 'gripper_left_blue') # roshelper.handle_block_pose(pose_list[1], self.sampler.planner.br, 'proposals_base', 'gripper_right_blue') #i. convert to gripper frame gripper_left_gripper_left = roshelper.convert_reference_frame( pose_list[0], pose_list[0], roshelper.unit_pose(), frame_id="gripper_left") gripper_right_gripper_left = roshelper.convert_reference_frame( pose_list[1], pose_list[0], roshelper.unit_pose(), frame_id="gripper_left") #ii. rotate grippers by angle about y axis of gripper left frame T_pose_rotation_gripper_left = tfm.euler_matrix( 0, angle, 0, 'rxyz') pose_rotation_gripper_left = roshelper.pose_from_matrix( T_pose_rotation_gripper_left, 'gripper_left') gripper_left_rotated_gripper_left = roshelper.transform_pose( gripper_left_gripper_left, pose_rotation_gripper_left) gripper_right_rotated_gripper_left = roshelper.transform_pose( gripper_right_gripper_left, pose_rotation_gripper_left) #iii. convert back to proposals_base gripper_left_rotated_proposals_base = roshelper.convert_reference_frame( gripper_left_rotated_gripper_left, roshelper.unit_pose(), pose_list[0], frame_id="proposals_base") gripper_right_rotated_proposals_base = roshelper.convert_reference_frame( gripper_right_rotated_gripper_left, roshelper.unit_pose(), pose_list[0], frame_id="proposals_base") #3. check collisions between grippers and table gripper_pose_list_new = [ gripper_left_rotated_proposals_base, gripper_right_rotated_proposals_base ] collision_list = self.check_collisions( gripper_pose_list_new) grasp_id_list_new.append(placement_id) grasp_point_list_new.append(np.array(point_list_new)) grasp_normal_list_new.append(np.array(normal_list_new)) grasp_gripper_pose_list_new.append( np.array(gripper_pose_list_new)) grasp_width_list_new.append(grasp_width) grasp_T_list_new.append(np.array(T_list_new)) grasp_T_pose_list_new.append(np.array(T_pose_list_new)) grasp_base_pose_list_new.append(base_pose) grasp_object_pose_list_new.append(object_pose) grasp_collisions_list.append(collision_list) grasp_sample_id_list.append(grasp_id) grasp_id += 1 self.samples_dict['placement_id'].append(grasp_id_list_new) self.samples_dict['points'].append(grasp_point_list_new) self.samples_dict['face_normals'].append(grasp_normal_list_new) self.samples_dict['gripper_poses'].append( grasp_gripper_pose_list_new) self.samples_dict['grasp_width'].append(grasp_width_list_new) self.samples_dict['T'].append(grasp_T_list_new) self.samples_dict['T_pose'].append(grasp_T_pose_list_new) self.samples_dict['base_pose'].append(grasp_base_pose_list_new) self.samples_dict['object_pose'].append(grasp_object_pose_list_new) self.samples_dict['collisions'].append(grasp_collisions_list) self.samples_dict['sample_ids'].append(grasp_sample_id_list)