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)