kinematics-dynamics
Public Member Functions | Static Public Member Functions | Private Attributes | List of all members
roboticslab::PoeExpression Class Reference

Abstraction of a product of exponentials (POE) formula. More...

#include <ProductOfExponentials.hpp>

Public Member Functions

 PoeExpression (const KDL::Frame &H_S_T=KDL::Frame::Identity())
 Constructor. More...
 
void append (const MatrixExponential &exp, const KDL::Frame &H_new_old=KDL::Frame::Identity())
 Appends a new term to this POE formula. More...
 
void append (const PoeExpression &poe, const KDL::Frame &H_new_old=KDL::Frame::Identity())
 Appends a POE to this formula. More...
 
const KDL::Frame & getTransform () const
 Retrieves the transformation between base and tool frames. More...
 
int size () const
 Size of this POE. More...
 
const MatrixExponentialexponentialAtJoint (int i) const
 Retrieves a term of the POE formula. More...
 
void changeBaseFrame (const KDL::Frame &H_new_old)
 Refers the internal coordinates of this POE to a different base. More...
 
void changeToolFrame (const KDL::Frame &H_new_old)
 Updates the tool frame. More...
 
bool evaluate (const KDL::JntArray &q, KDL::Frame &H) const
 Performs forward kinematics. More...
 
void reverseSelf ()
 Inverts this POE formula. More...
 
PoeExpression makeReverse () const
 Creates a new POE entity from the inverse of this POE formula. More...
 
KDL::Chain toChain () const
 Makes a KDL::Chain from this instance. More...
 

Static Public Member Functions

static PoeExpression fromChain (const KDL::Chain &chain)
 Builds a PoeExpression from a KDL::Chain. More...
 

Private Attributes

std::vector< MatrixExponentialexps
 
KDL::Frame H_S_T
 

Detailed Description

This entity is comprised of a sequence of matrix exponentials and a transformation between the base and the tool frame.

See also
MatrixExponential

Constructor & Destructor Documentation

◆ PoeExpression()

roboticslab::PoeExpression::PoeExpression ( const KDL::Frame &  H_S_T = KDL::Frame::Identity())
inlineexplicit
Parameters
H_S_TTransformation between the base and the tool frame.

Member Function Documentation

◆ append() [1/2]

void roboticslab::PoeExpression::append ( const MatrixExponential exp,
const KDL::Frame &  H_new_old = KDL::Frame::Identity() 
)
inline

All internal coordinates of the input POE term are referred to the current base frame after applying an (optional) intermediate transformation.

Parameters
expInput POE term.
H_new_oldTransformation between the base frame of this POE and the base frame of the input POE term.

◆ append() [2/2]

void PoeExpression::append ( const PoeExpression poe,
const KDL::Frame &  H_new_old = KDL::Frame::Identity() 
)

All internal coordinates of the input POE are referred to the current base frame after applying an (optional) intermediate transformation. The current tool frame is replaced by the appended POE's tool frame and updated in the same manner.

Parameters
poeInput POE formula.
H_new_oldTransformation between the new and the old base frame.

◆ changeBaseFrame()

void PoeExpression::changeBaseFrame ( const KDL::Frame &  H_new_old)

All POE terms as well as the base-to-tool reference frame will be updated accordingly.

Parameters
H_new_oldTransformation between the new and the old base frame.

◆ changeToolFrame()

void roboticslab::PoeExpression::changeToolFrame ( const KDL::Frame &  H_new_old)
inline
Parameters
H_new_oldTransformation between the new and the old tool frame.

◆ evaluate()

bool PoeExpression::evaluate ( const KDL::JntArray &  q,
KDL::Frame &  H 
) const
Parameters
qInput joint array (radians).
HOutput pose in cartesian space.
Returns
False if the size of the input joint array does not match the size of this POE.

◆ exponentialAtJoint()

const MatrixExponential& roboticslab::PoeExpression::exponentialAtJoint ( int  i) const
inline
Parameters
iZero-based index of the requested term.
Returns
An unmodifiable reference to said term.

◆ fromChain()

PoeExpression PoeExpression::fromChain ( const KDL::Chain &  chain)
static
Parameters
chainA KDL kinematic chain representation.
Returns
The resulting POE formula.
See also
toChain

◆ getTransform()

const KDL::Frame& roboticslab::PoeExpression::getTransform ( ) const
inline
Returns
Transformation between the base and the tool frame.

◆ makeReverse()

PoeExpression PoeExpression::makeReverse ( ) const

The sequence of POE terms is reversed and the base and tool frames interchanged. Note that theta_i = -theta_(size-i)_reversed.

Returns
A reversed copy of this instance.
See also
reverseSelf

◆ reverseSelf()

void PoeExpression::reverseSelf ( )

The sequence of POE terms is reversed and the base and tool frames interchanged. Note that theta_i = -theta_(size-i)_reversed.

See also
makeReverse

◆ size()

int roboticslab::PoeExpression::size ( ) const
inline
Returns
Number of terms in this POE formula.

◆ toChain()

KDL::Chain PoeExpression::toChain ( ) const
Returns
A KDL kinematic chain representation.
See also
fromChain

The documentation for this class was generated from the following files: