pyhpp.constraints

BySubstitution

Inherits: HierarchicalIterative

Solve a non-linear system equations with explicit and implicit constraints

This solver is defined in paper https://hal.archives-ouvertes.fr/hal-01804774/file/paper.pdf. We give here only a brief description

The unknows (denoted by $\mathbf{q}$) of the system of equations is a Lie group. It is usually a robot configuration space or the Cartesian product of robot configuration spaces.

The solver stores a set of implicit numerical constraints: $g_1(\mathbf{q})=0,g_2(\mathbf{q})=0,\cdots$. These implicit constraints are added using method HierarchicalIterative::add.

The solver also stores explicit numerical constraints (constraints where some configuration variables depend on others) in an instance of class ExplicitConstraintSet. This instance is accessible via method BySubstitution::explicitConstraintSet.

When an explicit constraint is added using method ExplicitConstraintSet::add, this method checks that the explicit constraint is compatible with the previously added ones. If so, the constraint is stored in the explicit constraint set. Otherwise, it has to be added as an implicit constraint.

See Section III of the above mentioned paper for the description of the constraint resolution.

ComparisonType

Inherits: int

ComparisonTypes

DifferentiableFunction

Differentiable function from a Lie group, for instance the configuration space of a robot (hpp::pinocchio::Device) to a another Lie group.

Note that the input Lie group is only represented by the sizes of the elements and of the velocities: methods inputSize and inputDerivativeSize

The output space can be accessed by method outputSpace.

The value of the function for a given input can be accessed by method value . The Jacobian of the function for a given input can be accessed by method jacobian .

Explicit

Explicit numerical constraint

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

Let

$(i{c}_1,\cdots ,i{c}_{n_{ic}})$ be the list of indices corresponding to ordered input configuration variables,

$(o{c}_1,\cdots ,o{c}_{n_{oc}})$ be the list of indices corresponding to ordered output configuration variables,

$(i{v}_1,\cdots ,i{v}_{n_{iv}})$ be the list of indices corresponding to ordered input degrees of freedom,

$(o{v}_1,\cdots ,o{v}_{n_{ov}})$ be the list of indices corresponding to ordered output degrees of freedom.

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{array}{rlr}{\mathbf{q}}_{out}-g\left({\mathbf{q}}_{in}\right)& \text{with}& {\mathbf{q}}_{out}={\left(q_{o{c}_1}\cdots q_{o{c}_{n_{oc}}}\right)}^T,{\mathbf{q}}_{in}=(q_{i{c}_1}\cdots q_{i{c}_{n_{ic}}}{)}^T\end{array}$$

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

$$\begin{array}{rl}{\mathbf{q}}_{out}& -g\left({\mathbf{q}}_{in}\right)=rhs\\ & \text{if and only if}\\ {\mathbf{q}}_{out}& =g\left({\mathbf{q}}_{in}\right)+rhs\end{array}$$

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{array}{c}\mathbf{q}+\mathbf{v}=\mathbf{q}.\exp (\mathbf{v})\end{array}$$

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

Considered as an Implicit instance, 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

${\dot{q}}_i$ is an input degree of freedom: $\exists j$ integer, $1\le j\le n_{iv}$ such that $i=i{v}_j$,

${\dot{q}}_i$ is an output degree of freedom: $\exists j$ integer, $1\le j\le n_{ov}$ such that $i=o{v}_j$, or

${\dot{q}}_i$ neither input nor output. In this case, the corresponding column is equal to 0.

$$\begin{array}{c}J=\left(\begin{array}{ccccccccccc}\cdots & o{v}_1& \cdots & i{v}_1& \cdots & o{v}_2& \cdots & i{v}_2& \cdots & o{v}_{n_{ov}}& \cdots \\ & 1& & & & 0& & & & & \\ & 0& & & & 1& & & & & \\ & & & -\frac{\partial g}{q_1}& & & & -\frac{\partial g}{q_2}& & & \\ & & & & & & & & & & \\ & 0& & & & 0& & & & 1& \end{array}\right)\end{array}$$

The rows corresponding to values in SO(3) have the following expression.

$$\begin{array}{c}J=\left(\begin{array}{cc}o{v}_{1}o{v}_{2}o{v}_3& i{v}_1\cdots i{v}_{n_{iv}}\\ J_{log}(R_g^T{R}_{out})& -J_{log}(R_g^T{R}_{out})R_{out}^T{R}_g\frac{\partial g}{\partial q_{in}}\end{array}\right)\end{array}$$

where

$R_{out}$ is the rotation matrix corresponding to unit quaternion $(q_{oc1},q_{oc2},q_{oc3},q_{oc4})$,

$R_g$ is the rotation matrix corresponding to the part of the output value of $f$ corresponding to SO(3),

$J_{log}$ is the Jacobian matrix of function that associates to a rotation matrix $R$ the vector $\omega$ such that

$$\begin{array}{c}R=\exp ({\left[\omega \right]}_{\times })\end{array}$$

“Domain ofdefinition” Some explicit constraints might be defined over only a subspace of the input

space. If the input value is not in the definition subspace, the explicit constraint will throw an exception of type FunctionNotDefinedForThisValue.

ExplicitConstraintSet

Set of explicit constraints

This class combines compatible explicit constraints as defined in the following paper published in Robotics Science and System 2018: https://hal.archives-ouvertes.fr/hal-01804774/file/paper.pdf, Section II-B Definition 4.

An explicit constraint $E=(in,out,f)$ on a robot configuration space $\mathcal{C}$ is defined by

a subset of input indices $in\subset \{1,\cdots ,\dim \mathcal{C}\}$,

a subset of output indices $out\subset \{1,\cdots ,\dim \mathcal{C}\}$,

a smooth mapping $f$ from ${\mathbf{R}}^{\Vert in\Vert }$ to ${\mathbf{R}}^{\Vert out\Vert }$, satisfying the following properties:

$in\cap out>>=\varnothing$,

for any $\mathbf{p}\in \mathcal{C}$, $\mathbf{q}=>>E(\mathbf{p})$ is defined by

$$\begin{array}{rl}& {\mathbf{q}}_{\overline{out}}={\mathbf{p}}_{\overline{out}}\\ & {\mathbf{q}}_{out}=f({\mathbf{p}}_{in}).\end{array}$$

Right hand side.

For manipulation planning, it is useful to handle a parameterizable right hand side $rhs$. The expression above thus becomes

$$\begin{array}{c}{\mathbf{q}}_{out}=f({\mathbf{p}}_{in})+rhs.\end{array}$$

The right hand side may be set using the various methods ExplicitConstraintSet::rightHandSide and ExplicitConstraintSet::rightHandSideFromInput.

For some applications like manipulation planning, an invertible function $g$ (of known inverse $g^{-1}$) can be specified for each explicit constraint $E$. The above expression then becomes:

$$\begin{array}{c}g({\mathbf{q}}_{out})=f({\mathbf{p}}_{in})+rhs\end{array}$$

To add explicit constraints, use methods ExplicitConstraintSet::add. If the constraint to add is not compatible with the previous one, this method returns -1.

Method ExplicitConstraintSet::solve solves the explicit constraints.

The combination of compatible explicit constraints is an explicit constraint. As such this class can be considered as an explicit constraint.

We will therefore use the following notation

$in$ for the set of indices of input variables,

$out$ for the set of indices of output variables.

HierarchicalIterative

Solve a system of non-linear equations on a robot configuration

The non-linear system of equations is built by adding equations with method HierarchicalIterative::add.

Note that a hierarchy between the equations can be provided. In this case, the solver will try to solve the highest priority equations first, and then to solve the lower priority equations. Note that priorities are in decreasing order: 0 has higher priority than 1.

The algorithm used is a Newton-Raphson like algorithm that works as follows: for a single level of priority, let $f(\mathbf{q})=0$ be the system of equations where $f$ is a $C^1$ mapping from the robot configuration space to a Lie group space $\mathcal{L}$.

Starting from initial guess ${\mathbf{q}}_0$, the method HierarchicalIterative::solve builds a sequence of configurations ${\mathbf{q}}_i$ as follows:

$$\begin{array}{r}{\mathbf{q}}_{i+1}={\mathbf{q}}_i-{\alpha }_i\frac{\partial f}{\partial \mathbf{q}}({\mathbf{q}}_i{)}^{+}f({\mathbf{q}}_i)\end{array}$$

where

$\frac{\partial f}{\partial \mathbf{q}}({\mathbf{q}}_i{)}^{+}$ is the Moore-Penrose pseudo-inverse of the system Jacobian,

${\alpha }_i$ is a sequence of real numbers depending on the line search strategy. Possible line-search strategies are lineSearch::Constant, lineSearch::Backtracking, lineSearch::FixedSequence, lineSearch::ErrorNormBased. until

the residual $$\begin{gathered} |f(\mathbf{q}) \\ | \end{gathered}$$ is below an error threshold, or

the maximal number of iterations has been reached.

The computation of the direction of descent in the case of multiple level of hierarchy is described in this paper.

The error threshold can be accessed by methods HierarchicalIterative::errorThreshold. The maximal number of iterations can be accessed by methods HierarchicalIterative::maxIterations.

Solving equations one after the other

For some applications, it can be more efficient to solve a set of equations one after the other. In other words, an equation is ignored until the previous one is solved (norm below the threshold). To do so, introduce the equations using method HierarchicalIterative::add with increasing value of priority, and call method HierarchicalIterative::solveLevelByLevel(true). Lie group

The unknowns $\mathbf{q}$ may take values in a more general set than the configuration space of a robot. This set should be a Cartesian product of Lie groups: hpp::pinocchio::LiegroupSpace.

Saturation

Right hand side and comparison types

Instead of $f(\mathbf{q})=0$, other constraints can be defined. Several comparison types are available:

Equality — $f(\mathbf{q})=rhs$, where $rhs$ is a parameterizable right hand side,

EqualToZero — $f(\mathbf{q})=0$,

Superior — $f(\mathbf{q})>0$

Inferior — $f(\mathbf{q})<0$ If several constraint are of type equality, the right hand side of the system of equations can be modified by methods HierarchicalIterative::rightHandSideFromInput, HierarchicalIterative::rightHandSide.

Free variables

Some variables can be locked, or computed explicitely. In this case, the iterative resolution will only change the other variables called free variables. methods

freeVariables (const Indices_t& indices) and

freeVariables (const Indices_t& indices).

Implicit

This class represents a parameterizable numerical constraint that compares the output of a function $h$ to a right hand side Lie group element.

Definition

The function $h$ takes input in a configuration space $\mathcal{C}$ and output in a Lie group $\mathbf{L}$,

the dimensions of $\mathbf{L}$ and of its tangent space are respectively $(n_q,n_v)$.

The comparison is represented by a vector $\mathbf{c}$ of dimension $n_v$ with values in enum hpp::constraints::ComparisonType = { $\mathbf{Equality}$, $\mathbf{EqualToZero}$, $\mathbf{Inferior}$, $\mathbf{Superior}$ }.

The right hand side is Lie group element of dimension $n_q$.

Error

A configuration $\mathbf{q}$ is said to satisfy the constraint for a given right hand side if and only if the error $e$ as computed below is smaller in norm than a given threshold.

Let

$$\Delta =h(\mathbf{q})-rhs\in {\mathbf{R}}^{n_v},$$

for each component $i\in \{0,\cdots ,n_v-1\}$,

if $c_i$ is $\mathbf{Inferior}$, $e_i=\max (0,{\Delta }_i)$,

if $c_i$ is $\mathbf{Superior}$, $e_i=\min (0,{\Delta }_i)$,

if $c_i$ is $\mathbf{Equality}$, $e_i={\Delta }_i$,

if $c_i$ is $\mathbf{EqualToZero}$, $e_i={\Delta }_i$.

Mask

A mask is a vector of Boolean values of size $n_v$. Values set to false means that the corresponding component of the error defined above is not taken into account to determine whether the constraint is satisfied. The active rows of the constraint may be accessed via method activeRows.

Parameterizable right hand side

Lines with $\mathbf{Equality}$ comparator in the above definition of the error need a parameter, while lines with comparators $\mathbf{Inferior}$, $\mathbf{Superior}$, or $\mathbf{EqualToZero}$ do not. As a consequence, the right hand side of the constraint is defined by a vector $\lambda$ of parameters of size the number of $\mathbf{Equality}$ occurences in vector $\mathbf{c}$. The right hand side is then defined as in the following example:

$$rhs=\exp \left(\begin{array}{c}{\lambda }_1\\ 0\\ 0\\ {\lambda }_2\\ ⋮\end{array}\right)\mathbf{c}=\left(\begin{array}{c}\mathbf{Equality}\\ \mathbf{EqualToZero}\\ \mathbf{Inferior}\\ \mathbf{Equality}\\ ⋮\end{array}\right)$$

To retrieve the size of vector $\lambda$, call method Implicit::parameterSize (). To set and get the right hand side value, use method Implicit::rightHandSide.

Time varying right hand side

The right hand side of the constraint may depend on time, for instance if the constraint is associated to a trajectory following task. In this case, the right hand side is a function from $\mathbf{R}$ to $\mathbf{L}$.

LockedJoint

Inherits: Implicit

Implementation of constraint specific to a locked joint.

The implicit formulation as defined in class Implicit is given by

$$\begin{array}{c}h(\mathbf{q})=q_{out}-value\end{array}$$

where $value$ is an element of the configuration space of the locked joint passed to method create.

Note that $h$ takes values in ${\mathbf{R}}^{nv}$ where $nv$ is the dimension of the joint tangent space.

The explicit formulation is given by

$$\begin{array}{c}{q}_{out}=value+rhs\end{array}$$

where coordinates of $rhs$ corresponding to comparison types different from Equality are set to 0.

As such, the relation between the explicit formulation and the implicit formulation is the default one.

Manipulability

Inherits: DifferentiableFunction

MinManipulability

Inherits: DifferentiableFunction

Orientation

Inherits: DifferentiableFunction

Position

Inherits: DifferentiableFunction

RelativeCom

Inherits: DifferentiableFunction

Constraint on the relative position of the center of mass

The value of the function is defined as the position of the center of mass in the reference frame of a joint.

$$\begin{array}{rlr}\mathbf{f}(\mathbf{q})& =& R^T\left(\mathbf{x}-\mathbf{t}\right)-{\mathbf{x}}^{\ast }\\ \dot{\mathbf{f}}& =& R^T\left(J_{com}+[\mathbf{x}-\mathbf{t}{]}_{\times }{J}_{joint}^{\omega }-J_{joint}^{\mathbf{v}}\right)\dot{\mathbf{q}}\end{array}$$

where

$$\left(\begin{array}{cc}R& \mathbf{t}\\ 0& 1\end{array}\right)$$

is the position of the joint,

$\mathbf{x}$ is the position of the center of mass,

${\mathbf{x}}^{\ast }$ is the desired position of the center of mass expressed in joint frame.

RelativeOrientation

Inherits: DifferentiableFunction

RelativePosition

Inherits: DifferentiableFunction

RelativeTransformation

Inherits: DifferentiableFunction

RelativeTransformationR3xSO3

Inherits: DifferentiableFunction

SolverStatus

Inherits: int

Transformation

Inherits: DifferentiableFunction

segment