def movePose(self, movingPose: Pose3):
motion = movingPose.toCenter6D()
rots = np.array(motion[3:]) # sequence: rz, ry, rx
trans = np.array(motion[:3]) * -1 # sequence: tx, ty, tz
rs = self.rotate(rots[2], rots[1], rots[0])
ts = self.translate(trans[0], trans[1], trans[2])
return rs and ts
def set_pose(self, pose: Pose3):
x, y, z, roll, yaw, pitch = pose.toCenter6D()
self.set_pose_center6D(x, y, z, roll, yaw, pitch)