Source code for pykin.kinematics.transform

import numpy as np
from pykin.utils import transform_utils as tf

[docs]class Transform: """ This class calculates the rotation and translation of a 3D rigid body. Args: rot (sequence of float) : The rotation parameter. Give in quaternions or roll pitch yaw. pos (sequence of float) : The translation parameter. """ def __init__( self, rot=np.array([1.0, 0.0, 0.0, 0.0]), pos=np.zeros(3) ): # Set rotation, position self.rot = self._to_quaternion(rot) self.pos = self._to_pos(pos) def __str__(self): return "Transform(rot={0}, pos={1})".format(self.rot, self.pos) def __repr__(self): return 'pykin.kinematics.transform.{}()'.format(type(self).__name__) def __mul__(self, other): rot = tf.quaternion_multiply(self.rot, other.rot) pos = self._to_rotation_vec(self.rot, other.pos) + self.pos return Transform(rot, pos)
[docs] def inverse(self): """ Returns: Transform : inverse transform """ rot = tf.get_quaternion_inverse(self.rot) pos = -self._to_rotation_vec(rot, self.pos) return Transform(rot, pos)
@property def rot(self): """ Returns: np.array: rotation (quaternion) """ return self._rot @rot.setter def rot(self, rot): self._rot = self._to_quaternion(rot) @property def pos(self): """ Returns: np.array: position """ return self._pos @pos.setter def pos(self, pos): self._pos = self._to_pos(pos) @property def pose(self): """ Returns: np.array: pose """ return np.hstack((self.pos, self.rot)) @property def rotation_matrix(self): """ Returns: np.array: rotation matrix """ return tf.get_rotation_matrix(self.rot) @property def h_mat(self): """ Returns: np.array: homogeneous matrix """ mat = tf.get_h_mat_from_quaternion(self.rot) mat[:3, 3] = self.pos return mat @staticmethod def _to_rotation_vec(rot, vec): """ Convert with quaternion and position to rotation vector Args: rot (np.array): rotation (quaternion) vec (np.array): position Returns: np.array: rotation vector """ v4 = np.hstack([np.array([0.0]), vec]) inv_rot = tf.get_quaternion_inverse(rot) ans = tf.quaternion_multiply(tf.quaternion_multiply(rot, v4), inv_rot) return ans[1:] @staticmethod def _to_quaternion(rot): """ Convert to rotation (qauternion) Args: rot (sequence of float): rotation (quaternion) Returns: np.array: rotation (quaternion) """ if len(rot) == 3: rot = tf.get_quaternion_from_rpy(rot, convention='wxyz') elif len(rot) == 4: rot = np.array(rot) else: raise ValueError("Size of rot must be 3 or 4.") return rot @staticmethod def _to_pos(pos): """ Convert to pos vector Args: pos (sequence of float): position Returns: np.array: position """ if len(pos) == 3: pos = np.array(pos) else: raise ValueError("Size of pos must be 3.") return pos