def superqs(self):
sq_out = sb.vector_superquadric(np.size(self._superqs, 0))
for i, sq in enumerate(self._superqs):
sq_out[i] = sq
# Express the sq pose wrt the robot base, instead of the icub base
quat_sq = tr.axis_angle_to_quaternion(
(sq.axisangle[0][0], sq.axisangle[1][0], sq.axisangle[2][0],
sq.axisangle[3][0]))
pos_sq = [sq.center[0][0], sq.center[1][0], sq.center[2][0]]
robot_base_T_icub_base = np.linalg.inv(
self._icub_base_T_robot_base)
icub_base_T_icub_sq = np.eye(4)
icub_base_R_icub_sq = tr.quaternion_to_matrix(quat_sq)
icub_base_T_icub_sq[:3] = np.append(icub_base_R_icub_sq,
np.array([pos_sq]).T,
axis=1)
robot_base_T_icub_sq = np.matmul(robot_base_T_icub_base,
icub_base_T_icub_sq)
vec_aa_sq = tr.quaternion_to_axis_angle(
tr.matrix_to_quaternion(robot_base_T_icub_sq[:3, :3]))
sq_out[i].setSuperqOrientation(vec_aa_sq)
sq_out[i].setSuperqCenter(robot_base_T_icub_sq[:3, 3])
return sq_out
def rotation(self, rotation):
# Convert lists and tuples
if type(rotation) in (list, tuple):
rotation = np.array(rotation).astype(np.float32)
self._check_valid_rotation(rotation)
self._rotation = rotation * 1.
self._quaternion = transformations.matrix_to_quaternion(rotation)
def transform_grasp_to_world(self, grasp_pose, camera_viewpoint):
"""Refer the grasp pose to 6D, if the camera viewpoint is given
Parameters
---------
grasp_pose: geometry_msgs/PoseStamped
The candidate to transform
camera_viewpoint: geometry_msgs/PoseStamped
The camera viewpoint wrt world reference frame
Returns
-------
geometry_msgs/PoseStamped
The candidate in the world reference frame
"""
# Need to tranform the grasp pose from camera frame to world frame
# w_T_cam : camera pose in world ref frame
# cam_T_grasp : grasp pose in camera ref frame
# w_T_grasp = w_T_cam * cam_T_grasp
# Construct the 4x4 affine transf matrices from ROS poses
grasp_quat = grasp_pose.pose.orientation
grasp_pos = grasp_pose.pose.position
cam_T_grasp = np.eye(4)
cam_T_grasp[:3, :3] = quaternion_to_matrix(
[grasp_quat.x, grasp_quat.y, grasp_quat.z, grasp_quat.w])
cam_T_grasp[:3, 3] = np.array([grasp_pos.x, grasp_pos.y, grasp_pos.z])
cam_quat = camera_viewpoint.pose.orientation
cam_pos = camera_viewpoint.pose.position
w_T_cam = np.eye(4)
w_T_cam[:3, :3] = quaternion_to_matrix(
[cam_quat.x, cam_quat.y, cam_quat.z, cam_quat.w])
w_T_cam[:3, 3] = np.array([cam_pos.x, cam_pos.y, cam_pos.z])
# Obtain the w_T_grasp affine transformation
w_T_grasp = np.matmul(w_T_cam, cam_T_grasp)
if DEBUG:
print("[DEBUG] Grasp pose reference system transform")
print("w_T_cam\n ", w_T_cam)
print("cam_T_grasp\n ", cam_T_grasp)
print("w_T_grasp\n ", w_T_grasp)
# Create and return a StampedPose object
w_T_grasp_quat = matrix_to_quaternion(w_T_grasp[:3, :3])
result_pose = PoseStamped()
result_pose.pose.orientation.x = w_T_grasp_quat[0]
result_pose.pose.orientation.y = w_T_grasp_quat[1]
result_pose.pose.orientation.z = w_T_grasp_quat[2]
result_pose.pose.orientation.w = w_T_grasp_quat[3]
result_pose.pose.position.x = w_T_grasp[0, 3]
result_pose.pose.position.y = w_T_grasp[1, 3]
result_pose.pose.position.z = w_T_grasp[2, 3]
result_pose.header = camera_viewpoint.header
return result_pose
def __init__(self,
position=np.zeros(3),
rotation=np.eye(3),
width=0.0,
score=0.0,
ref_frame="camera"):
self._position = position
self._rotation = rotation
self._check_valid_position(self._position)
self._check_valid_rotation(self._rotation)
self.width = width
self.ref_frame = ref_frame
self.score = score
self._quaternion = transformations.matrix_to_quaternion(self._rotation)