Symbolical implementations
Main functions
modern_robotics.sym
This module contains symbolic implementations of most of the same functions as
modern_robotics.num, using
sympy. These implementations can be
loaded by using import modern_robotics.sym as mr.
- R_p_to_SE3(R, p)[source]
Constructs a homogeneous transformation matrix from a rotation matrix and displacement vector.
- Parameters
R (sympy.Matrix) – 3 by 3 rotation matrix \(\RotationMatrix\in\SOthree\)
p (sympy.Matrix) – 3 by 1 displacement vector
- Returns
4 by 4 homogeneous transformation matrix \(\HomogeneousTransformationMatrix \in \SEthree\)
- Return type
sympy.Matrix
- 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 (sympy.Matrix) – 3 by 3 rotation matrix \(\RotationMatrix \in \SOthree\).
- Returns
Angular velocity vector \(\omega\) with length \(\theta\).
- Return type
sympy.Matrix
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 (sympy.Matrix) – 4 by 4 homogeneous transformation matrix \(\HomogeneousTransformationMatrix \in \SEthree\)
- Returns
6 by 6 ‘big adjoint’ form of the transformation matrix T
- Return type
sympy.Matrix
- 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 (sympy.Matrix) – 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
sympy.Matrix
- 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 (sympy.Matrix) – 6 by 1 velocity twist
- Returns
6 by 6 ‘little adjoint’ form of the twist V
- Return type
sympy.Matrix
- 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 (sympy.Matrix) – 3 by 3 skew-symmetric matrix form of v, \(\TildeSkew{v}\).
- Returns
3 vector.
- Return type
sympy.Matrix
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 (sympy.Matrix) – 6 vector velocity twist
theta (sympy.Number) – Distance travelled along the velocity twist screw axis
- Returns
4 by 4 homogeneous transformation matrix \(\HomogeneousTransformationMatrix \in \SEthree\)
- Return type
sympy.Matrix
See also
SE3_to_vec()
- 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 (sympy.Matrix) – 3 vector angular velocity. Can be unit length, in that case theta should also be provided.
theta (Optional[sympy.Number]) – If omitted, the Euclidean norm of omega is assumed to be theta.
- Returns
3 by 3 rotation matrix \(\RotationMatrix \in \SOthree\).
- Return type
sympy.Matrix
See also
gen submodule
modern_robotics.sym.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
sympy.Matrix
Example
>>> import sympy as sp >>> import modern_robotics.sym as mr >>> mr.gen.TRx(sp.pi/3) Matrix([ [1, 0, 0, 0], [0, 1/2, -sqrt(3)/2, 0], [0, sqrt(3)/2, 1/2, 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
sympy.Matrix
Example
>>> import sympy as sp >>> import modern_robotics.sym as mr >>> mr.gen.TRy(sp.pi/3) Matrix([ [ 1/2, 0, sqrt(3)/2, 0], [ 0, 1, 0, 0], [-sqrt(3)/2, 0, 1/2, 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
sympy.Matrix
Example
>>> import sympy as sp >>> import modern_robotics.sym as mr >>> mr.gen.TRz(sp.pi/3) Matrix([ [ 1/2, -sqrt(3)/2, 0, 0], [sqrt(3)/2, 1/2, 0, 0], [ 0, 0, 1, 0], [ 0, 0, 0, 1]])
- Tt(t=None, x=0, y=0, z=0)[source]
Creates a transformation matrix with only a translational part.
- Parameters
t (Optional[sympy.Matrix]) – 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
sympy.Matrix
Examples
>>> import sympy as sp >>> import modern_robotics.sym as mr >>> mr.gen.Tt(sp.Matrix([1, 2, 3])) Matrix([ [1, 0, 0, 1], [0, 1, 0, 2], [0, 0, 1, 3], [0, 0, 0, 1]])
>>> import modern_robotics.sym as mr >>> mr.gen.Tt(y=2, z=6) Matrix([ [1, 0, 0, 0], [0, 1, 0, 2], [0, 0, 1, 6], [0, 0, 0, 1]])