31 #ifndef HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH 32 #define HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH 38 #include <hpp/pinocchio/joint.hh> 39 #include <hpp/util/serialization-fwd.hh> 40 #include <pinocchio/spatial/se3.hpp> 43 namespace constraints {
46 struct GenericTransformationModel {
48 bool R1isID, R2isID, t1isZero, t2isZero;
50 bool fullPos, fullOri;
56 GenericTransformationModel(
const size_type nCols)
68 void checkIsIdentity1() {
69 R1isID = F1inJ1.rotation().isIdentity();
70 t1isZero = F1inJ1.translation().isZero();
72 void checkIsIdentity2() {
73 R2isID = F2inJ2.rotation().isIdentity();
74 t2isZero = F2inJ2.translation().isZero();
78 struct GenericTransformationModel<true> : GenericTransformationModel<false> {
82 GenericTransformationModel(
const size_type nCols)
83 : GenericTransformationModel<false>(nCols), joint1() {}
133 template <
int _Options>
135 :
public DifferentiableFunction {
137 typedef shared_ptr<GenericTransformation>
Ptr_t;
138 typedef weak_ptr<GenericTransformation>
WkPtr_t;
139 #if __cplusplus >= 201103L 140 static constexpr
bool IsRelative = _Options & RelativeBit,
141 ComputeOrientation = _Options & OrientationBit,
142 ComputePosition = _Options & PositionBit,
143 OutputR3xSO3 = _Options & OutputR3xSO3Bit,
144 IsPosition = ComputePosition && !ComputeOrientation,
145 IsOrientation = !ComputePosition && ComputeOrientation,
146 IsTransform = ComputePosition && ComputeOrientation;
147 static constexpr
int ValueSize =
148 (ComputePosition ? 3 : 0) +
149 (ComputeOrientation ? (OutputR3xSO3 ? 4 : 3) : 0),
150 DerSize = (ComputePosition ? 3 : 0) +
151 (ComputeOrientation ? 3 : 0);
154 IsRelative = _Options & RelativeBit,
155 ComputeOrientation = _Options & OrientationBit,
156 ComputePosition = _Options & PositionBit,
157 OutputR3xSO3 = _Options & OutputR3xSO3Bit,
158 IsPosition = ComputePosition && !ComputeOrientation,
159 IsOrientation = !ComputePosition && ComputeOrientation,
160 IsTransform = ComputePosition && ComputeOrientation,
161 ValueSize = (ComputePosition ? 3 : 0) +
162 (ComputeOrientation ? (OutputR3xSO3 ? 4 : 3) : 0),
163 DerSize = (ComputePosition ? 3 : 0) + (ComputeOrientation ? 3 : 0)
168 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
181 static Ptr_t create(
const std::string& name,
const DevicePtr_t& robot,
184 std::vector<bool> mask = std::vector<bool>(DerSize,
201 static Ptr_t create(
const std::string& name,
const DevicePtr_t& robot,
204 std::vector<bool> mask = std::vector<bool>(DerSize,
219 static Ptr_t create(
const std::string& name,
const DevicePtr_t& robot,
223 std::vector<bool> mask = std::vector<bool>(DerSize,
244 static Ptr_t create(
const std::string& name,
const DevicePtr_t& robot,
248 std::vector<bool> mask = std::vector<bool>(DerSize,
256 m_.F1inJ1 = reference;
257 m_.checkIsIdentity1();
258 m_.F2inJ2.setIdentity();
259 m_.checkIsIdentity2();
269 computeActiveParams();
270 assert(!joint || joint->robot() == robot_);
279 computeActiveParams();
280 assert(!joint || (joint->index() > 0 && joint->robot() == robot_));
289 m_.checkIsIdentity1();
296 m_.checkIsIdentity2();
301 virtual std::ostream& print(std::ostream& o)
const;
311 std::vector<bool> mask);
314 void init(
const WkPtr_t&
self) {
316 computeActiveParams();
331 if (robot_ != castother.robot_)
return false;
332 if (mask_ != castother.mask_)
return false;
338 void computeActiveParams();
340 GenericTransformationModel<IsRelative> m_;
342 const std::vector<bool> mask_;
351 #endif // HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH
Definition: active-set-differentiable-function.hh:36
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:108
virtual bool isEqual(const DifferentiableFunction &other) const
Definition: differentiable-function.hh:190
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:105
assert(d.lhs()._blocks()==d.rhs()._blocks())
pinocchio::JointConstPtr_t JointConstPtr_t
Definition: fwd.hh:50
Definition: differentiable-function.hh:63
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:65
pinocchio::size_type size_type
Definition: fwd.hh:47
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:57
pinocchio::Transform3f Transform3f
Definition: fwd.hh:63