hpp-constraints  6.1.0
Definition of basic geometric constraints for motion planning
Classes | Typedefs | Functions
Tools
Constraints
Collaboration diagram for Tools:

Classes

struct  Eigen::BlockIndex
 
class  Eigen::MatrixBlockView< _ArgType, _Rows, _Cols, _allRows, _allCols >
 
class  Eigen::MatrixBlocks< _allRows, _allCols >
 
class  Eigen::MatrixBlocksBase< Derived >
 

Typedefs

typedef Eigen::MatrixBlocks< false, true > Eigen::RowBlockIndices
 
typedef Eigen::MatrixBlocks< true, false > Eigen::ColBlockIndices
 

Functions

template<typename Derived >
std::ostream & Eigen::operator<< (std::ostream &os, const MatrixBlocksBase< Derived > &mbi)
 
template<typename Derived >
void hpp::constraints::logSO3 (const matrix3_t &R, value_type &theta, Eigen::MatrixBase< Derived > const &result)
 
template<typename Derived >
void hpp::constraints::JlogSO3 (const value_type &theta, const Eigen::MatrixBase< Derived > &log, matrix3_t &Jlog)
 
template<typename Derived >
void hpp::constraints::logSE3 (const Transform3s &M, Eigen::MatrixBase< Derived > &result)
 
template<typename Derived >
void hpp::constraints::JlogSE3 (const Transform3s &M, Eigen::MatrixBase< Derived > const &Jlog)
 
template<typename Derived1 , typename Derived2 >
void hpp::constraints::matrixToQuat (const Eigen::MatrixBase< Derived1 > &M, Eigen::MatrixBase< Derived2 > const &q)
 
template<typename Derived >
void hpp::constraints::se3ToConfig (const Transform3s &M, Eigen::MatrixBase< Derived > const &q)
 

Detailed Description

Typedef Documentation

◆ ColBlockIndices

typedef Eigen::MatrixBlocks<true, false> Eigen::ColBlockIndices

◆ RowBlockIndices

typedef Eigen::MatrixBlocks<false, true> Eigen::RowBlockIndices

Function Documentation

◆ JlogSE3()

template<typename Derived >
void hpp::constraints::JlogSE3 ( const Transform3s &  M,
Eigen::MatrixBase< Derived > const &  Jlog 
)

◆ JlogSO3()

template<typename Derived >
void hpp::constraints::JlogSO3 ( const value_type &  theta,
const Eigen::MatrixBase< Derived > &  log,
matrix3_t &  Jlog 
)

Compute jacobian of function log of rotation matrix in SO(3)

Let us consider a matrix $R=\exp \left[\mathbf{r}\right]_{\times}\in SO(3)$. This functions computes the Jacobian of the function from $SO(3)$ into $\mathbf{R}^3$ that maps $R$ to $\mathbf{r}$. In other words,

\begin{equation*} \dot{\mathbf{r}} = J_{log}(R)\ \omega\,\,\,\mbox{with}\,\,\, \dot {R} = \left[\omega\right]_{\times} R \end{equation*}

Warning
Two representations of the angular velocity $\omega$ are possible:
  • $\dot{R} = \left[\omega\right]_{\times}R$ or
  • $\dot{R} = R\left[\omega\right]_{\times}$.
The expression below assumes the second representation is used.
Parameters
thetaangle of rotation $R$, also $\|r\|$,
log3d vector $\mathbf{r}$,
Return values
Jlogmatrix $J_{log} (R)$.

\begin{align*} J_{log} (R) &=& \frac{\|\mathbf{r}\|\sin\|\mathbf{r}\|}{2(1-\cos\|\mathbf{r}\|)} I_3 - \frac {1}{2}\left[\mathbf{r}\right]_{\times} + (\frac{1}{\|\mathbf{r}\|^2} - \frac{\sin\|\mathbf{r}\|}{2\|\mathbf{r}\|(1-\cos\|\mathbf{r}\|)}) \mathbf{r}\mathbf{r}^T\\ &=& I_3 +\frac{1}{2}\left[\mathbf{r}\right]_{\times} + \left(\frac{2(1-\cos\|\mathbf{r}\|) - \|\mathbf{r}\|\sin\|\mathbf{r}\|}{2\|\mathbf{r}\|^2(1-\cos\|\mathbf{r}\|)}\right)\left[\mathbf{r}\right]_{\times}^2 \end{align*}

Todo:
remove this and use pinocchio::Jlog3

◆ logSE3()

template<typename Derived >
void hpp::constraints::logSE3 ( const Transform3s &  M,
Eigen::MatrixBase< Derived > &  result 
)
inline

Compute log of rigid-body transform

Parameters
Mrigid body transform,
Return values
thetaangle of rotation,
result6d vector $(\mathbf{v},\mathbf{r})$ such that the screw motion of linear velocity (of the origin) $\mathbf{v}$ expressed in the moving frame and of angular velocity $\mathbf{r}$ expressed in the moving reaches $M$ in unit time.

◆ logSO3()

template<typename Derived >
void hpp::constraints::logSO3 ( const matrix3_t &  R,
value_type &  theta,
Eigen::MatrixBase< Derived > const &  result 
)
inline

Compute log of rotation matrix as a 3d vector

Parameters
Rrotation matrix in SO(3),
Return values
thetaangle of rotation,
result3d vector $\mathbf{r}$ such that $R=\exp [\mathbf{r}]_{\times}$

◆ matrixToQuat()

template<typename Derived1 , typename Derived2 >
void hpp::constraints::matrixToQuat ( const Eigen::MatrixBase< Derived1 > &  M,
Eigen::MatrixBase< Derived2 > const &  q 
)

◆ operator<<()

template<typename Derived >
std::ostream& Eigen::operator<< ( std::ostream &  os,
const MatrixBlocksBase< Derived > &  mbi 
)

◆ se3ToConfig()

template<typename Derived >
void hpp::constraints::se3ToConfig ( const Transform3s &  M,
Eigen::MatrixBase< Derived > const &  q 
)