def _update_mesh(self, quat):
q = Quaternion(quat[1], quat[2], quat[3], quat[4])
axis = q.get_axis(undefined=[1, 0, 0])
angle = q.angle * 180.0 / pi
def do_things(target):
target.resetTransform()
self._default_transform(target)
target.rotate(angle, axis[0], axis[1], axis[2])
do_things(self.mesh)
do_things(self.plane_axis)
def from_mat4(cls, m):
"""
Construct Transform from matrix m
:param m: 4x4 affine transformation matrix
:return: rotation (quaternion), translation, scale
"""
translation = m[:3, 3]
scale_rotation = m[:3, :3]
u, _ = scipy.linalg.polar(scale_rotation)
rotation_q = Quaternion(matrix=u)
r_axis = rotation_q.get_axis(undefined=[0, 1, 0])
# ensure that rotation axis is +y
if np.array(r_axis).dot(np.array([0, 1, 0])) < 0:
rotation_q = -rotation_q
scale = np.linalg.norm(scale_rotation, axis=0)
return cls(translation=translation, rotation=rotation_q, scale=scale)
def _get_orientation_from_quaternion(elements):
q = Quaternion(elements)
return q.get_axis()[2] * q.angle
def quat2aa(q): # from pyQuaternion
my_quaternion = Quaternion(q) # pyQuaternion
ax = my_quaternion.get_axis()
theta = my_quaternion.radians
return theta * ax
move_to_pose = rospy.ServiceProxy(
'panda_grasp_server/panda_move_pose', PandaMove)
move_success = move_to_pose(pose).success
if move_success:
poses_reached += 1
get_robot_state = rospy.ServiceProxy(
'panda_grasp_server/panda_get_state', PandaGetState)
robot_state = get_robot_state().robot_state
eef_pose = robot_state.eef_state.pose
print(eef_pose)
# we need the pose in axis-angle representation
eef_orient = Quaternion(eef_pose.orientation.w,
eef_pose.orientation.x,
eef_pose.orientation.y,
eef_pose.orientation.z)
eef_orient_rotvec = eef_orient.get_axis() * eef_orient.angle
# pose_euler = euler_from_quaternion([eef_pose.orientation.x, eef_pose.orientation.x, eef_pose.orientation.z, eef_pose.orientation.w], axes=)
cv_image = bridge.imgmsg_to_cv2(camera_image,
desired_encoding='passthrough')
cv2.imwrite(
'/home/icub/calib_data/image-{}.png'.format(poses_reached),
cv_image)
with open(
'/home/icub/calib_data/pose_fPe_{}.yaml'.format(
poses_reached), 'w') as f:
s = ("rows: 6\n"
"cols: 1\n"
"data:\n"
" - [{:.6f}]\n"
" - [{:.6f}]\n"
" - [{:.6f}]\n"
