#include <mc_solver/utils/ContactWrenchMatrixToLambdaMatrix.h>

Public Member Functions
	ContactWrenchMatrixToLambdaMatrix (const mc_solver::QPSolver &solver, const tasks::qp::ContactId &cid)

Eigen::MatrixXd	transform (const Eigen::MatrixXd &A) const

const Eigen::MatrixXd &	transform () const

Detailed Description

Transform contact wrench matrix to lambda matrix

This transforms a given matrix A that applies to a wrench (e.g. such as in \(A F <= F_{max}\)) into a matrix that applies to lambdas (i.e. such \(A' \boldsymbol{\lambda} <= F_{max}\))

For a given force \(f_p\) at point \(p\), the linearized friction cone provides:

\[f_p = \sum^{N^{Gen}_{p}}_{i=1} G^{p}_{i} \lambda^{p}_{i}\]

Where \(G^{p}_{i} \in \mathbb{R}^{3,1}\) are the generatrix of the friction cone. In the remainder of these equations we write \(N = N^{Gen}_{p}\) for sparcity

In matrix form:

\[ f_p = \begin{bmatrix} G^p_{1x} & \cdots & G^p_{Nx} \\ G^p_{1y} & \cdots & G^p_{Ny} \\ G^p_{1z} & \cdots & G^p_{Nz} \end{bmatrix} \begin{bmatrix} \lambda^{p}_{i} \\ \vdots \\ \lambda^{p}_{N} \end{bmatrix} \]

For a given wrench \(F\) at frame b, resulting from \(P\) contacts

\begin{align} F & = \sum^{P}_{i=1} {}^{p_{i}}X_{b}^{T} f_{p_{i}} \\ & = \begin{bmatrix} \begin{bmatrix} r^{\times}_{1}E^{T}_{1} \\ E^{T}_{1} \end{bmatrix} \begin{bmatrix} G^{1}_{1} & \cdots & G^{1}_{N} \end{bmatrix} & \cdots & \begin{bmatrix} r^{\times}_{P}E^{T}_{P} \\ E^{T}_{P} \end{bmatrix} \begin{bmatrix} G^{P}_{1} & \cdots & G^{P}_{N} \end{bmatrix} \end{bmatrix} \begin{bmatrix} \lambda^{1}_{1} \\ \vdots \\ \lambda^{P}_{N} \end{bmatrix} \end{align}

Where \({}^{p_{i}}X_{b}\) is the Plücker transform from the point \(p\) to the wrench frame \(b\):

\begin{align*} {}^{p_{i}}X_{b} = \begin{bmatrix} E_{i} & 0 \\ -E_{i}r_{i}^{\times} & E_{i} \end{bmatrix} & & {}^{p_{i}}X_{b}^{T} = \begin{bmatrix} E^{T}_{i} & r^{\times}_{i}E^{T}_{i} \\ 0 & E^{T}_{i} \end{bmatrix} \end{align*}

Note: we only take the last three columns on \({}^{p_{i}}X_{b}^{T}\) in Equation (2)

Hence:

\begin{align*} A' = A \begin{bmatrix} \begin{bmatrix} r^{\times}_{1}E^{T}_{1} \\ E^{T}_{1} \end{bmatrix} \begin{bmatrix} G^{1}_{1} & \cdots & G^{1}_{N} \end{bmatrix} & \cdots & \begin{bmatrix} r^{\times}_{P}E^{T}_{P} \\ E^{T}_{P} \end{bmatrix} \begin{bmatrix} G^{P}_{1} & \cdots & G^{P}_{N} \end{bmatrix} \end{bmatrix} \end{align*}

Constructor & Destructor Documentation

◆ ContactWrenchMatrixToLambdaMatrix()

mc_solver::utils::ContactWrenchMatrixToLambdaMatrix::ContactWrenchMatrixToLambdaMatrix	(	const mc_solver::QPSolver &	solver,
		const tasks::qp::ContactId &	cid
	)

Creates the transformation matrix for a given contact

Exceptions

If	the contact has not been added to the solver

Member Function Documentation

◆ transform() [1/2]

const Eigen::MatrixXd& mc_solver::utils::ContactWrenchMatrixToLambdaMatrix::transform ( ) const

inline

◆ transform() [2/2]

Eigen::MatrixXd mc_solver::utils::ContactWrenchMatrixToLambdaMatrix::transform ( const Eigen::MatrixXd & A ) const