Screws

Overview

In the early 1800s, Chasles proved that any change in the configuration of a rigid body can be parameterized in terms of a helical motion along a fixed axis [2]. Poinsot is credited with the discovery that a system of forces acting on a rigid body can be resolved as a force acting along a line combined with a torque acting about the same line [6]. Chasles’ and Poinsot’s work was later developed by Sir Robert Ball into the theory of screws [1]. Screws represent angular and translational quantities in a single 6x1 vector quantity. Not only do screws provide a systematic framework for kinematics and dynamics of mechanisms, their compact representation is well-suited for compact computer storage and facilitates vectorization.

Linear and angular velocity can be combined into a single 6x1 quantity as:

(1)\[\begin{split}\underline{\mathbf{v}} = \begin{Bmatrix}\underline{v}\\ \underline{\omega}\end{Bmatrix}\end{split}\]

In 1873, Clifford coined the word motor to refer to this screw quantity (i.e. a combination of motion and vector) [4]. Later, Ball referred to this screw quantity as a twist [1]. The term twist is used more frequently in modern literature, and therefore is used in the present work.

Adjoint Matrix \([Ad]\)

The \(6 \times 6\) adjoint transformation matrix, \([Ad]\), for transforming twists from one coordinate system to another can be formed from \(SE(3)\) matrices. The derivation is briefly summarized below:

Consider a displacement about the \(z\) axis given by the Special Orthogonal \(SO(3)\) matrix:

(2)\[\begin{split}\underline{a}=R_{ab}\underline{b}= \begin{bmatrix} cos\theta_z & -sin\theta_z & 0\\ sin \theta_z & cos\theta_z & 0\\ 0 & 0 & 1 \end{bmatrix}\underline{b}\end{split}\]

Then

(3)\[\dot{a}=\dot{R}_{ab}=\left[ \dot{R_{ab}}R^T_{ab} \right] R_{ab}b\]
(4)\[\begin{split}\dot{R}^T= \begin{bmatrix} -\dot{\theta_z} sin \theta_z & -\dot{\theta_z} cos \theta_z & 0\\ \dot{\theta_z} cos \theta_z & -\dot{\theta_z} sin \theta_z & 0\\ 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} cos \theta_z & sin \theta_z & 0\\ -sin \theta_z & cos \theta_z & 0\\ 0 & 0 & 1 \end{bmatrix}= \begin{bmatrix} 0 & -\dot{\theta_z} & 0\\ \dot{\theta_z} & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix}=\tilde{\omega}_z\end{split}\]

Where \(\tilde{\omega}_z\) is a skew symmetric matrix and the spatial velocity is given by

(5)\[\begin{split}\underline{\omega}=\begin{Bmatrix} 0\\ 0\\ \dot{\theta_z} \end{Bmatrix}= \begin{Bmatrix} 0\\0\\ \omega_z \end{Bmatrix}\end{split}\]

The derivation can be extended to \(SE(3)\) matrices for rigid body transformations:

(6)\[\begin{split}\underline{a}=\Phi_{ab}\underline{b}= \begin{bmatrix} R_{ab} & \underline{r}_{ab}\\ 0 & 1 \end{bmatrix}\underline{b}\end{split}\]

and

(7)\[\begin{split}\dot{a}=\left[\dot{\Phi}_{ab}\Phi_{ab}^{-1}\right] q_{ab}\underline{b}= \begin{bmatrix} \dot{R}_{ab} & \dot{\underline{r}}_{ab}\\ 0 & 0 \end{bmatrix} \begin{bmatrix} R^T_{ab} & -R^T_{ab}\underline{r}_{ab}\\ 0 & 1 \end{bmatrix} \begin{bmatrix} R_{ab} & \underline{r}_{ab}\\ 0 & 1 \end{bmatrix}\underline{b}\end{split}\]

simplifying yields:

(8)\[\begin{split}\dot{\Phi}_{ab}\Phi_{ab}^{-1}= \begin{bmatrix} \dot{R}_{ab}R^T_{ab} & -\dot{R}_{ab}R^T_{ab} \underline{r}_{ab}+\dot{\underline{r}}_{ab}\\ 0 & 0 \end{bmatrix}= \begin{bmatrix} 0 & -\omega_z & \omega_y & v_x\\ \omega_z & 0 & -\omega_x & v_y\\ -\omega_y & \omega_x & 0 & v_z\\ 0 & 0 & 0 & 0 \end{bmatrix}= \begin{bmatrix} \tilde{\omega} & \underline{v}\\ 0 & 0 \end{bmatrix}\end{split}\]

Which yields a \(6\times 1\) velocity screw, also known as a twist, \(\underline{\mathbf{v}}\), where

(9)\[\begin{split}\underline{\mathbf{v}}=\begin{Bmatrix} v_x\\ v_y\\ v_z\\ \omega_x\\ \omega_y\\ \omega_z \end{Bmatrix}= \begin{Bmatrix} \underline{v}\\ \underline{\omega} \end{Bmatrix}\end{split}\]

In the language of modern differential geometry, velocity screws are known as the Lie algebra of the SE(3) group; their group is designated as \(se(3)\) [5]. The quantity in Eq. (5) can be expressed in other coordinate systems by a similarity transformation.

(10)\[\begin{split}\begin{bmatrix} \Omega' & \underline{v}' \\ 0 & 0 \end{bmatrix}= \begin{bmatrix} R & \underline{r} \\ 0 & 1 \end{bmatrix} \begin{bmatrix} \Omega & \underline{v} \\ 0 & 0 \end{bmatrix} \begin{bmatrix} R^T & -R^T\underline{r} \\ 0 & 1 \end{bmatrix}= \begin{bmatrix} R\Omega R^T & R\underline{v}-R\Omega R^T \underline{r} \\ 0 & 0 \end{bmatrix}\end{split}\]

In computer implementation of the spatial velocity transformation, it is not necessary to perform the entire sequence of \(4\times4\) matrix operations. Not only can one exploit the matrix sparsity, but it is also possible to take advantage of the following identity to reduce the number of operations required. As the quantity \(R\Omega R^T\) is skew symmetric, it is only necessary to compute the \(\omega'\) terms as

(11)\[\omega'_x= (R_{22}R_{33}- R_{23}R_{32})\omega_x + (R_{23}R_{31} - R_{21}R_{32})\omega_y + (R_{21}R_{32} - R_{22}R_{31})\omega_z\]
(12)\[\omega'_y= (R_{13}R_{32}- R_{12}R_{33})\omega_x + (R_{11}R_{33} - R_{13}R_{31})\omega_y + (R_{12}R_{31} - R_{11}R_{32})\omega_z\]
(13)\[\omega'_z=(R_{12}R_{23} - R_{13}R_{22})\omega_x + (R_{13}R_{21} - R_{11}R_{23})\omega_y + (R_{12}R_{21} - R_{11}R_{22})\omega_z\]

R is orthonormal and therefore the second row of R crossed with the third row yields the first row, and the following is true:

(14)\[R_{22}R_{33} - R_{23}R_{32} = R_{11}\]
(15)\[R_{23}R_{31} - R_{21}R_{32} = R_{12}\]
(16)\[R_{21}R_{32} - R_{22}R_{31} = R_{13}\]

and therefore:

(17)\[\omega'_x= R_{11}\omega_x + R_{12}\omega_y + R_{13}\omega_z\]
(18)\[\omega'_y= R_{21}\omega_x + R_{22}\omega_y + R_{23}\omega_z\]
(19)\[\omega'_z=R_{31}\omega_x + R_{32}\omega_y + R_{33}\omega_z\]
(20)\[\underline{\omega}'=R\underline{\omega}\]

and

(21)\[\begin{split}\begin{bmatrix} \Omega' & \underline{v}' \\ 0 & 0 \end{bmatrix}= \begin{bmatrix} \widetilde{[R\underline\omega]} & R\underline{v}-\widetilde{[R\underline\omega]} \underline{r}\\ 0 & 0 \end{bmatrix}= \begin{bmatrix} \widetilde{[R\underline\omega]} & R\underline{v}+\tilde{r} R\underline\omega\\ 0 & 0 \end{bmatrix}\end{split}\]

Using this information, a \(6\times6\) adjoint transformation matrix can be generated to transform twists as

(22)\[\begin{split}\begin{Bmatrix} \underline{v}'\\\underline{\omega}' \end{Bmatrix} =\begin{bmatrix} R & \tilde{r}R\\ 0 & R \end{bmatrix} \begin{Bmatrix} \underline{v}\\ \underline{\omega} \end{Bmatrix}=[Ad]\begin{Bmatrix} \underline{v}\\ \underline{\omega} \end{Bmatrix}\end{split}\]

The matrix \([Ad]\) forms a \(6\times6\) screw basis. A similar matrix operator for transforming spatial velocities was introduced by Yuan and Freudenstein in 1970 [11]. Some authors put \(\underline\omega\) above \(\underline v\) as Yuan did. The form of spatial velocity specified in Eq. (22) was chosen in this work as it is the original order specified in Clifford’s Algebra [3] and is commonly used in flexible multibody dynamics formulations [8].

In the language of differential geometry, \([Ad]\) is known as the adjoint representation and provides a linear mapping of elements of a Lie algebra.

The \(6\times6\) screw basis, \([Ad]\), provides a convenient and compact notation for transforming screws, and therefore in computer implementation of the transformation, it is tempting to carry out full \(6\times6\) matrix algebra to transform twists. However, it is strongly emphasized that the full \(6\times6\) matrix algebra should never be carried out in computer code, and additionally that the full \(6\times6 [Ad]\) adjoint transformation matrix need not be stored or created in computer memory. In the present work, rigid body displacements are stored as a 12x1 array with the entries of a SO(3) rotation matrix stored in column-major order in the first 9 entries of the array, followed by a translation vector in the last 3 entries of the array. When computing \(v'\) and \(\omega'\) in Eq. (22), it is only necessary to multiply the non-zero terms of the various quantities as:

(23)\[\begin{split}\begin{Bmatrix}\underline{v}'\\ \underline{\omega}'\end{Bmatrix} = \begin{Bmatrix}R\underline{v} +\tilde{r}R\underline{\omega}\\ R\underline{\omega}\end{Bmatrix}\end{split}\]

The number of floating point operations required to compute the Eq. (23) are shown below:

Operation Multiplies Adds Notes
\(R\underline{\omega}\) 9 6  
\(\tilde{r}R\underline{\omega}\) 6 3 \(R\underline{\omega}\) computed above
\(R\underline{v}\) 9 6  
\(\tilde{r}R{\underline{\omega}} + R\underline{v}\) 0 3  
total 24 18  

On the other hand, carrying out full, dense matrix algebra requires the following operations:

Operation Multiplies Adds Notes
\(\tilde{r}R\) 27 18 Counts refers to the full, dense version.
Full, dense Eq. (22) 36 30  
total 63 48  

Carrying out the minimal required operations to transform screw quantities requires 62% fewer operations, 66% less memory and many of the matrix-matrix and matrix-vector operations benefit from vectorization. The above example illustrates that although the notational convention of Lie algebra provides a convenient and compact representation of kinematic and dynamic quantities, care should be taken in the computer implementation of the operations described here. KSL consists of a custom, sparse, vectorized kinematics library consisting of over 200 functions for processing screw quantities and SE(3) displacements that uses the minimal set of operations as described above.

Derivative operator matrices: \([ad]\)

In Eq. (8), the derivative operator of the SE(3) group was found. Similarly, the derivative operator matrices for the \(6 \times 6\) transformation matrices, \([Ad]\) and \([Ad]^*\) can be found as follows:

(24)\[\begin{split}[ad] = \dot{[Ad]}[Ad]^{-1} = \begin{bmatrix} 0 & -\omega_z & \omega_{y} & 0 & -v_z & v_y\\ \omega_z & 0 & -\omega_x & v_z & 0 & -v_x\\ -\omega_y & \omega_x & 0 & -v_y & v_x & 0\\ 0 & 0 & 0 & 0 & -\omega_z & \omega_{y} \\ 0 & 0 & 0 & \omega_z & 0 & -\omega_x\\ 0 & 0 & 0 & -\omega_y & \omega_x & 0 \end{bmatrix}= \begin{bmatrix} \tilde{\omega} & \tilde{v}\\ 0 & \tilde{\omega} \end{bmatrix}\end{split}\]

Similarly, the left derivative operator for \([Ad]^*\) matrices is:

\[\begin{split}[ad]^* = \dot{[Ad]^*}[Ad]^{*-1} = \begin{bmatrix} 0 & -\omega_z & \omega_{y} & 0 & 0 & 0\\ \omega_z & 0 & -\omega_x & 0 & 0 & 0\\ -\omega_y & \omega_x & 0 & 0 & 0 & 0\\ 0 & -v_z & v_y & 0 & -\omega_z & \omega_{y} \\ v_z & 0 & -v_x & \omega_z & 0 & -\omega_x\\ -v_y & v_x & 0 & -\omega_y & \omega_x & 0 \end{bmatrix}= \begin{bmatrix} \tilde{\omega} & 0\\ \tilde{v} & \tilde{\omega} \end{bmatrix}\end{split}\]

Previously, it was discussed how the derivative operators form the Lie algebra of the SE(3) group. Similarly, the \(6\times6\) \([Ad]\) matrices themselves fulfill the necessary requirements of a Lie group [7]. The \([ad]\) and \([ad]^*\) matrices comprise their respective Lie algebras.

Influence coefficient matrices

An important tool in the analysis of mechanisms and multibody systems is a map of a particular joint’s velocity to the body velocity. From Eqs. (4) and (9), it is possible to find the velocity screw associated with a revolute joint. The joint is aligned with \(z\) axis of reference frame \(a\).

(25)\[\begin{split}\underline{\mathbf{v}}_a= \begin{Bmatrix} 0\\0\\ 0\\ 0\\ 0\\\dot{q} \end{Bmatrix}= \begin{Bmatrix} 0\\0\\ 0 \\0 \\0 \\1 \end{Bmatrix}\dot{q}=\underline{\mathbf{h}}_a\dot{q}\end{split}\]

Here \(\underline{h}_a\) is the influence coefficient matrix [9] associated with the joint. Note that the influence coefficient matrix taken at the joint’s origin is simply a unit twist, \(\omega_z\), about the \(z\) axis.

Usually it is desirable to express the influence coefficient matrix with respect to other coordinate systems, such as the inertial reference frame. Using the \([Ad]\) operator in Eq. (9) yields:

(26)\[\begin{split}\underline{\mathbf{v}}_0= \underline{\mathbf{h}}_0\dot{q}= [Ad]_{0a} \underline{\mathbf{h}}_a\dot{q}= \begin{bmatrix} R_{0a} &\tilde{r}_{0a}R_{0a}\\ 0 & R_{0a} \end{bmatrix} \begin{Bmatrix} 0\\0\\ 0 \\0 \\0 \\1 \end{Bmatrix}\dot{q}\end{split}\]

These influence coefficient matrices taken with respect to the inertial reference frame form elements of the Jacobian matrix of a mechanism. As a mechanism moves through varying configurations, \(\underline{h}_0\) changes, and may require updating many millions of times in the course of a simulation. Equation (26) reveals important insight that allows one to reduce the number of floating point operations (FLOPs) required to update the influence coefficient matrix of primitive joints.

(27)\[\begin{split}\underline{\mathbf{h}}_0= \begin{Bmatrix} r_yR_{33}-r_zR_{23}\\ r_zR_{13}-r_xR_{33}\\ r_xR_{23}-r_yR_{13}\\ R_{13}\\ R_{23}\\ R_{33}\\ \end{Bmatrix}= \begin{bmatrix} R_{11} & R_{12} & R_{13} & r_yR_{31}-r_zR_{21} & r_yR_{32}-r_zR_{22} & r_yR_{33}-r_zR_{23}\\ R_{21} & R_{22} & R_{11} & r_zR_{11}-r_xR_{31} & r_zR_{12}-r_xR_{32} & r_zR_{13}-r_xR_{33}\\ R_{31} & R_{32} & R_{33} & r_xR_{21}-r_yR_{11} & r_xR_{22}-r_yR_{12} & r_xR_{23}-r_yR_{13}\\ 0 & 0 & 0 & R_{11} & R_{12} & R_{13}\\ 0 & 0 & 0 & R_{21} & R_{22} & R_{23}\\ 0 & 0 & 0 & R_{31} & R_{32} & R_{33}\\ \end{bmatrix} \begin{Bmatrix} 0\\0\\ 0 \\0 \\0 \\1 \end{Bmatrix}\end{split}\]

Once the position and orientation of each joint in a mechanism has been computed, the influence coefficient matrix can be computed simply by forming the corresponding column of \([Ad]\), which requires only 9 additional FLOPs for revolute joints and no additional FLOPs for prismatic joints. Additionally, if SE(3) displacements and the Jacobian matrix are stored in column-major order, updating the influence coefficient matrices and elements Jacobian matrix can be vectorized with a stride of 1, which is optimal in terms of memory access.

Double precision

union ksl_screw_t
#include <screw.h>

double precision screw consisting of a linear and angular vector pair in configuration space

Public Members

double at[6]
struct ksl_screw_t::[anonymous] [anonymous]

anonymous union allows accessing screw quantities at specified index

ksl_vec3_t lin

free linear vector, e.g. linear velocity or acceleration

ksl_vec3_t ang

bound angular vector, e.g. angular velocity or angular acceleration

struct ksl_screw_t::[anonymous] [anonymous]

anonymous union allows accessing screw quantities by linear or angular vector components

double m0
double m1
double m2
double m3
double m4
double m5
struct ksl_screw_t::[anonymous] [anonymous]

anonymous union allows accessing screw quantities by field name

ksl_screw_t ksl_screw(const double m0, const double m1, const double m2, const double m3, const double m4, const double m5)

double precision screw constructor

ksl_screw_t *ksl_screw_alloc(int n)

allocate n double precision screw quantities on the heap, must be freed by the user.

double ksl_screw_norm(const ksl_screw_t *s)

Compute the screw norm of a double precision spatial screw.

Return
screw norm
Parameters
  • s: [in] input screw

void ksl_screw_normalize(ksl_screw_t *s)

TODO document this function.

void ksl_screw_normalized(const ksl_screw_t *si, ksl_screw_t *so)

TODO document this function.

double ksl_dot_cs(const ksl_coscrew_t *ci, const ksl_screw_t *si)

compute double precision dot product between a coscrew and screw

void ksl_screw_scale(ksl_screw_t *s, const double a)

TODO document this function.

void ksl_screw_copy(const ksl_screw_t *si, ksl_screw_t *so)

Copy double precision ksl_screw_t si to so.

\(S_i \rightarrow S_o\)

Parameters
  • si: [in] screw to copy
  • so: [out] si is copied to so

void ksl_screw_invert(ksl_screw_t *si)

Invert (i.e. negate) a ksl_screw_t in place.

\(\mathbf{\underline{s}}^{-1} \rightarrow \mathbf{\underline{s}}\)

\(-\mathbf{\underline{s}} \rightarrow \mathbf{\underline{s}}\)

Parameters
  • si: [in/out] screw to invert

void ksl_screw_inverted(const ksl_screw_t *si, ksl_screw_t *so)

Returns the inverse of a ksl_screw_t.

\(\mathbf{\underline{s}}_i^{-1} \rightarrow \mathbf{\underline{s}}_o\)

Parameters
  • si: [in] screw to invert
  • so: [out] inverse of si

void ksl_axpy_ss(const double, const ksl_screw_t *, ksl_screw_t *)

compute axpy (a * x + y y) operation for double precision screws

void ksl_xpy_ss(const ksl_screw_t *, ksl_screw_t *)

TODO document this function.

void ksl_nxpy_ss(const ksl_screw_t *, ksl_screw_t *)

TODO document this function.

void ksl_product_as(const double k, const ksl_screw_t *si, ksl_screw_t *so)

TODO document this function.

void ksl_add_ss(const ksl_screw_t *s1i, const ksl_screw_t *s2i, ksl_screw_t *so)

TODO document this function.

void ksl_subtract_ss(const ksl_screw_t *s1i, const ksl_screw_t *s2i, ksl_screw_t *so)

TODO document this function.

void ksl_add_sst(const ksl_screw_t *si1, const ksl_screw_t *si2, ksl_screw_t *so)

TODO document this function.

void ksl_hstxf(const ksl_SE3f_t *Di, ksl_screwf_t *ho)

TODO document this function.

void ksl_hstyf(const ksl_SE3f_t *Di, ksl_screwf_t *ho)

TODO document this function.

void ksl_hstz(const ksl_SE3_t *ri, ksl_screw_t *ho)

TODO document this function.

void ksl_hsrx(const ksl_SE3_t *Di, ksl_screw_t *ho)

TODO document this function.

void ksl_hsry(const ksl_SE3_t *Di, ksl_screw_t *ho)

TODO document this function.

void ksl_hsrz(const ksl_SE3_t *Di, ksl_screw_t *ho)

TODO document this function.

void ksl_hstxinv(const ksl_SE3_t *Di, ksl_screw_t *ho)

TODO document this function.

void ksl_hstyinv(const ksl_SE3_t *Di, ksl_screw_t *ho)

TODO document this function.

void ksl_hstzinv(const ksl_SE3_t *Di, ksl_screw_t *ho)

TODO document this function.

void ksl_hsrxinv(const ksl_SE3_t *Di, ksl_screw_t *ho)

TODO document this function.

void ksl_hsryinv(const ksl_SE3_t *Di, ksl_screw_t *ho)

TODO document this function.

void ksl_hsrzinv(const ksl_SE3_t *Di, ksl_screw_t *ho)

TODO document this function.

void ksl_cross_ss(const ksl_screw_t *s1i, const ksl_screw_t *s2i, ksl_screw_t *so)

double precision screw cross product

void ksl_cross_sst(const ksl_screw_t *s1i, const ksl_screw_t *s2i, ksl_screw_t *so)

TODO document this function.

void ksl_product_Adrs(const ksl_mat3x3_t *r_oi, const ksl_screw_t *s_i, ksl_screw_t *s_o)

Rotate a double precision screw.

Store product of direction cosine matrix r_oi and screw s_i in s_o.

\( [Ad]_{(R^{oi})} \mathbf{\underline{s}}_{io,i}^i \rightarrow \mathbf{\underline{s}}_{io,i}^o \)

\( \begin{bmatrix} R^{oi} & 0\\0 & R^{oi} \end{bmatrix} \begin{Bmatrix} \underline{v}_{io,i}^i \\ \underline{\omega}_{io}^i \end{Bmatrix} \rightarrow \begin{Bmatrix} \underline{v}_{io,i}^o \\ \underline{\omega}_{io}^o \end{Bmatrix}\)

\( R^{oi} \underline{v}_{io,i}^i \rightarrow \underline{v}_{io,i}^i, R^{oi} \underline{\omega}_{io}^i \rightarrow \underline{\omega}_{io}^o \)

Parameters
  • r_oi: [in] an SO3 rotation matrix
  • s_i: [in] a 6x1 screw
  • s_o: [out] the product of r_oi * s_i is returned in s_o

void ksl_product_Adrsinv(const ksl_mat3x3_t *r_oi, const ksl_screw_t *s_i, ksl_screw_t *s_o)

Rotate the inverse of a double precision screw.

Store product of direction cosine matrix r_oi and the inverse of screw s_i in s_o.

\( [Ad]_{(R^{oi})} (-\mathbf{\underline{s}}_{io,i}^i) \rightarrow \mathbf{\underline{s}}_{io,i}^o \)

\( -[Ad]_{(R^{oi})} \mathbf{\underline{s}}_{io,i}^i \rightarrow \mathbf{\underline{s}}_{io,i}^o \)

\( \begin{bmatrix} R^{oi} & 0\\0 & R^{oi} \end{bmatrix} \begin{Bmatrix} -\underline{v}_{io,i}^i \\ -\underline{\omega}_{io}^i \end{Bmatrix} \rightarrow \begin{Bmatrix} \underline{v}_{io,i}^o \\ \underline{\omega}_{io}^o \end{Bmatrix}\)

\( \begin{bmatrix} -R^{oi} & 0\\0 & -R^{oi} \end{bmatrix} \begin{Bmatrix} \underline{v}_{io,i}^i \\ \underline{\omega}_{io}^i \end{Bmatrix} \rightarrow \begin{Bmatrix} \underline{v}_{io,i}^o \\ \underline{\omega}_{io}^o \end{Bmatrix}\)

\( -R^{oi} \underline{v}_{io,i}^i \rightarrow \underline{v}_{io,i}^i, -R^{oi} \underline{\omega}_{io}^i \rightarrow \underline{\omega}_{io}^o \)

Parameters
  • r_oi: [in] an SO3 rotation matrix
  • s_i: [in] a 6x1 screw
  • s_o: [out] the product of r_oi * s_i is returned in s_o

void ksl_product_Adrinvs(const ksl_mat3x3_t *r_io, const ksl_screw_t *s_i, ksl_screw_t *s_o)

Perform inverse rotation of a double precision screw.

Store product of the inverse of direction cosine matrix r_io with screw s_i in s_o, where r_io is a member of SO(3).

\( [Ad]_{(R^{io})^{-1}} \mathbf{\underline{s}}_{io,i}^i \rightarrow \mathbf{\underline{s}}_{io,i}^o \)

\( [Ad]_{(R^{oi})} \mathbf{\underline{s}}_{io,i}^i \rightarrow \mathbf{\underline{s}}_{io,i}^o \)

\( \begin{bmatrix} (R^{io})^{-1} & 0\\0 & (R^{io})^{-1} \end{bmatrix} \begin{Bmatrix} \underline{v}_{io,i}^i \\ \underline{\omega}_{io}^i \end{Bmatrix} \rightarrow \begin{Bmatrix} \underline{v}_{io,i}^o \\ \underline{\omega}_{io}^o \end{Bmatrix}\)

\( \begin{bmatrix} R^{oi} & 0\\0 & R^{oi} \end{bmatrix} \begin{Bmatrix} \underline{v}_{io,i}^i \\ \underline{\omega}_{io}^i \end{Bmatrix} \rightarrow \begin{Bmatrix} \underline{v}_{io,i}^o \\ \underline{\omega}_{io}^o \end{Bmatrix}\)

\( (R^{io})^{-1} \underline{v}_{io,i}^i \rightarrow \underline{v}_{io,i}^i, (R^{io})^{-1} \underline{\omega}_{io}^i \rightarrow \underline{\omega}_{io}^o \)

\( R^{oi} \underline{v}_{io,i}^i \rightarrow \underline{v}_{io,i}^i, R^{oi} \underline{\omega}_{io}^i \rightarrow \underline{\omega}_{io}^o \)

Parameters
  • r_io: [in] an SO3 rotation matrix
  • s_i: [in] a 6x1 screw
  • s_o: [out] the product of r_oi * s_i is returned in s_o

void ksl_product_Adts(const ksl_vec3_t *t_oi, const ksl_screw_t *s_i, ksl_screw_t *s_o)

Translate a double precision screw.

Perform a spatial translation of screw s_i using the translation vector t_oi, storing results in s_o.

\( [Ad]_{(\underline{t}_{oi}^i)} \mathbf{\underline{s}}_{io,i}^i \rightarrow \mathbf{\underline{s}}_{io,o}^i \)

\( \begin{bmatrix} I & \underline{\tilde{r}}_{oi}^i \\0 & I \end{bmatrix} \begin{Bmatrix} \underline{v}_{io,i}^i \\ \underline{\omega}_{io}^i \end{Bmatrix} \rightarrow \begin{Bmatrix} \underline{v}_{io,o}^i \\ \underline{\omega}_{io}^i \end{Bmatrix}\)

\( \underline{v}_{io,i}^i + \underline{\tilde{r}}_{oi}^i \underline{\omega}_{io}^i \rightarrow \underline{v}_{io,o}^i, \underline{\omega}_{io}^i \rightarrow \underline{\omega}_{io}^i \)

void ksl_product_Adtinvs(const ksl_vec3_t *ti, const ksl_screw_t *si, ksl_screw_t *so)

TODO document this function.

void ksl_product_Adtsinv(const ksl_vec3_t *ti, const ksl_screw_t *si, ksl_screw_t *so)

TODO document this function.

void ksl_product_Ads(const ksl_SE3_t *Di, const ksl_screw_t *si, ksl_screw_t *so)

General spatial transformation a double precision screw.

Store product of direction cosine matrix ri with spatial vector si in so.

\( R_i * S_i[0:2] \rightarrow S_o[0:2]\)

\( R_i * S_i[3:5] \rightarrow S_o[3:5]\)

Parameters
  • ri: a SO3 rotation matrix
  • si: a 1x6 screw
  • so: the output of ri * si is returned in so

void ksl_product_Adsinv(const ksl_SE3_t *Di, const ksl_screw_t *si, ksl_screw_t *so)

TODO document this function.

void ksl_product_Adinvs(const ksl_SE3_t *Di, const ksl_screw_t *si, ksl_screw_t *so)

TODO document this function.

Single precision

union ksl_screwf_t
#include <screw.h>

single precision screw consisting of a linear and angular vector pair in configuration space

Public Members

float at[6]

allows accessing screw quantities at specified index

struct ksl_screwf_t::[anonymous] [anonymous]

anonymous union allows accessing screw quantities at specified index

ksl_vec3f_t lin

free linear vector, e.g. linear velocity or acceleration

struct ksl_screwf_t::[anonymous] [anonymous]

anonymous union allows accessing screw quantities by linear or angular vector components

float m0
float m1
float m2
float m3
float m4
float m5
struct ksl_screwf_t::[anonymous] [anonymous]

anonymous union allows accessing screw quantities by field name

ksl_screwf_t ksl_screwf(const float m0, const float m1, const float m2, const float m3, const float m4, const float m5)

single precision screw constructor

ksl_screwf_t *ksl_screwf_alloc(int n)

allocate n single precision screw quantities on the heap, must be freed by the user.

float ksl_screwf_norm(const ksl_screwf_t *s)

Compute the screw norm of a single precision spatial screw.

Return
screw norm
Parameters
  • s: [in] input screw

void ksl_screwf_normalize(ksl_screwf_t *s)

TODO document this function.

void ksl_screw_normalized(const ksl_screw_t *si, ksl_screw_t *so)

TODO document this function.

float ksl_dot_csf(const ksl_coscrewf_t *ci, const ksl_screwf_t *si)

compute single precision dot product between a coscrew and screw

void ksl_screwf_scale(ksl_screwf_t *s, const float a)

TODO document this function.

void ksl_screwf_copy(const ksl_screwf_t *si, ksl_screwf_t *so)

Copy single precision ksl_screwf_t si to so.

\(S_i \rightarrow S_o\)

Parameters
  • si: [in] screw to copy
  • so: [out] si is copied to so

void ksl_screwf_invert(ksl_screwf_t *si)

Invert (i.e. negate) a ksl_screwf_t in place.

\(\mathbf{\underline{s}}^{-1} \rightarrow \mathbf{\underline{s}}\)

\(-\mathbf{\underline{s}} \rightarrow \mathbf{\underline{s}}\)

Parameters
  • si: [in/out] screw to invert

void ksl_screwf_inverted(const ksl_screwf_t *si, ksl_screwf_t *so)

Returns the inverse of a ksl_screwf_t.

\(\mathbf{\underline{s}}_i^{-1} \rightarrow \mathbf{\underline{s}}_o\)

Parameters
  • si: [in] screw to invert
  • so: [out] inverse of si

void ksl_axpy_ssf(const float, const ksl_screwf_t *, ksl_screwf_t *)

compute axpy (a * x + y y) operation for single precision screws

void ksl_xpy_ssf(const ksl_screwf_t *, ksl_screwf_t *)

TODO document this function.

void ksl_nxpy_ssf(const ksl_screwf_t *, ksl_screwf_t *)

TODO document this function.

void ksl_product_asf(const float k, const ksl_screwf_t *si, ksl_screwf_t *so)

TODO document this function.

void ksl_add_ssf(const ksl_screwf_t *s1i, const ksl_screwf_t *s2i, ksl_screwf_t *so)

TODO document this function.

void ksl_subtract_ssf(const ksl_screwf_t *s1i, const ksl_screwf_t *s2i, ksl_screwf_t *so)

TODO document this function.

void ksl_add_sstf(const ksl_screwf_t *si1, const ksl_screwf_t *si2, ksl_screwf_t *so)

TODO document this function.

void ksl_hstxf(const ksl_SE3f_t *Di, ksl_screwf_t *ho)

TODO document this function.

void ksl_hstyf(const ksl_SE3f_t *Di, ksl_screwf_t *ho)

TODO document this function.

void ksl_hstzf(const ksl_SE3f_t *ri, ksl_screwf_t *ho)

TODO document this function.

void ksl_hsrxf(const ksl_SE3f_t *Di, ksl_screwf_t *ho)

TODO document this function.

void ksl_hsryf(const ksl_SE3f_t *Di, ksl_screwf_t *ho)

TODO document this function.

void ksl_hsrzf(const ksl_SE3f_t *Di, ksl_screwf_t *ho)

TODO document this function.

void ksl_hstxinvf(const ksl_SE3f_t *Di, ksl_screwf_t *ho)

TODO document this function.

void ksl_hstyinvf(const ksl_SE3f_t *Di, ksl_screwf_t *ho)

TODO document this function.

void ksl_hstzinvf(const ksl_SE3f_t *Di, ksl_screwf_t *ho)

TODO document this function.

void ksl_hsrxinvf(const ksl_SE3f_t *Di, ksl_screwf_t *ho)

TODO document this function.

void ksl_hsryinvf(const ksl_SE3f_t *Di, ksl_screwf_t *ho)

TODO document this function.

void ksl_hsrzinvf(const ksl_SE3f_t *Di, ksl_screwf_t *ho)

TODO document this function.

void ksl_cross_ssf(const ksl_screwf_t *s1i, const ksl_screwf_t *s2i, ksl_screwf_t *so)

single precision screw cross product

void ksl_cross_sstf(const ksl_screwf_t *s1i, const ksl_screwf_t *s2i, ksl_screwf_t *so)

TODO document this function.

void ksl_product_Adrsf(const ksl_mat3x3f_t *r_oi, const ksl_screwf_t *s_i, ksl_screwf_t *s_o)

Rotate a single precision screw.

Store product of direction cosine matrix r_oi and screw s_i in s_o.

\( [Ad]_{(R^{oi})} \mathbf{\underline{s}}_{io,i}^i \rightarrow \mathbf{\underline{s}}_{io,i}^o \)

\( \begin{bmatrix} R^{oi} & 0\\0 & R^{oi} \end{bmatrix} \begin{Bmatrix} \underline{v}_{io,i}^i \\ \underline{\omega}_{io}^i \end{Bmatrix} \rightarrow \begin{Bmatrix} \underline{v}_{io,i}^o \\ \underline{\omega}_{io}^o \end{Bmatrix}\)

\( R^{oi} \underline{v}_{io,i}^i \rightarrow \underline{v}_{io,i}^i, R^{oi} \underline{\omega}_{io}^i \rightarrow \underline{\omega}_{io}^o \)

Parameters
  • r_oi: [in] an SO3 rotation matrix
  • s_i: [in] a 6x1 screw
  • s_o: [out] the product of r_oi * s_i is returned in s_o

void ksl_product_Adrsinvf(const ksl_mat3x3f_t *r_oi, const ksl_screwf_t *s_i, ksl_screwf_t *s_o)

Rotate the inverse of a single precision screw.

Store product of direction cosine matrix r_oi and the inverse of screw s_i in s_o.

\( [Ad]_{(R^{oi})} (-\mathbf{\underline{s}}_{io,i}^i) \rightarrow \mathbf{\underline{s}}_{io,i}^o \)

\( -[Ad]_{(R^{oi})} \mathbf{\underline{s}}_{io,i}^i \rightarrow \mathbf{\underline{s}}_{io,i}^o \)

\( \begin{bmatrix} R^{oi} & 0\\0 & R^{oi} \end{bmatrix} \begin{Bmatrix} -\underline{v}_{io,i}^i \\ -\underline{\omega}_{io}^i \end{Bmatrix} \rightarrow \begin{Bmatrix} \underline{v}_{io,i}^o \\ \underline{\omega}_{io}^o \end{Bmatrix}\)

\( \begin{bmatrix} -R^{oi} & 0\\0 & -R^{oi} \end{bmatrix} \begin{Bmatrix} \underline{v}_{io,i}^i \\ \underline{\omega}_{io}^i \end{Bmatrix} \rightarrow \begin{Bmatrix} \underline{v}_{io,i}^o \\ \underline{\omega}_{io}^o \end{Bmatrix}\)

\( -R^{oi} \underline{v}_{io,i}^i \rightarrow \underline{v}_{io,i}^i, -R^{oi} \underline{\omega}_{io}^i \rightarrow \underline{\omega}_{io}^o \)

Parameters
  • r_oi: [in] an SO3 rotation matrix
  • s_i: [in] a 6x1 screw
  • s_o: [out] the product of r_oi * s_i is returned in s_o

void ksl_product_Adrinvsf(const ksl_mat3x3f_t *r_io, const ksl_screwf_t *s_i, ksl_screwf_t *s_o)

Perform inverse rotation of a single precision screw.

Store product of the inverse of direction cosine matrix r_io with screw s_i in s_o, where r_io is a member of SO(3).

\( [Ad]_{(R^{io})^{-1}} \mathbf{\underline{s}}_{io,i}^i \rightarrow \mathbf{\underline{s}}_{io,i}^o \)

\( [Ad]_{(R^{oi})} \mathbf{\underline{s}}_{io,i}^i \rightarrow \mathbf{\underline{s}}_{io,i}^o \)

\( \begin{bmatrix} (R^{io})^{-1} & 0\\0 & (R^{io})^{-1} \end{bmatrix} \begin{Bmatrix} \underline{v}_{io,i}^i \\ \underline{\omega}_{io}^i \end{Bmatrix} \rightarrow \begin{Bmatrix} \underline{v}_{io,i}^o \\ \underline{\omega}_{io}^o \end{Bmatrix}\)

\( \begin{bmatrix} R^{oi} & 0\\0 & R^{oi} \end{bmatrix} \begin{Bmatrix} \underline{v}_{io,i}^i \\ \underline{\omega}_{io}^i \end{Bmatrix} \rightarrow \begin{Bmatrix} \underline{v}_{io,i}^o \\ \underline{\omega}_{io}^o \end{Bmatrix}\)

\( (R^{io})^{-1} \underline{v}_{io,i}^i \rightarrow \underline{v}_{io,i}^i, (R^{io})^{-1} \underline{\omega}_{io}^i \rightarrow \underline{\omega}_{io}^o \)

\( R^{oi} \underline{v}_{io,i}^i \rightarrow \underline{v}_{io,i}^i, R^{oi} \underline{\omega}_{io}^i \rightarrow \underline{\omega}_{io}^o \)

Parameters
  • r_io: [in] an SO3 rotation matrix
  • s_i: [in] a 6x1 screw
  • s_o: [out] the product of r_oi * s_i is returned in s_o

void ksl_product_Adtsf(const ksl_vec3f_t *ti, const ksl_screwf_t *si, ksl_screwf_t *so)

Translate a single precision screw.

Perform a spatial translation of screw s_i using the translation vector t_oi, storing results in s_o.

\( [Ad]_{(\underline{t}_{oi}^i)} \mathbf{\underline{s}}_{io,i}^i \rightarrow \mathbf{\underline{s}}_{io,o}^i \)

\( \begin{bmatrix} I & \underline{\tilde{r}}_{oi}^i \\0 & I \end{bmatrix} \begin{Bmatrix} \underline{v}_{io,i}^i \\ \underline{\omega}_{io}^i \end{Bmatrix} \rightarrow \begin{Bmatrix} \underline{v}_{io,o}^i \\ \underline{\omega}_{io}^i \end{Bmatrix}\)

\( \underline{v}_{io,i}^i + \underline{\tilde{r}}_{oi}^i \underline{\omega}_{io}^i \rightarrow \underline{v}_{io,o}^i, \underline{\omega}_{io}^i \rightarrow \underline{\omega}_{io}^i \)

void ksl_product_Adtinvsf(const ksl_vec3f_t *ti, const ksl_screwf_t *si, ksl_screwf_t *so)

TODO document this function.

void ksl_product_Adtsinvf(const ksl_vec3f_t *ti, const ksl_screwf_t *si, ksl_screwf_t *so)

TODO document this function.

void ksl_product_Adsf(const ksl_SE3f_t *Di, const ksl_screwf_t *si, ksl_screwf_t *so)

General spatial transformation a single precision screw.

Store product of direction cosine matrix ri with spatial vector si in so.

\( R_i * S_i[0:2] \rightarrow S_o[0:2]\)

\( R_i * S_i[3:5] \rightarrow S_o[3:5]\)

Parameters
  • ri: a SO3 rotation matrix
  • si: a 1x6 screw
  • so: the output of ri * si is returned in so

void ksl_product_Adsinvf(const ksl_SE3f_t *Di, const ksl_screwf_t *si, ksl_screwf_t *so)

TODO document this function.

void ksl_product_Adinvsf(const ksl_SE3f_t *Di, const ksl_screwf_t *si, ksl_screwf_t *so)

TODO document this function.