Public Member Functions | Static Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
hpp::core::ExplicitNumericalConstraint Class Reference

Explicit numerical constraint. More...

#include <hpp/core/explicit-numerical-constraint.hh>

Inheritance diagram for hpp::core::ExplicitNumericalConstraint:
[legend]
Collaboration diagram for hpp::core::ExplicitNumericalConstraint:
[legend]

Public Member Functions

virtual EquationPtr_t copy () const
 Copy object and return shared pointer to copy. More...
 
virtual DifferentiableFunctionPtr_t explicitFunction () const
 
DifferentiableFunctionPtr_t outputFunction () const
 
DifferentiableFunctionPtr_t outputFunctionInverse () const
 
const segments_toutputConf () const
 Get output configuration variables. More...
 
const segments_toutputVelocity () const
 Get output degrees of freedom. More...
 
const segments_tinputConf () const
 Get input configuration variables. More...
 
const segments_tinputVelocity () const
 Get input degrees of freedom. More...
 
- Public Member Functions inherited from hpp::core::NumericalConstraint
virtual ~NumericalConstraint ()
 
void rightHandSideFromConfig (ConfigurationIn_t config)
 
DifferentiableFunctionfunction () const
 Return a reference to the inner function. More...
 
const DifferentiableFunctionPtr_tfunctionPtr () const
 Return a reference to the inner function. More...
 
vector_tvalue ()
 Return a reference to the value. More...
 
matrix_tjacobian ()
 Return a reference to the jacobian. More...
 
- Public Member Functions inherited from hpp::core::Equation
bool operator== (const Equation &other) const
 Operator equality. More...
 
void rightHandSide (vectorIn_t rhs)
 Set the right hand side of the equation. More...
 
vectorIn_t rightHandSide () const
 Return the right hand side of the equation. More...
 
size_type rhsSize () const
 Return the size of the right hand side constraint. More...
 
const ComparisonTypes_tcomparisonType () const
 Return the ComparisonType. More...
 
void comparisonType (const ComparisonTypes_t &comp)
 Set the comparison type. More...
 
bool constantRightHandSide () const
 
vectorOut_t nonConstRightHandSide ()
 Return the right hand side of the equation. More...
 
virtual ~Equation ()
 

Static Public Member Functions

static ExplicitNumericalConstraintPtr_t create (const DevicePtr_t &robot, const DifferentiableFunctionPtr_t &function, const segments_t &inputConf, const segments_t &inputVelocity, const segments_t &outputConf, const segments_t &outputVelocity, const ComparisonTypes_t &comp=ComparisonTypes_t())
 Create instance and return shared pointer. More...
 
static ExplicitNumericalConstraintPtr_t create (const DevicePtr_t &robot, const DifferentiableFunctionPtr_t &function, const DifferentiableFunctionPtr_t &g, const DifferentiableFunctionPtr_t &ginv, const segments_t &inputConf, const segments_t &inputVelocity, const segments_t &outputConf, const segments_t &outputVelocity, const ComparisonTypes_t &comp=ComparisonTypes_t())
 Create instance and return shared pointer. More...
 
static ExplicitNumericalConstraintPtr_t createCopy (const ExplicitNumericalConstraintPtr_t &other)
 Create a copy and return shared pointer. More...
 
- Static Public Member Functions inherited from hpp::core::NumericalConstraint
static NumericalConstraintPtr_t create (const DifferentiableFunctionPtr_t &function)
 Create a shared pointer to a new instance. More...
 
static NumericalConstraintPtr_t create (const DifferentiableFunctionPtr_t &function, ComparisonTypes_t comp)
 Create a shared pointer to a new instance. More...
 
static NumericalConstraintPtr_t create (const DifferentiableFunctionPtr_t &function, ComparisonTypes_t comp, vectorIn_t rhs)
 Create a shared pointer to a new instance. More...
 
static NumericalConstraintPtr_t createCopy (const NumericalConstraintPtr_t &other)
 Create a copy and return shared pointer. More...
 

Protected Member Functions

 ExplicitNumericalConstraint (const DevicePtr_t &robot, const DifferentiableFunctionPtr_t &function, const segments_t &inputConf, const segments_t &inputVelocity, const segments_t &outputConf, const segments_t &outputVelocity, const ComparisonTypes_t &comp)
 Constructor. More...
 
 ExplicitNumericalConstraint (const DevicePtr_t &robot, const DifferentiableFunctionPtr_t &function, const DifferentiableFunctionPtr_t &g, const DifferentiableFunctionPtr_t &ginv, const segments_t &inputConf, const segments_t &inputVelocity, const segments_t &outputConf, const segments_t &outputVelocity, const ComparisonTypes_t &comp)
 Constructor. More...
 
 ExplicitNumericalConstraint (const ExplicitNumericalConstraint &other)
 Constructor. More...
 
void init (const ExplicitNumericalConstraintWkPtr_t &weak)
 
- Protected Member Functions inherited from hpp::core::NumericalConstraint
 NumericalConstraint (const DifferentiableFunctionPtr_t &function, ComparisonTypes_t comp)
 Constructor. More...
 
 NumericalConstraint (const DifferentiableFunctionPtr_t &function, ComparisonTypes_t comp, vectorIn_t rhs)
 Constructor. More...
 
 NumericalConstraint (const NumericalConstraint &other)
 Copy constructor. More...
 
virtual bool isEqual (const Equation &other, bool swapAndTest) const
 Test equality with other instance. More...
 
void init (const NumericalConstraintWkPtr_t &weak)
 
- Protected Member Functions inherited from hpp::core::Equation
 Equation (const ComparisonTypes_t &comp, vectorIn_t rhs)
 
 Equation (const Equation &other)
 
void init (const EquationWkPtr_t &weak)
 

Protected Attributes

DifferentiableFunctionPtr_t inputToOutput_
 
DifferentiableFunctionPtr_t g_
 
DifferentiableFunctionPtr_t ginv_
 
segments_t inputConf_
 
segments_t inputVelocity_
 
segments_t outputConf_
 
segments_t outputVelocity_
 
ExplicitNumericalConstraintWkPtr_t weak_
 

Detailed Description

Explicit numerical constraint.

An explicit numerical constraint is a constraint such that some configuration variables called output are function of the others called input.

Let

Recall that degrees of freedom refer to velocity vectors.

Let us notice that \(n_{ic} + n_{oc}\) is less than the robot configuration size, and \(n_{iv} + n_{ov}\) is less than the velocity size. Some degrees of freedom may indeed be neither input nor output.

Then the differential function is of the form

\begin{equation*} \left(\begin{array}{c} q_{oc_{1}} \\ \vdots \\ q_{oc_{n_{oc}}} \end{array}\right) - f \left((q_{ic_{1}} \cdots q_{ic_{n_{ic}}})^T\right) \end{equation*}

It is straightforward that an equality constraint with this function can solved explicitely:

\begin{align*} \left(\begin{array}{c} q_{oc_{1}} \\ \vdots \\ q_{oc_{n_{oc}}} \end{array}\right) &- f \left((q_{ic_{1}} \cdots q_{ic_{n_{ic}}})^T\right) = rhs \\ & \mbox {if and only if}\\ \left(\begin{array}{c} q_{oc_{1}} \\ \vdots \\ q_{oc_{n_{oc}}} \end{array}\right) &= f \left((q_{ic_{1}} \cdots q_{ic_{n_{ic}}})^T\right) + rhs \\ \end{align*}

If function \(f\) takes values in a Lie group (SO(2), SO(3)), the above "+" between a Lie group element and a tangent vector has to be undestood as the integration of the constant velocity from the Lie group element:

\begin{equation*} \mathbf{q} + \mathbf{v} = \mathbf{q}.\exp (\mathbf{v}) \end{equation*}

where \(\mathbf{q}\) is a Lie group element and \(\mathbf{v}\) is a tangent vector.

Considered as a NumericalConstraint, the expression of the Jacobian of the DifferentiableFunction above depends on the output space of function \(f\). The rows corresponding to values in a vector space are expressed as follows.

for any index \(i\) between 0 and the size of velocity vectors, either

Constructor & Destructor Documentation

◆ ExplicitNumericalConstraint() [1/3]

hpp::core::ExplicitNumericalConstraint::ExplicitNumericalConstraint ( const DevicePtr_t robot,
const DifferentiableFunctionPtr_t function,
const segments_t inputConf,
const segments_t inputVelocity,
const segments_t outputConf,
const segments_t outputVelocity,
const ComparisonTypes_t comp 
)
protected

Constructor.

Parameters
robotRobot for which the constraint is defined.
functionrelation between input configuration variables and output configuration variables,
outputConfset of integer intervals defining indices \((oc_{1}, \cdots, oc_{n_{oc}})\),
outputVeclocityset of integer defining indices \((ov_{1}, \cdots, ov_{n_{ov}})\).
rhsright hand side.
Note
comparison type for this constraint is always equality

◆ ExplicitNumericalConstraint() [2/3]

hpp::core::ExplicitNumericalConstraint::ExplicitNumericalConstraint ( const DevicePtr_t robot,
const DifferentiableFunctionPtr_t function,
const DifferentiableFunctionPtr_t g,
const DifferentiableFunctionPtr_t ginv,
const segments_t inputConf,
const segments_t inputVelocity,
const segments_t outputConf,
const segments_t outputVelocity,
const ComparisonTypes_t comp 
)
protected

Constructor.

Parameters
robotRobot for which the constraint is defined.
functionrelation between input configuration variables and output configuration variables,
g,ginv
outputConfset of integer intervals defining indices \((oc_{1}, \cdots, oc_{n_{oc}})\),
outputVeclocityset of integer defining indices \((ov_{1}, \cdots, ov_{n_{ov}})\).
rhsright hand side.
Note
comparison type for this constraint is always equality

◆ ExplicitNumericalConstraint() [3/3]

hpp::core::ExplicitNumericalConstraint::ExplicitNumericalConstraint ( const ExplicitNumericalConstraint other)
protected

Constructor.

Parameters
robotRobot for which the constraint is defined.
functionrelation between input configuration variables and output configuration variables,
outputConfset of integer intervals defining indices \((oc_{1}, \cdots, oc_{n_{oc}})\),
outputVeclocityset of integer defining indices \((ov_{1}, \cdots, ov_{n_{ov}})\).
rhsright hand side.
Note
comparison type for this constraint is always equality

Member Function Documentation

◆ copy()

virtual EquationPtr_t hpp::core::ExplicitNumericalConstraint::copy ( ) const
virtual

Copy object and return shared pointer to copy.

Reimplemented from hpp::core::NumericalConstraint.

Reimplemented in hpp::core::LockedJoint.

◆ create() [1/2]

static ExplicitNumericalConstraintPtr_t hpp::core::ExplicitNumericalConstraint::create ( const DevicePtr_t robot,
const DifferentiableFunctionPtr_t function,
const segments_t inputConf,
const segments_t inputVelocity,
const segments_t outputConf,
const segments_t outputVelocity,
const ComparisonTypes_t comp = ComparisonTypes_t() 
)
static

Create instance and return shared pointer.

Parameters
robotRobot for which the constraint is defined.
functionrelation between input configuration variables and output configuration variables,
outputConfset of integer intervals defining indices \((oc_{1}, \cdots, oc_{n_{oc}})\),
outputVeclocityset of integer defining indices \((ov_{1}, \cdots, ov_{n_{ov}})\).
rhsright hand side.
Note
comparison type for this constraint is always equality

◆ create() [2/2]

static ExplicitNumericalConstraintPtr_t hpp::core::ExplicitNumericalConstraint::create ( const DevicePtr_t robot,
const DifferentiableFunctionPtr_t function,
const DifferentiableFunctionPtr_t g,
const DifferentiableFunctionPtr_t ginv,
const segments_t inputConf,
const segments_t inputVelocity,
const segments_t outputConf,
const segments_t outputVelocity,
const ComparisonTypes_t comp = ComparisonTypes_t() 
)
static

Create instance and return shared pointer.

Parameters
robotRobot for which the constraint is defined.
functionrelation between input configuration variables and output configuration variables,
g,ginv
outputConfset of integer intervals defining indices \((oc_{1}, \cdots, oc_{n_{oc}})\),
outputVeclocityset of integer defining indices \((ov_{1}, \cdots, ov_{n_{ov}})\).
rhsright hand side.
Note
comparison type for this constraint is always equality

◆ createCopy()

static ExplicitNumericalConstraintPtr_t hpp::core::ExplicitNumericalConstraint::createCopy ( const ExplicitNumericalConstraintPtr_t other)
static

Create a copy and return shared pointer.

◆ explicitFunction()

virtual DifferentiableFunctionPtr_t hpp::core::ExplicitNumericalConstraint::explicitFunction ( ) const
inlinevirtual

◆ init()

void hpp::core::ExplicitNumericalConstraint::init ( const ExplicitNumericalConstraintWkPtr_t &  weak)
inlineprotected

◆ inputConf()

const segments_t& hpp::core::ExplicitNumericalConstraint::inputConf ( ) const
inline

Get input configuration variables.

◆ inputVelocity()

const segments_t& hpp::core::ExplicitNumericalConstraint::inputVelocity ( ) const
inline

Get input degrees of freedom.

◆ outputConf()

const segments_t& hpp::core::ExplicitNumericalConstraint::outputConf ( ) const
inline

Get output configuration variables.

◆ outputFunction()

DifferentiableFunctionPtr_t hpp::core::ExplicitNumericalConstraint::outputFunction ( ) const
inline

◆ outputFunctionInverse()

DifferentiableFunctionPtr_t hpp::core::ExplicitNumericalConstraint::outputFunctionInverse ( ) const
inline

◆ outputVelocity()

const segments_t& hpp::core::ExplicitNumericalConstraint::outputVelocity ( ) const
inline

Get output degrees of freedom.

Member Data Documentation

◆ g_

DifferentiableFunctionPtr_t hpp::core::ExplicitNumericalConstraint::g_
protected

◆ ginv_

DifferentiableFunctionPtr_t hpp::core::ExplicitNumericalConstraint::ginv_
protected

◆ inputConf_

segments_t hpp::core::ExplicitNumericalConstraint::inputConf_
protected

◆ inputToOutput_

DifferentiableFunctionPtr_t hpp::core::ExplicitNumericalConstraint::inputToOutput_
protected

◆ inputVelocity_

segments_t hpp::core::ExplicitNumericalConstraint::inputVelocity_
protected

◆ outputConf_

segments_t hpp::core::ExplicitNumericalConstraint::outputConf_
protected

◆ outputVelocity_

segments_t hpp::core::ExplicitNumericalConstraint::outputVelocity_
protected

◆ weak_

ExplicitNumericalConstraintWkPtr_t hpp::core::ExplicitNumericalConstraint::weak_
protected