20 #ifndef HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH 21 # define HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH 23 # include <pinocchio/spatial/se3.hpp> 25 # include <hpp/util/serialization-fwd.hh> 27 # include <hpp/pinocchio/joint.hh> 35 namespace constraints {
37 template <
bool rel>
struct GenericTransformationModel
40 bool R1isID, R2isID, t1isZero, t2isZero;
42 bool fullPos, fullOri;
48 GenericTransformationModel (
const size_type nCols) :
49 joint2(), R1isID(true), R2isID(true), t1isZero(true), t2isZero(true),
50 fullPos(false), fullOri(false), cols (nCols)
51 { F1inJ1.setIdentity(); F2inJ2.setIdentity(); }
52 void checkIsIdentity1() {
53 R1isID = F1inJ1.rotation().isIdentity();
54 t1isZero = F1inJ1.translation().isZero();
56 void checkIsIdentity2() {
57 R2isID = F2inJ2.rotation().isIdentity();
58 t2isZero = F2inJ2.translation().isZero();
61 template <>
struct GenericTransformationModel<true> :
62 GenericTransformationModel<false>
67 GenericTransformationModel (
const size_type nCols) :
68 GenericTransformationModel<false>(nCols), joint1() {}
110 template <
int _Options>
112 public DifferentiableFunction
115 typedef shared_ptr <GenericTransformation>
Ptr_t;
116 typedef weak_ptr <GenericTransformation>
WkPtr_t;
117 #if __cplusplus >= 201103L 118 static constexpr
bool 119 IsRelative = _Options & RelativeBit,
120 ComputeOrientation = _Options & OrientationBit,
121 ComputePosition = _Options & PositionBit,
122 OutputR3xSO3 = _Options & OutputR3xSO3Bit,
123 IsPosition = ComputePosition && !ComputeOrientation,
124 IsOrientation = !ComputePosition && ComputeOrientation,
125 IsTransform = ComputePosition && ComputeOrientation;
127 ValueSize = (ComputePosition?3:0) +
128 (ComputeOrientation?(OutputR3xSO3?4:3):0),
129 DerSize = (ComputePosition?3:0) + (ComputeOrientation ?3:0);
132 IsRelative = _Options & RelativeBit,
133 ComputeOrientation = _Options & OrientationBit,
134 ComputePosition = _Options & PositionBit,
135 OutputR3xSO3 = _Options & OutputR3xSO3Bit,
136 IsPosition = ComputePosition && !ComputeOrientation,
137 IsOrientation = !ComputePosition && ComputeOrientation,
138 IsTransform = ComputePosition && ComputeOrientation,
139 ValueSize = (ComputePosition?3:0) +
140 (ComputeOrientation?(OutputR3xSO3?4:3):0),
141 DerSize = (ComputePosition?3:0) + (ComputeOrientation ?3:0)
146 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
159 static Ptr_t create (
const std::string& name,
const DevicePtr_t& robot,
161 std::vector <bool> mask = std::vector<bool>(DerSize,
true));
177 static Ptr_t create (
const std::string& name,
const DevicePtr_t& robot,
180 std::vector <bool> mask = std::vector<bool>(DerSize,
true));
194 static Ptr_t create (
const std::string& name,
const DevicePtr_t& robot,
197 std::vector <bool> mask = std::vector<bool>(DerSize,
true));
217 static Ptr_t create (
const std::string& name,
const DevicePtr_t& robot,
220 std::vector <bool> mask = std::vector<bool>(DerSize,
true));
228 m_.F1inJ1 = reference;
229 m_.checkIsIdentity1();
230 m_.F2inJ2.setIdentity ();
231 m_.checkIsIdentity2();
237 return m_.F1inJ1.actInv(m_.F2inJ2);
244 computeActiveParams();
245 assert (!joint || joint->robot () == robot_);
250 return m_.getJoint1();
256 computeActiveParams();
257 assert (!joint || (joint->index() > 0 && joint->robot () == robot_));
268 m_.checkIsIdentity1();
277 m_.checkIsIdentity2();
284 virtual std::ostream& print (std::ostream& o)
const;
295 std::vector <bool> mask);
301 computeActiveParams();
313 void computeActiveParams ();
315 GenericTransformationModel<IsRelative> m_;
317 const std::vector <bool> mask_;
326 #endif // HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH
Definition: active-set-differentiable-function.hh:24
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:97
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:94
assert(d.lhs()._blocks()==d.rhs()._blocks())
pinocchio::JointConstPtr_t JointConstPtr_t
Definition: fwd.hh:39
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:54
pinocchio::size_type size_type
Definition: fwd.hh:36
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:46
pinocchio::Transform3f Transform3f
Definition: fwd.hh:52