hpp::constraints::RelativeOrientation Class Reference

Constraint on the relative orientation of two robot joints. More...

#include <hpp/constraints/relative-orientation.hh>

Inheritance diagram for hpp::constraints::RelativeOrientation:
Collaboration diagram for hpp::constraints::RelativeOrientation:

Public Member Functions

virtual ~RelativeOrientation () throw ()
 
void reference (const matrix3_t &reference)
 Set desired relative orientation as a rotation matrix. More...
 
const matrix3_treference () const
 Get desired relative orientation. More...
 
 RelativeOrientation (const std::string &name, const DevicePtr_t &, const JointPtr_t &joint1, const JointPtr_t &joint2, const matrix3_t &reference, std::vector< bool > mask=boost::assign::list_of(true)(true)(true).convert_to_container< std::vector< bool > >())
 Constructor. More...
 
- Public Member Functions inherited from hpp::constraints::DifferentiableFunction
virtual ~DifferentiableFunction ()
 
void operator() (vectorOut_t result, vectorIn_t argument) const
 Evaluate the function at a given parameter. More...
 
void jacobian (matrixOut_t jacobian, vectorIn_t argument) const
 Computes the jacobian. More...
 
size_type inputSize () const
 Get dimension of input vector. More...
 
size_type inputDerivativeSize () const
 Get dimension of input derivative vector. More...
 
size_type outputSize () const
 Get dimension of output vector. More...
 
const std::string & name () const
 Get function name. More...
 
virtual std::ostream & print (std::ostream &o) const
 Display object in a stream. More...
 

Static Public Member Functions

static EIGEN_MAKE_ALIGNED_OPERATOR_NEW RelativeOrientationPtr_t create (const std::string &name, const DevicePtr_t &robot, const JointPtr_t &joint1, const JointPtr_t &joint2, const matrix3_t &reference, std::vector< bool > mask=boost::assign::list_of(true)(true)(true).convert_to_container< std::vector< bool > >())
 Return a shared pointer to a new instance. More...
 
static RelativeOrientationPtr_t create (const DevicePtr_t &robot, const JointPtr_t &joint1, const JointPtr_t &joint2, const matrix3_t &reference, std::vector< bool > mask=boost::assign::list_of(true)(true)(true).convert_to_container< std::vector< bool > >())
 Return a shared pointer to a new instance. More...
 

Protected Member Functions

virtual void impl_compute (vectorOut_t result, ConfigurationIn_t argument) const throw ()
 Compute value of error. More...
 
virtual void impl_jacobian (matrixOut_t jacobian, ConfigurationIn_t arg) const throw ()
 
- Protected Member Functions inherited from hpp::constraints::DifferentiableFunction
 DifferentiableFunction (size_type inputSize, size_type inputDerivativeSize, size_type outputSize, std::string name=std::string())
 Concrete class constructor should call this constructor. More...
 
virtual void impl_compute (vectorOut_t result, vectorIn_t argument) const =0
 User implementation of function evaluation. More...
 
virtual void impl_jacobian (matrixOut_t jacobian, vectorIn_t arg) const =0
 

Detailed Description

Constraint on the relative orientation of two robot joints.

The value of the function is defined as follows:

\begin{eqnarray*} \mathbf{r} (\mathbf{q}) &=& \log \left((R_1(\mathbf{q})^T R_2(\mathbf{q}))^T R^*\right) = \log \left(R_2(\mathbf{q})^T R_1(\mathbf{q}) R^*\right)\\ \mathbf{\dot{r}} &=& J_{\log}\left(R_2(\mathbf{q})^T R_1(\mathbf{q}) R^*\right) R_2(\mathbf{q})^T \left(J_{joint\ 1}^{\omega}(\mathbf{q}) - J_{joint\ 2}^{\omega}(\mathbf{q})\right) \mathbf{\dot{q}}\ \ \end{eqnarray*}

where $R^*$ is the desired value of $R_1(\mathbf{q})^T R_2(\mathbf{q})$ and

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

Constructor & Destructor Documentation

◆ ~RelativeOrientation()

virtual hpp::constraints::RelativeOrientation::~RelativeOrientation ( )
throw (
)
inlinevirtual

◆ RelativeOrientation()

hpp::constraints::RelativeOrientation::RelativeOrientation ( const std::string &  name,
const DevicePtr_t ,
const JointPtr_t joint1,
const JointPtr_t joint2,
const matrix3_t reference,
std::vector< bool >  mask = boost::assign::list_of(true)(true)(true).convert_to_container< std::vector< bool > >() 
)

Constructor.

Parameters
namethe name of the constraints,
robotthe robot the constraints is applied to,
jointthe joint the orientation of which is constrained
referencereference orientation of the joint,
maskwhich component of the error vector to take into account.

Member Function Documentation

◆ create() [1/2]

static EIGEN_MAKE_ALIGNED_OPERATOR_NEW RelativeOrientationPtr_t hpp::constraints::RelativeOrientation::create ( const std::string &  name,
const DevicePtr_t robot,
const JointPtr_t joint1,
const JointPtr_t joint2,
const matrix3_t reference,
std::vector< bool >  mask = boost::assign::list_of(true)(true)(true).convert_to_container< std::vector< bool > >() 
)
static

Return a shared pointer to a new instance.

Parameters
namethe name of the constraints,
robotthe robot the constraints is applied to,
joint1the first joint the orientation of which is constrained
joint2the second joint the orientation of which is constrained
referencedesired relative orientation $R_1(\mathbf{q})^T R_2(\mathbf{q})$ between the joints,
maskwhich component of the error vector to take into account.

◆ create() [2/2]

static RelativeOrientationPtr_t hpp::constraints::RelativeOrientation::create ( const DevicePtr_t robot,
const JointPtr_t joint1,
const JointPtr_t joint2,
const matrix3_t reference,
std::vector< bool >  mask = boost::assign::list_of(true)(true)(true).convert_to_container< std::vector< bool > >() 
)
static

Return a shared pointer to a new instance.

Parameters
robotthe robot the constraints is applied to,
joint1the first joint the orientation of which is constrained
joint2the second joint the orientation of which is constrained
referencedesired relative orientation $R_1(\mathbf{q})^T R_2(\mathbf{q})$ between the joints,
maskwhich component of the error vector to take into account.

◆ impl_compute()

virtual void hpp::constraints::RelativeOrientation::impl_compute ( vectorOut_t  result,
ConfigurationIn_t  argument 
) const
throw (
)
protectedvirtual

Compute value of error.

Parameters
argumentconfiguration of the robot,
Return values
resulterror vector

◆ impl_jacobian()

virtual void hpp::constraints::RelativeOrientation::impl_jacobian ( matrixOut_t  jacobian,
ConfigurationIn_t  arg 
) const
throw (
)
protectedvirtual

◆ reference() [1/2]

void hpp::constraints::RelativeOrientation::reference ( const matrix3_t reference)
inline

Set desired relative orientation as a rotation matrix.

◆ reference() [2/2]

const matrix3_t& hpp::constraints::RelativeOrientation::reference ( ) const
inline

Get desired relative orientation.