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 setCollisionPose(self, collision_object, pose_world):
T_gripper_pose_world = roshelper.matrix_from_pose(pose_world)
R = T_gripper_pose_world[0:3, 0:3]
t = T_gripper_pose_world[0:3, 3]
collision_object.setRotation(R)
collision_object.setTranslation(t)
self.collision_object = collision_object
def find_lever_angle_sign(self, gripper_pose_proposals_base):
#1. extract vectors
T = roshelper.matrix_from_pose(gripper_pose_proposals_base)
x_vec, y_vec, z_vec = helper.matrix2vec(T)
#2. return sign based on condition
if x_vec[2] > 0:
return -1
else:
return 1
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 transform_object(self, pose):
T = roshelper.matrix_from_pose(pose)
euler = tf.transformations.euler_from_matrix(T.transpose(), 'rxyz')
#mesh library performs rotatin with DCM rather than R matrix
trans = tf.transformations.translation_from_matrix(T)
mesh_new = deepcopy(self.mesh)
mesh_new.rotate([.5, 0.0,0.0], euler[0])
mesh_new.rotate([0.0, .5 ,0.0], euler[1])
mesh_new.rotate([0.0, 0.0, .5], euler[2])
mesh_new.x += trans[0]
mesh_new.y += trans[1]
mesh_new.z += trans[2]
vertices = list(np.array(deepcopy(self.trimesh.vertices)))
vertices_transformed = []
for vertex in vertices:
point_transformed = helper.vector_transform(T, vertex)
vertices_transformed.append(point_transformed)
return mesh_new, vertices_transformed
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