|
virtual ImplicitPtr_t | copy () const |
| Copy object and return shared pointer to copy. More...
|
|
virtual void | outputValue (LiegroupElementRef result, vectorIn_t qin, LiegroupElementConstRef rhs) const |
|
virtual void | jacobianOutputValue (vectorIn_t qin, LiegroupElementConstRef f_value, LiegroupElementConstRef rhs, matrixOut_t jacobian) const |
|
virtual ImplicitPtr_t | copy () const |
| Copy object and return shared pointer to copy. More...
|
|
DifferentiableFunctionPtr_t | explicitFunction () const |
|
const segments_t & | outputConf () const |
| Get output configuration variables. More...
|
|
const segments_t & | outputVelocity () const |
| Get output degrees of freedom. More...
|
|
const segments_t & | inputConf () const |
| Get input configuration variables. More...
|
|
const segments_t & | inputVelocity () const |
| Get input degrees of freedom. More...
|
|
virtual void | outputValue (LiegroupElementRef result, vectorIn_t qin, LiegroupElementConstRef rhs) const |
|
virtual void | jacobianOutputValue (vectorIn_t qin, LiegroupElementConstRef g_value, LiegroupElementConstRef rhs, matrixOut_t jacobian) const |
|
bool | operator== (const Implicit &other) const |
| Operator equality. More...
|
|
virtual ImplicitPtr_t | copy () const |
| Copy object and return shared pointer to copy. More...
|
|
virtual | ~Implicit () |
|
const ComparisonTypes_t & | comparisonType () const |
| Return the ComparisonType. More...
|
|
void | comparisonType (const ComparisonTypes_t &comp) |
| Set the comparison type. More...
|
|
const segments_t & | activeRows () const |
|
bool | checkAllRowsActive () const |
| Check if all rows are active (no inactive rows) More...
|
|
const Eigen::RowBlockIndices & | equalityIndices () const |
| Get indices of constraint coordinates that are equality. More...
|
|
void | setInactiveRowsToZero (vectorOut_t error) const |
|
DifferentiableFunction & | function () const |
| Return a reference to function \(h\). More...
|
|
const DifferentiableFunctionPtr_t & | functionPtr () const |
| Return a reference to function \(h\). More...
|
|
virtual std::pair< JointConstPtr_t, JointConstPtr_t > | doesConstrainRelPoseBetween (DeviceConstPtr_t robot) const |
|
void | rightHandSideFromConfig (ConfigurationIn_t config, LiegroupElementRef rhs) |
|
bool | checkRightHandSide (LiegroupElementConstRef rhs) const |
|
size_type | parameterSize () const |
|
size_type | rightHandSideSize () const |
|
void | rightHandSideFunction (const DifferentiableFunctionPtr_t &rhsF) |
|
const DifferentiableFunctionPtr_t & | rightHandSideFunction () const |
|
vectorIn_t | rightHandSideAt (const value_type &s) |
|
|
static RelativePosePtr_t | create (const std::string &name, const DevicePtr_t &robot, const JointConstPtr_t &joint1, const JointConstPtr_t &joint2, const Transform3f &frame1, const Transform3f &frame2, ComparisonTypes_t comp, std::vector< bool > mask=std::vector< bool >()) |
|
static RelativePosePtr_t | createCopy (const RelativePosePtr_t &other) |
|
static ExplicitPtr_t | create (const LiegroupSpacePtr_t &configSpace, const DifferentiableFunctionPtr_t &function, const segments_t &inputConf, const segments_t &outputConf, const segments_t &inputVelocity, const segments_t &outputVelocity, const ComparisonTypes_t &comp=ComparisonTypes_t(), std::vector< bool > mask=std::vector< bool >()) |
|
static ExplicitPtr_t | createCopy (const ExplicitPtr_t &other) |
| Create a copy and return shared pointer. More...
|
|
static ImplicitPtr_t | create (const DifferentiableFunctionPtr_t &func, ComparisonTypes_t comp, std::vector< bool > mask=std::vector< bool >()) |
|
static ImplicitPtr_t | createCopy (const ImplicitPtr_t &other) |
| Create a copy and return shared pointer. More...
|
|
|
| RelativePose (const std::string &name, const DevicePtr_t &robot, const JointConstPtr_t &joint1, const JointConstPtr_t &joint2, const Transform3f &frame1, const Transform3f &frame2, ComparisonTypes_t comp=ComparisonTypes_t(), std::vector< bool > mask=std::vector< bool >(6, true)) |
|
| RelativePose (const RelativePose &other) |
| Copy constructor. More...
|
|
void | init (RelativePoseWkPtr_t weak) |
| Store weak pointer to itself. More...
|
|
| Explicit (const LiegroupSpacePtr_t &configSpace, const DifferentiableFunctionPtr_t &function, const segments_t &inputConf, const segments_t &outputConf, const segments_t &inputVelocity, const segments_t &outputVelocity, const ComparisonTypes_t &comp, std::vector< bool > mask) |
|
| Explicit (const DifferentiableFunctionPtr_t &implicitFunction, const DifferentiableFunctionPtr_t &function, const segments_t &inputConf, const segments_t &outputConf, const segments_t &inputVelocity, const segments_t &outputVelocity, const ComparisonTypes_t &comp, std::vector< bool > mask) |
| (const LiegroupSpacePtr_t&, constDifferentiableFunctionPtr_t&, const segments_t& inputConf, constsegments_t& outputConf, const segments_t& inputVelocity, constsegments_t&, const ComparisonTypes_t&); - Parameters
-
More...
|
|
| Explicit (const Explicit &other) |
| Copy constructor. More...
|
|
bool | isEqual (const Implicit &other, bool swapAndTest) const |
|
void | init (const ExplicitWkPtr_t &weak) |
|
| Explicit () |
|
| Implicit (const DifferentiableFunctionPtr_t &function, ComparisonTypes_t comp, std::vector< bool > mask) |
|
| Implicit (const Implicit &other) |
| Copy constructor. More...
|
|
virtual bool | isEqual (const Implicit &other, bool swapAndTest) const |
|
void | init (const ImplicitWkPtr_t &weak) |
|
| Implicit () |
|
Constraint of relative pose between two frames on a kinematic chain
Given an input configuration \(\mathbf{q}\), solving this constraint consists in computing output variables with respect to input variables:
\[
\mathbf{q}_{out} = g(\mathbf{q}_{in})
\]
where \(\mathbf{q}_{out}\) are the configuration variables of input joint2, \({q}_{in}\) are the configuration variables of input joint1 and parent joints, and \(g\) is a mapping with values is SE(3). Note that joint2 should be a freeflyer joint.
- Note
- According to the documentation of class Explicit, the implicit formulation should be
\[
f(\mathbf{q}) = \mathbf{q}_{out} - g(\mathbf{q}_{in}).
\]
As function \(g\) takes values in SE(3), the above expression is equivalent to
\[
f(\mathbf{q}) = \log_{SE(3)}\left(g(\mathbf{q}_{in})^{-1}
\mathbf{q}_{out}\right)
\]
that represents the log of the error of input joint2 pose ( \(\mathbf{q}_{out}\)) with respect to its desired value ( \(g(\mathbf{q}_{in}\)). The problem with this expression is that it is different from the corresponding implicit formulation hpp::constraints::RelativeTransformationR3xSO3 that compares the poses of input joint1 and joint2. For manipulation planning applications where pairs of constraints and complements are replaced by an explicit constraint, this difference of formulation results in inconsistencies such as a configuration satisfying one formulation (the error being below the threshold) but not the other one. To cope with this issue, the default implicit formulation is replaced by the one defined by class hpp::constraints::RelativeTransformationR3xSO3.
Compute the value of the output configuration variables
- Parameters
-
qin | input configuration variables, |
rhs | right hand side of constraint |
\begin{equation}
f \left(\mathbf{q}_{in}\right) + rhs_{expl}
\end{equation}
where \(rhs_{expl}\) is the explicit right hand side converted using the following expression:
\begin{equation}
rhs_{expl} = \log_{SE(3)}\left( F_{2/J_2} rhs_{impl} F_{2/J_2}^{-1}
\right)
\end{equation}
Reimplemented from hpp::constraints::Explicit.