| Line |
Branch |
Exec |
Source |
| 1 |
|
|
// Copyright (c) 2015 - 2018, CNRS |
| 2 |
|
|
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr), 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_TRANSFORMATION_HH |
| 30 |
|
|
#define HPP_CONSTRAINTS_EXPLICIT_RELATIVE_TRANSFORMATION_HH |
| 31 |
|
|
|
| 32 |
|
|
#include <hpp/constraints/explicit.hh> |
| 33 |
|
|
#include <hpp/constraints/generic-transformation.hh> |
| 34 |
|
|
#include <hpp/constraints/matrix-view.hh> |
| 35 |
|
|
|
| 36 |
|
|
namespace hpp { |
| 37 |
|
|
namespace constraints { |
| 38 |
|
|
using constraints::RelativeTransformation; |
| 39 |
|
|
using constraints::RelativeTransformationPtr_t; |
| 40 |
|
|
|
| 41 |
|
|
namespace explicit_ { |
| 42 |
|
|
/// \addtogroup constraints |
| 43 |
|
|
/// \{ |
| 44 |
|
|
|
| 45 |
|
|
/// Relative transformation as a mapping between configuration variables |
| 46 |
|
|
/// |
| 47 |
|
|
/// When the positions of two joints are constrained by a full |
| 48 |
|
|
/// transformation constraint, if the second joint is held by a freeflyer |
| 49 |
|
|
/// joint, the position of this latter joint can be |
| 50 |
|
|
/// explicitly expressed with respect to the position of the first joint. |
| 51 |
|
|
/// |
| 52 |
|
|
/// This class provides this expression. The input configuration variables |
| 53 |
|
|
/// are the joint values of all joints except the above mentioned freeflyer |
| 54 |
|
|
/// joint. The output configuration variables are the 7 configuration |
| 55 |
|
|
/// variables of the freeflyer joint. |
| 56 |
|
|
/// |
| 57 |
|
|
class HPP_CONSTRAINTS_DLLAPI RelativeTransformation |
| 58 |
|
|
: public DifferentiableFunction { |
| 59 |
|
|
public: |
| 60 |
|
|
/// Return a shared pointer to a new instance |
| 61 |
|
|
/// |
| 62 |
|
|
/// \param name the name of the constraints, |
| 63 |
|
|
/// \param robot the robot the constraints is applied to, |
| 64 |
|
|
/// \param joint1 input joint |
| 65 |
|
|
/// \param joint2 output joint: position of this joint is computed with |
| 66 |
|
|
/// respect to joint1 position |
| 67 |
|
|
/// \param frame1 position of a fixed frame in joint 1, |
| 68 |
|
|
/// \param frame2 position of a fixed frame in joint 2, |
| 69 |
|
|
static RelativeTransformationPtr_t create(const std::string& name, |
| 70 |
|
|
const DevicePtr_t& robot, |
| 71 |
|
|
const JointConstPtr_t& joint1, |
| 72 |
|
|
const JointConstPtr_t& joint2, |
| 73 |
|
|
const Transform3s& frame1, |
| 74 |
|
|
const Transform3s& frame2); |
| 75 |
|
|
|
| 76 |
|
|
/// Get joint 1 |
| 77 |
|
✗ |
const JointConstPtr_t& joint1() const { return joint1_; } |
| 78 |
|
|
|
| 79 |
|
|
/// Get joint 2 |
| 80 |
|
✗ |
const JointConstPtr_t& joint2() const { return joint2_; } |
| 81 |
|
|
|
| 82 |
|
|
/// Return pair of joints the relative pose between which |
| 83 |
|
|
/// modifies the function value if any |
| 84 |
|
|
/// |
| 85 |
|
|
/// This method is useful to know whether an instance of Implicit constrains |
| 86 |
|
|
/// the relative pose between two joints. |
| 87 |
|
|
/// \return the pair of joints involved, arranged in order of increasing |
| 88 |
|
|
/// joint index, or a pair of empty shared pointers. |
| 89 |
|
|
/// \note if absolute pose (relative pose with respect to "universe"), |
| 90 |
|
|
/// "universe" is returned as empty shared pointer |
| 91 |
|
✗ |
std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween( |
| 92 |
|
|
DeviceConstPtr_t /*robot*/) const { |
| 93 |
|
✗ |
JointConstPtr_t j1 = joint1(); |
| 94 |
|
✗ |
JointConstPtr_t j2 = joint2(); |
| 95 |
|
✗ |
size_type index1 = (j1 ? j1->index() : 0); |
| 96 |
|
✗ |
size_type index2 = (j2 ? j2->index() : 0); |
| 97 |
|
✗ |
if (index1 <= index2) { |
| 98 |
|
✗ |
return std::pair<JointConstPtr_t, JointConstPtr_t>(j1, j2); |
| 99 |
|
|
} else { |
| 100 |
|
✗ |
return std::pair<JointConstPtr_t, JointConstPtr_t>(j2, j1); |
| 101 |
|
|
} |
| 102 |
|
✗ |
}; |
| 103 |
|
|
|
| 104 |
|
|
protected: |
| 105 |
|
|
typedef Eigen::BlockIndex BlockIndex; |
| 106 |
|
|
typedef Eigen::RowBlockIndices RowBlockIndices; |
| 107 |
|
|
typedef Eigen::ColBlockIndices ColBlockIndices; |
| 108 |
|
|
|
| 109 |
|
|
RelativeTransformation(const std::string& name, const DevicePtr_t& robot, |
| 110 |
|
|
const JointConstPtr_t& joint1, |
| 111 |
|
|
const JointConstPtr_t& joint2, |
| 112 |
|
|
const Transform3s& frame1, const Transform3s& frame2, |
| 113 |
|
|
const segments_t inConf, const segments_t outConf, |
| 114 |
|
|
const segments_t inVel, const segments_t outVel, |
| 115 |
|
|
std::vector<bool> mask = std::vector<bool>(6, true)); |
| 116 |
|
|
|
| 117 |
|
|
RelativeTransformation(const RelativeTransformation& other) |
| 118 |
|
|
: DifferentiableFunction(other), |
| 119 |
|
|
robot_(other.robot_), |
| 120 |
|
|
parentJoint_(other.parentJoint_), |
| 121 |
|
|
inConf_(other.inConf_), |
| 122 |
|
|
inVel_(other.inVel_), |
| 123 |
|
|
outConf_(other.outConf_), |
| 124 |
|
|
outVel_(other.outVel_), |
| 125 |
|
|
F1inJ1_invF2inJ2_(other.F1inJ1_invF2inJ2_) {} |
| 126 |
|
|
|
| 127 |
|
|
// Store weak pointer to itself |
| 128 |
|
8 |
void init(const RelativeTransformationWkPtr_t& weak) { weak_ = weak; } |
| 129 |
|
|
|
| 130 |
|
|
/// Compute the value (dimension 7) of the freeflyer joint 2 |
| 131 |
|
|
/// |
| 132 |
|
|
/// \param argument vector of input configuration variables (all joints |
| 133 |
|
|
/// except freeflyer joint) |
| 134 |
|
|
/// \retval result vector of output configuration variables corresponding |
| 135 |
|
|
/// to the freeflyer value. |
| 136 |
|
|
void impl_compute(LiegroupElementRef result, vectorIn_t argument) const; |
| 137 |
|
|
|
| 138 |
|
|
void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg) const; |
| 139 |
|
|
|
| 140 |
|
✗ |
bool isEqual(const DifferentiableFunction& other) const { |
| 141 |
|
|
const RelativeTransformation& castother = |
| 142 |
|
✗ |
dynamic_cast<const RelativeTransformation&>(other); |
| 143 |
|
✗ |
if (!DifferentiableFunction::isEqual(other)) return false; |
| 144 |
|
|
|
| 145 |
|
✗ |
if (robot_ != castother.robot_) return false; |
| 146 |
|
✗ |
if (parentJoint_ != castother.parentJoint_) return false; |
| 147 |
|
✗ |
if (joint1_ != castother.joint1_) return false; |
| 148 |
|
✗ |
if (joint2_ != castother.joint2_) return false; |
| 149 |
|
✗ |
if (frame1_ != castother.frame1_) return false; |
| 150 |
|
✗ |
if (frame2_ != castother.frame2_) return false; |
| 151 |
|
✗ |
if (inConf_.rows() != castother.inConf_.rows()) return false; |
| 152 |
|
✗ |
if (inVel_.cols() != castother.inVel_.cols()) return false; |
| 153 |
|
✗ |
if (outConf_.rows() != castother.outConf_.rows()) return false; |
| 154 |
|
✗ |
if (outVel_.rows() != castother.outVel_.rows()) return false; |
| 155 |
|
✗ |
if (F1inJ1_invF2inJ2_ != castother.F1inJ1_invF2inJ2_) return false; |
| 156 |
|
|
|
| 157 |
|
✗ |
return true; |
| 158 |
|
|
} |
| 159 |
|
|
|
| 160 |
|
|
private: |
| 161 |
|
|
void forwardKinematics(vectorIn_t arg) const; |
| 162 |
|
|
|
| 163 |
|
|
DevicePtr_t robot_; |
| 164 |
|
|
// Parent of the R3 joint. |
| 165 |
|
|
JointConstPtr_t parentJoint_; |
| 166 |
|
|
JointConstPtr_t joint1_, joint2_; |
| 167 |
|
|
Transform3s frame1_, frame2_; |
| 168 |
|
|
RowBlockIndices inConf_; |
| 169 |
|
|
ColBlockIndices inVel_; |
| 170 |
|
|
RowBlockIndices outConf_, outVel_; |
| 171 |
|
|
Transform3s F1inJ1_invF2inJ2_; |
| 172 |
|
|
|
| 173 |
|
|
RelativeTransformationWkPtr_t weak_; |
| 174 |
|
|
|
| 175 |
|
|
// Tmp variables |
| 176 |
|
|
mutable vector_t qsmall_, q_; |
| 177 |
|
|
mutable matrix_t tmpJac_, J2_parent_minus_J1_; |
| 178 |
|
|
|
| 179 |
|
✗ |
RelativeTransformation() {} |
| 180 |
|
|
HPP_SERIALIZABLE(); |
| 181 |
|
|
}; // class RelativeTransformation |
| 182 |
|
|
/// \} |
| 183 |
|
|
|
| 184 |
|
|
} // namespace explicit_ |
| 185 |
|
|
} // namespace constraints |
| 186 |
|
|
} // namespace hpp |
| 187 |
|
|
|
| 188 |
|
12 |
BOOST_CLASS_EXPORT_KEY(hpp::constraints::explicit_::RelativeTransformation) |
| 189 |
|
|
|
| 190 |
|
|
#endif // HPP_CONSTRAINTS_EXPLICIT_RELATIVE_TRANSFORMATION_HH |
| 191 |
|
|
|