20 #ifndef HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH 21 # define HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH 23 # include <pinocchio/spatial/se3.hpp> 25 # include <hpp/pinocchio/joint.hh> 33 namespace constraints {
35 template <
bool rel>
struct GenericTransformationModel
38 bool R1isID, R2isID, t1isZero, t2isZero;
40 bool fullPos, fullOri;
46 GenericTransformationModel (
const size_type nCols) :
47 joint2(), R1isID(true), R2isID(true), t1isZero(true), t2isZero(true),
48 fullPos(false), fullOri(false), cols (nCols)
49 { F1inJ1.setIdentity(); F2inJ2.setIdentity(); }
50 void checkIsIdentity1() {
51 R1isID = F1inJ1.rotation().isIdentity();
52 t1isZero = F1inJ1.translation().isZero();
54 void checkIsIdentity2() {
55 R2isID = F2inJ2.rotation().isIdentity();
56 t2isZero = F2inJ2.translation().isZero();
59 template <>
struct GenericTransformationModel<true> :
60 GenericTransformationModel<false>
65 GenericTransformationModel (
const size_type nCols) :
66 GenericTransformationModel<false>(nCols), joint1() {}
108 template <
int _Options>
110 public DifferentiableFunction
113 typedef boost::shared_ptr <GenericTransformation>
Ptr_t;
114 typedef boost::weak_ptr <GenericTransformation>
WkPtr_t;
115 #if __cplusplus >= 201103L 116 static constexpr
bool 117 IsRelative = _Options & RelativeBit,
118 ComputeOrientation = _Options & OrientationBit,
119 ComputePosition = _Options & PositionBit,
120 OutputSE3 = _Options & OutputSE3Bit,
121 IsPosition = ComputePosition && !ComputeOrientation,
122 IsOrientation = !ComputePosition && ComputeOrientation,
123 IsTransform = ComputePosition && ComputeOrientation;
125 ValueSize = (ComputePosition?3:0) + (ComputeOrientation?(OutputSE3?4:3):0),
126 DerSize = (ComputePosition?3:0) + (ComputeOrientation ?3:0);
129 IsRelative = _Options & RelativeBit,
130 ComputeOrientation = _Options & OrientationBit,
131 ComputePosition = _Options & PositionBit,
132 OutputSE3 = _Options & OutputSE3Bit,
133 IsPosition = ComputePosition && !ComputeOrientation,
134 IsOrientation = !ComputePosition && ComputeOrientation,
135 IsTransform = ComputePosition && ComputeOrientation,
136 ValueSize = (ComputePosition?3:0) + (ComputeOrientation?(OutputSE3?4:3):0),
137 DerSize = (ComputePosition?3:0) + (ComputeOrientation ?3:0)
142 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
155 static Ptr_t create (
const std::string& name,
const DevicePtr_t& robot,
157 std::vector <bool> mask = std::vector<bool>(DerSize,
true));
173 static Ptr_t create (
const std::string& name,
const DevicePtr_t& robot,
176 std::vector <bool> mask = std::vector<bool>(DerSize,
true));
190 static Ptr_t create (
const std::string& name,
const DevicePtr_t& robot,
193 std::vector <bool> mask = std::vector<bool>(DerSize,
true));
213 static Ptr_t create (
const std::string& name,
const DevicePtr_t& robot,
216 std::vector <bool> mask = std::vector<bool>(DerSize,
true));
224 m_.F1inJ1 = reference;
225 m_.checkIsIdentity1();
226 m_.F2inJ2.setIdentity ();
227 m_.checkIsIdentity2();
233 return m_.F1inJ1.actInv(m_.F2inJ2);
240 computeActiveParams();
241 assert (!joint || joint->robot () == robot_);
246 return m_.getJoint1();
252 computeActiveParams();
253 assert (!joint || (joint->index() > 0 && joint->robot () == robot_));
264 m_.checkIsIdentity1();
273 m_.checkIsIdentity2();
280 virtual std::ostream& print (std::ostream& o)
const;
291 std::vector <bool> mask);
294 void init (
const WkPtr_t&
self)
297 computeActiveParams();
309 void computeActiveParams ();
311 GenericTransformationModel<IsRelative> m_;
313 const std::vector <bool> mask_;
319 #endif // HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH
Definition: active-set-differentiable-function.hh:24
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:91
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:88
assert(d.lhs()._blocks()==d.rhs()._blocks())
pinocchio::JointConstPtr_t JointConstPtr_t
Definition: fwd.hh:38
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:52
pinocchio::size_type size_type
Definition: fwd.hh:35
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:44
pinocchio::Transform3f Transform3f
Definition: fwd.hh:50