Numerical implementations
Main functions
modern_robotics.num
This module contains numerical implementations, using
numpy. These implementations are the default when using
import modern_robotics. They can be loaded specifically by using
import modern_robotics.num as mr.
- R_p_to_SE3(R, p)[source]
Constructs a homogeneous transformation matrix from a rotation matrix and displacement vector.
- Parameters
R (numpy.array) – 3 by 3 rotation matrix \(\RotationMatrix\in\SOthree\)
p (numpy.array) – 3 by 1 displacement vector
- Returns
4 by 4 homogeneous transformation matrix \(\HomogeneousTransformationMatrix \in \SEthree\)
- Return type
numpy.array
- SE3_to_vec(T)[source]
‘Differentiation’ of a transformation matrix by using the matrix log of its rotation matrix to determine the screw velocity vector (twist) that reaches this transformation in unit time.
- Parameters
T (numpy.array) – Homogeneous transformation matrix
- Returns
Velocity twist vector \(\Twist\) with length \(\theta\)
- Return type
numpy.array
See also
- SO3_to_vec(R)[source]
‘Differentiation’ of a rotation matrix by using the matrix log to determine the angular velocity that reaches that orientation in unit time.
- Parameters
R (numpy.array) – 3 by 3 rotation matrix \(\RotationMatrix \in \SOthree\).
- Returns
Angular velocity vector \(\omega\) with length \(\theta\).
- Return type
numpy.array
See also
- big_adjoint(T)[source]
Constructs the ‘big adjoint’ form of the transformation matrix T.
\[\begin{split}\HomogeneousTransformationMatrix = \begin{bmatrix} \RotationMatrix & p \\ 0 & 1 \end{bmatrix} \quad\Rightarrow\quad \Adjoint{\HomogeneousTransformationMatrix} = \begin{bmatrix} \RotationMatrix & 0 \\ \TildeSkew{p}\RotationMatrix & \RotationMatrix \end{bmatrix}\end{split}\]- Parameters
T (numpy.array) – 4 by 4 homogeneous transformation matrix \(\HomogeneousTransformationMatrix \in \SEthree\)
- Returns
6 by 6 ‘big adjoint’ form of the transformation matrix T
- Return type
numpy.array
- inv_SE3(T)[source]
Inverts the transformation matrix T.
\[\begin{split}\HomogeneousTransformationMatrix = \begin{bmatrix} \RotationMatrix & p \\ 0 & 1 \end{bmatrix}\in\SEthree \quad\Rightarrow\quad \HomogeneousTransformationMatrix^{-1} = \begin{bmatrix} \RotationMatrix\Transposed & -\RotationMatrix\Transposed p \\ 0 & 1 \end{bmatrix}\in\SEthree\end{split}\]- Parameters
T (numpy.array) – 4 by 4 homogeneous transformation matrix \(\HomogeneousTransformationMatrix \in \SEthree\)
- Returns
4 by 4 inverse of the homogeneous transformation matrix \(\HomogeneousTransformationMatrix^{-1} \in \SEthree\)
- Return type
numpy.array
- little_adjoint(V)[source]
Constructs the ‘little adjoint’ form of the velocity twist V.
\[\begin{split}\Twist = \begin{bmatrix} \omega \\ v \end{bmatrix} \quad\Rightarrow\quad \LittleAdjoint{\Twist} = \begin{bmatrix} \TildeSkew{\omega} & 0 \\ \TildeSkew{v} & \TildeSkew{\omega} \end{bmatrix}\end{split}\]- Parameters
V (numpy.array) – 6 by 1 velocity twist
- Returns
6 by 6 ‘little adjoint’ form of the twist V
- Return type
numpy.array
- manipulability(J)[source]
Calculates the direction and magnitude of the principal axes of the manipulability ellipsoid, and the ratio between largest and smallest eigenvalue of \(\Jacobian\Jacobian\Transposed\).
- Parameters
J (numpy.array) – Jacobian matrix
- Returns
Tuple with 2D array containing the principal axes of the manipulability ellipsoid, and the condition number.
- Return type
numpy.array
- so3_to_vec(v_tilde)[source]
Reconstruct a 3 vector from its skew-symmetric matrix form. No check is performed to see if v_tilde is really skew-symmetric.
- Parameters
v_tilde (numpy.array) – 3 by 3 skew-symmetric matrix form of v, \(\TildeSkew{v}\).
- Returns
3 vector.
- Return type
numpy.array
See also
- vec_to_SE3(S, theta)[source]
‘Integration’ of a velocity twist to determine the configuration (expressed as transformation matrix) that this velocity twist reaches in unit time.
- Parameters
S (numpy.array) – 6 vector velocity twist
theta (float) – Distance travelled along the velocity twist screw axis
- Returns
4 by 4 homogeneous transformation matrix \(\HomogeneousTransformationMatrix \in \SEthree\)
- Return type
numpy.array
See also
- vec_to_SO3(omega, theta=None)[source]
‘Integration’ of an angular velocity to determine the orientation (expressed as rotation matrix) that this angular velocity reaches in unit time.
Uses Rodrigues’ formula to solve the matrix exponential of the vector’s skew-symmetric matrix form.
\[\RotationMatrix = \exp(\TildeSkew{\UnitLength{\omega}}\theta) = I + \TildeSkew{\UnitLength{\omega}}\sin\theta + \TildeSkew{\UnitLength{\omega}}^2(1-\cos\theta) \in \SOthree\]- Parameters
omega (numpy.array) – 3 vector angular velocity. Can be unit length, in that case theta should also be provided.
theta (Optional[float]) – If omitted, the Euclidean norm of omega is assumed to be theta.
- Returns
3 by 3 rotation matrix \(\RotationMatrix \in \SOthree\).
- Return type
numpy.array
See also
gen submodule
modern_robotics.num.gen
This submodule contains convenient functions to generate transformation matrices.
- TRx(theta)[source]
Creates a transformation matrix with a single rotation around the \(x\)-axis.
- Parameters
theta (float) – Angle to rotate with around \(x\), in radians.
- Returns
Homogeneous transformation matrix with \(x\)-rotation and zero translation.
- Return type
numpy.array
Example
>>> import numpy as np >>> import modern_robotics.num as mr >>> mr.gen.TRx(np.pi/3) array([[ 1. , 0. , 0. , 0. ], [ 0. , 0.5 , -0.8660254, 0. ], [ 0. , 0.8660254, 0.5 , 0. ], [ 0. , 0. , 0. , 1. ]])
- TRy(theta)[source]
Creates a transformation matrix with a single rotation around the \(y\)-axis.
- Parameters
theta (float) – Angle to rotate with around \(y\), in radians.
- Returns
Homogeneous transformation matrix with \(y\)-rotation and zero translation.
- Return type
numpy.array
Example
>>> import numpy as np >>> import modern_robotics.num as mr >>> mr.gen.TRy(np.pi/3) array([[ 0.5 , 0. , 0.8660254, 0. ], [ 0. , 1. , 0. , 0. ], [-0.8660254, 0. , 0.5 , 0. ], [ 0. , 0. , 0. , 1. ]])
- TRz(theta)[source]
Creates a transformation matrix with a single rotation around the \(z\)-axis.
- Parameters
theta (float) – Angle to rotate with around \(z\), in radians.
- Returns
Homogeneous transformation matrix with \(z\)-rotation and zero translation.
- Return type
numpy.array
Example
>>> import numpy as np >>> import modern_robotics.num as mr >>> mr.gen.TRz(np.pi/3) array([[ 0.5 , -0.8660254, 0. , 0. ], [ 0.8660254, 0.5 , 0. , 0. ], [ 0. , 0. , 1. , 0. ], [ 0. , 0. , 0. , 1. ]])
- Tt(t=None, x=0.0, y=0.0, z=0.0)[source]
Creates a transformation matrix with only a translational part.
- Parameters
t (Optional[numpy.array]) – Translation three vector. Should be a column vector. Optional, if omitted use one of the following to create
t.x (float) – \(x\)-coordinate of the translation. Optional and only used if
tis not provided.y (float) – \(y\)-coordinate of the translation. Optional and only used if
tis not provided.z (float) – \(z\)-coordinate of the translation. Optional and only used if
tis not provided.
- Returns
Homogeneous transformation matrix without rotation, only translation.
- Return type
numpy.array
Examples
>>> import numpy as np >>> import modern_robotics.num as mr >>> mr.gen.Tt(np.array([[1], [2], [3]])) array([[1., 0., 0., 1.], [0., 1., 0., 2.], [0., 0., 1., 3.], [0., 0., 0., 1.]])
>>> import numpy as np >>> import modern_robotics.num as mr >>> mr.gen.Tt(y=2, z=6) array([[1., 0., 0., 0.], [0., 1., 0., 2.], [0., 0., 1., 6.], [0., 0., 0., 1.]])