| Line |
Branch |
Exec |
Source |
| 1 |
|
|
// Copyright (c) 2018, LAAS-CNRS |
| 2 |
|
|
// Authors: Florent Lamiraux |
| 3 |
|
|
// |
| 4 |
|
|
|
| 5 |
|
|
// Redistribution and use in source and binary forms, with or without |
| 6 |
|
|
// modification, are permitted provided that the following conditions are |
| 7 |
|
|
// met: |
| 8 |
|
|
// |
| 9 |
|
|
// 1. Redistributions of source code must retain the above copyright |
| 10 |
|
|
// notice, this list of conditions and the following disclaimer. |
| 11 |
|
|
// |
| 12 |
|
|
// 2. Redistributions in binary form must reproduce the above copyright |
| 13 |
|
|
// notice, this list of conditions and the following disclaimer in the |
| 14 |
|
|
// documentation and/or other materials provided with the distribution. |
| 15 |
|
|
// |
| 16 |
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
| 17 |
|
|
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
| 18 |
|
|
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR |
| 19 |
|
|
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT |
| 20 |
|
|
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, |
| 21 |
|
|
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT |
| 22 |
|
|
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, |
| 23 |
|
|
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY |
| 24 |
|
|
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
| 25 |
|
|
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
| 26 |
|
|
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH |
| 27 |
|
|
// DAMAGE. |
| 28 |
|
|
|
| 29 |
|
|
#ifndef HPP_CONSTRAINTS_EXPLICIT_RELATIVE_POSE_HH |
| 30 |
|
|
#define HPP_CONSTRAINTS_EXPLICIT_RELATIVE_POSE_HH |
| 31 |
|
|
|
| 32 |
|
|
#include <hpp/constraints/explicit.hh> |
| 33 |
|
|
#include <pinocchio/spatial/se3.hpp> |
| 34 |
|
|
|
| 35 |
|
|
namespace hpp { |
| 36 |
|
|
namespace constraints { |
| 37 |
|
|
namespace explicit_ { |
| 38 |
|
|
/// Constraint of relative pose between two frames on a kinematic chain |
| 39 |
|
|
/// |
| 40 |
|
|
/// Given an input configuration \f$\mathbf{q}\f$, solving this constraint |
| 41 |
|
|
/// consists in computing output variables with respect to input |
| 42 |
|
|
/// variables: |
| 43 |
|
|
/// \f[ |
| 44 |
|
|
/// \mathbf{q}_{out} = g(\mathbf{q}_{in}) |
| 45 |
|
|
///\f] |
| 46 |
|
|
/// where \f$\mathbf{q}_{out}\f$ are the configuration variables of |
| 47 |
|
|
/// input joint2, \f${q}_{in}\f$ are the configuration variables |
| 48 |
|
|
/// of input joint1 and parent joints, and \f$g\f$ is a mapping |
| 49 |
|
|
/// with values is SE(3). Note that joint2 should be a |
| 50 |
|
|
/// freeflyer joint. |
| 51 |
|
|
/// |
| 52 |
|
|
/// \note According to the documentation of class Explicit, the |
| 53 |
|
|
/// implicit formulation should be |
| 54 |
|
|
/// \f[ |
| 55 |
|
|
/// f(\mathbf{q}) = \mathbf{q}_{out} - g(\mathbf{q}_{in}). |
| 56 |
|
|
/// \f] |
| 57 |
|
|
/// As function \f$g\f$ takes values in SE(3), the above expression is |
| 58 |
|
|
/// equivalent to |
| 59 |
|
|
/// \f[ |
| 60 |
|
|
/// f(\mathbf{q}) = \log_{SE(3)}\left(g(\mathbf{q}_{in})^{-1} |
| 61 |
|
|
/// \mathbf{q}_{out}\right) |
| 62 |
|
|
/// \f] |
| 63 |
|
|
/// that represents the log of the error of input joint2 pose |
| 64 |
|
|
/// (\f$\mathbf{q}_{out}\f$) with respect to its desired value |
| 65 |
|
|
/// (\f$g(\mathbf{q}_{in}\f$). The problem with this expression is |
| 66 |
|
|
/// that it is different from the corresponding implicit formulation |
| 67 |
|
|
/// hpp::constraints::RelativeTransformationR3xSO3 that compares the poses of |
| 68 |
|
|
/// input joint1 and joint2. For manipulation planning applications where pairs |
| 69 |
|
|
/// of constraints and complements are replaced by an explicit constraint, |
| 70 |
|
|
/// this difference of formulation results in inconsistencies such as a |
| 71 |
|
|
/// configuration satisfying one formulation (the error being below the |
| 72 |
|
|
/// threshold) but not the other one. To cope with this issue, the default |
| 73 |
|
|
/// implicit formulation is replaced by the one defined by class |
| 74 |
|
|
/// hpp::constraints::RelativeTransformationR3xSO3. |
| 75 |
|
|
class HPP_CONSTRAINTS_DLLAPI RelativePose : public Explicit { |
| 76 |
|
|
public: |
| 77 |
|
|
/// Copy object and return shared pointer to copy |
| 78 |
|
|
virtual ImplicitPtr_t copy() const; |
| 79 |
|
|
/// Create instance and return shared pointer |
| 80 |
|
|
/// |
| 81 |
|
|
/// \param name the name of the constraints, |
| 82 |
|
|
/// \param robot the robot the constraints is applied to, |
| 83 |
|
|
/// \param joint1 the first joint the transformation of which is |
| 84 |
|
|
/// constrained, |
| 85 |
|
|
/// \param joint2 the second joint the transformation of which is |
| 86 |
|
|
/// constrained, |
| 87 |
|
|
/// \param frame1 position of a fixed frame in joint 1, |
| 88 |
|
|
/// \param frame2 position of a fixed frame in joint 2, |
| 89 |
|
|
/// \param comp vector of comparison types |
| 90 |
|
|
/// \param mask mask defining which components of the error are |
| 91 |
|
|
/// taken into account to determine whether the constraint |
| 92 |
|
|
/// is satisfied. |
| 93 |
|
|
/// \note if joint1 is 0x0, joint 1 frame is considered to be the global |
| 94 |
|
|
/// frame. |
| 95 |
|
|
static RelativePosePtr_t create( |
| 96 |
|
|
const std::string& name, const DevicePtr_t& robot, |
| 97 |
|
|
const JointConstPtr_t& joint1, const JointConstPtr_t& joint2, |
| 98 |
|
|
const Transform3s& frame1, const Transform3s& frame2, |
| 99 |
|
|
ComparisonTypes_t comp, std::vector<bool> mask = std::vector<bool>()); |
| 100 |
|
|
|
| 101 |
|
|
static RelativePosePtr_t createCopy(const RelativePosePtr_t& other); |
| 102 |
|
|
|
| 103 |
|
|
/// Compute the value of the output configuration variables |
| 104 |
|
|
/// \param qin input configuration variables, |
| 105 |
|
|
/// \param rhs right hand side of constraint |
| 106 |
|
|
/// |
| 107 |
|
|
/// \f{equation} |
| 108 |
|
|
/// f \left(\mathbf{q}_{in}\right) + rhs_{expl} |
| 109 |
|
|
/// \f} |
| 110 |
|
|
/// where \f$rhs_{expl}\f$ is the explicit right hand side converted |
| 111 |
|
|
/// using the following expression: |
| 112 |
|
|
/// \f{equation} |
| 113 |
|
|
/// rhs_{expl} = \log_{SE(3)}\left( F_{2/J_2} rhs_{impl} F_{2/J_2}^{-1} |
| 114 |
|
|
/// \right) |
| 115 |
|
|
/// \f} |
| 116 |
|
|
virtual void outputValue(LiegroupElementRef result, vectorIn_t qin, |
| 117 |
|
|
LiegroupElementConstRef rhs) const; |
| 118 |
|
|
|
| 119 |
|
|
/// Compute Jacobian of output value |
| 120 |
|
|
/// |
| 121 |
|
|
/// \f{eqnarray*} |
| 122 |
|
|
/// J &=& \frac{\partial}{\partial\mathbf{q}_{in}}\left( |
| 123 |
|
|
/// f(\mathbf{q}_{in}) + rhs\right). |
| 124 |
|
|
/// \f} |
| 125 |
|
|
/// \param qin vector of input variables, |
| 126 |
|
|
/// \param f_value \f$f(\mathbf{q}_{in})\f$ to avoid recomputation, |
| 127 |
|
|
/// \param rhs right hand side (of implicit formulation). |
| 128 |
|
|
virtual void jacobianOutputValue(vectorIn_t qin, |
| 129 |
|
|
LiegroupElementConstRef f_value, |
| 130 |
|
|
LiegroupElementConstRef rhs, |
| 131 |
|
|
matrixOut_t jacobian) const; |
| 132 |
|
|
|
| 133 |
|
|
protected: |
| 134 |
|
|
/// Constructor |
| 135 |
|
|
/// |
| 136 |
|
|
/// \param name the name of the constraints, |
| 137 |
|
|
/// \param robot the robot the constraints is applied to, |
| 138 |
|
|
/// \param joint1 the first joint the transformation of which is |
| 139 |
|
|
/// constrained, |
| 140 |
|
|
/// \param joint2 the second joint the transformation of which is |
| 141 |
|
|
/// constrained, |
| 142 |
|
|
/// \param frame1 position of a fixed frame in joint 1, |
| 143 |
|
|
/// \param frame2 position of a fixed frame in joint 2, |
| 144 |
|
|
/// \param comp vector of comparison types |
| 145 |
|
|
/// \param mask mask defining which components of the error are |
| 146 |
|
|
/// taken into account to determine whether the constraint |
| 147 |
|
|
/// is satisfied. |
| 148 |
|
|
/// \note if joint1 is 0x0, joint 1 frame is considered to be the global |
| 149 |
|
|
/// frame. |
| 150 |
|
|
RelativePose(const std::string& name, const DevicePtr_t& robot, |
| 151 |
|
|
const JointConstPtr_t& joint1, const JointConstPtr_t& joint2, |
| 152 |
|
|
const Transform3s& frame1, const Transform3s& frame2, |
| 153 |
|
|
ComparisonTypes_t comp = ComparisonTypes_t(), |
| 154 |
|
|
std::vector<bool> mask = std::vector<bool>(6, true)); |
| 155 |
|
|
|
| 156 |
|
|
/// Copy constructor |
| 157 |
|
|
RelativePose(const RelativePose& other); |
| 158 |
|
|
|
| 159 |
|
|
/// Store weak pointer to itself |
| 160 |
|
|
void init(RelativePoseWkPtr_t weak); |
| 161 |
|
|
|
| 162 |
|
|
private: |
| 163 |
|
|
/** Convert right hand side |
| 164 |
|
|
|
| 165 |
|
|
\param implicitRhs right hand side of implicit formulation, this |
| 166 |
|
|
is an element of $SE(3)$ |
| 167 |
|
|
\retval explicitRhs right hand side of explicit formulation, this |
| 168 |
|
|
is an element of $\mathbf{R}^6$. |
| 169 |
|
|
|
| 170 |
|
|
For this constraint, the implicit formulation does not derive |
| 171 |
|
|
from the explicit formulation. The explicit form writes |
| 172 |
|
|
|
| 173 |
|
|
\f{eqnarray} |
| 174 |
|
|
rhs_{expl} &=& \log_{SE(3)} \left(F_{2/J_2} F_{1/J_1}^{-1} J_1^{-1} |
| 175 |
|
|
J_2\right)\\ |
| 176 |
|
|
rhs_{impl} &=& F_{1/J_1}^{-1} J_1^{-1}J_2 F_{2/J_2} |
| 177 |
|
|
\f} |
| 178 |
|
|
Thus |
| 179 |
|
|
\f{equation} |
| 180 |
|
|
rhs_{expl} = \log_{SE(3)}\left( F_{2/J_2}rhs_{impl} |
| 181 |
|
|
F_{2/J_2}^{-1}\right) |
| 182 |
|
|
\f} |
| 183 |
|
|
*/ |
| 184 |
|
|
void implicitToExplicitRhs(LiegroupElementConstRef implicitRhs, |
| 185 |
|
|
vectorOut_t explicitRhs) const; |
| 186 |
|
|
/** Convert right hand side |
| 187 |
|
|
|
| 188 |
|
|
\param explicitRhs right hand side of explicit formulation, |
| 189 |
|
|
\retval implicitRhs right hand side of implicit formulation. |
| 190 |
|
|
|
| 191 |
|
|
For this constraint, the implicit formulation does not derive |
| 192 |
|
|
from the explicit formulation. The explicit form writes |
| 193 |
|
|
|
| 194 |
|
|
\f{eqnarray} |
| 195 |
|
|
rhs_{expl} &=& \log_{SE(3)} \left(F_{2/J_2} F_{1/J_1}^{-1} J_1^{-1} |
| 196 |
|
|
J_2\right)\\ |
| 197 |
|
|
rhs_{impl} &=& F_{1/J_1}^{-1} J_1^{-1}J_2 F_{2/J_2} |
| 198 |
|
|
\f} |
| 199 |
|
|
Thus |
| 200 |
|
|
\f{equation} |
| 201 |
|
|
rhs_{impl} = F_{2/J_2}^{-1} \exp_{SE(3)}(rhs_{expl}) F_{2/J_2} |
| 202 |
|
|
\f} |
| 203 |
|
|
*/ |
| 204 |
|
|
void explicitToImplicitRhs(vectorIn_t explicitRhs, |
| 205 |
|
|
LiegroupElementRef implicitRhs) const; |
| 206 |
|
|
// Create LiegroupSpace instances to avoid useless allocation. |
| 207 |
|
|
static LiegroupSpacePtr_t SE3; |
| 208 |
|
|
static LiegroupSpacePtr_t R3xSO3; |
| 209 |
|
|
JointConstPtr_t joint1_, joint2_; |
| 210 |
|
|
Transform3s frame1_; |
| 211 |
|
|
Transform3s frame2_; |
| 212 |
|
|
RelativePoseWkPtr_t weak_; |
| 213 |
|
|
|
| 214 |
|
✗ |
RelativePose() {} |
| 215 |
|
|
HPP_SERIALIZABLE(); |
| 216 |
|
|
}; // class RelativePose |
| 217 |
|
|
} // namespace explicit_ |
| 218 |
|
|
} // namespace constraints |
| 219 |
|
|
} // namespace hpp |
| 220 |
|
|
|
| 221 |
|
12 |
BOOST_CLASS_EXPORT_KEY(hpp::constraints::explicit_::RelativePose) |
| 222 |
|
|
|
| 223 |
|
|
#endif // HPP_CONSTRAINTS_EXPLICIT_RELATIVE_POSE_HH |
| 224 |
|
|
|