6 #ifndef __pinocchio_multibody_joint_free_flyer_hpp__
7 #define __pinocchio_multibody_joint_free_flyer_hpp__
9 #include "pinocchio/macros.hpp"
10 #include "pinocchio/spatial/inertia.hpp"
11 #include "pinocchio/spatial/explog.hpp"
12 #include "pinocchio/multibody/joint/joint-base.hpp"
13 #include "pinocchio/multibody/joint-motion-subspace.hpp"
14 #include "pinocchio/math/fwd.hpp"
15 #include "pinocchio/math/quaternion.hpp"
20 template<
typename Scalar,
int Options>
21 struct JointMotionSubspaceIdentityTpl;
23 template<
typename _Scalar,
int _Options>
26 typedef _Scalar Scalar;
31 typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
38 typedef Eigen::Matrix<Scalar, 6, 1, Options> JointForce;
39 typedef Eigen::Matrix<Scalar, 6, 6, Options> DenseBase;
40 typedef Eigen::Matrix<Scalar, 6, 6, Options> ReducedSquaredMatrix;
42 typedef typename Matrix6::IdentityReturnType ConstMatrixReturnType;
43 typedef typename Matrix6::IdentityReturnType MatrixReturnType;
44 typedef typename Matrix6::IdentityReturnType StDiagonalMatrixSOperationReturnType;
47 template<
typename _Scalar,
int _Options>
51 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
59 template<
typename Vector6Like>
60 JointMotion __mult__(
const Eigen::MatrixBase<Vector6Like> & vj)
const
62 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like, 6);
63 return JointMotion(vj);
66 template<
typename S1,
int O1>
72 template<
typename S1,
int O1>
85 template<
typename Derived>
93 template<
typename MatrixDerived>
94 typename PINOCCHIO_EIGEN_REF_CONST_TYPE(MatrixDerived)
95 operator*(
const Eigen::MatrixBase<MatrixDerived> & F)
105 MatrixReturnType matrix_impl()
const
107 return DenseBase::Identity();
110 template<
typename MotionDerived>
111 typename MotionDerived::ActionMatrixType motionAction(
const MotionBase<MotionDerived> & v)
const
113 return v.toActionMatrix();
116 bool isEqual(
const JointMotionSubspaceIdentityTpl &)
const
123 template<
typename Scalar,
int Options,
typename Vector6Like>
124 MotionRef<const Vector6Like> operator*(
125 const JointMotionSubspaceIdentityTpl<Scalar, Options> &,
126 const Eigen::MatrixBase<Vector6Like> & v)
128 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like, 6);
130 typedef MotionRef<const Vector6Like> Motion;
131 return Motion(v.derived());
135 template<
typename S1,
int O1,
typename S2,
int O2>
136 inline typename InertiaTpl<S1, O1>::Matrix6
137 operator*(
const InertiaTpl<S1, O1> & Y,
const JointMotionSubspaceIdentityTpl<S2, O2> &)
143 template<
typename Matrix6Like,
typename S2,
int O2>
144 inline typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(
145 const Eigen::MatrixBase<Matrix6Like> & Y,
const JointMotionSubspaceIdentityTpl<S2, O2> &)
150 template<
typename S1,
int O1>
156 template<
typename S1,
int O1,
typename MotionDerived>
162 template<
typename Scalar,
int Options>
165 template<
typename _Scalar,
int _Options>
174 typedef _Scalar Scalar;
187 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
188 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
189 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
191 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
192 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
194 typedef boost::mpl::false_ is_mimicable_t;
196 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
199 template<
typename _Scalar,
int _Options>
203 typedef _Scalar Scalar;
206 template<
typename _Scalar,
int _Options>
210 typedef _Scalar Scalar;
213 template<
typename _Scalar,
int _Options>
216 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
219 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
221 ConfigVector_t joint_q;
222 TangentVector_t joint_v;
236 :
joint_q(ConfigVector_t::Zero())
237 ,
joint_v(TangentVector_t::Zero())
238 , M(Transformation_t::Identity())
239 , v(Motion_t::Zero())
242 , UDinv(UD_t::Identity())
248 static std::string classname()
250 return std::string(
"JointDataFreeFlyer");
252 std::string shortname()
const
284 template<
typename _Scalar,
int _Options>
287 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
295 using Base::idx_vExtended;
296 using Base::setIndexes;
298 JointDataDerived createData()
const
300 return JointDataDerived();
303 const std::vector<bool> hasConfigurationLimit()
const
305 return {
true,
true,
true,
false,
false,
false,
false};
308 const std::vector<bool> hasConfigurationLimitInTangent()
const
310 return {
true,
true,
true,
false,
false,
false};
313 template<
typename ConfigVectorLike>
314 inline void forwardKinematics(
315 Transformation_t & M,
const Eigen::MatrixBase<ConfigVectorLike> & q_joint)
const
317 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t, ConfigVectorLike);
318 typedef typename Eigen::Quaternion<
319 typename ConfigVectorLike::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options>
321 typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
323 ConstQuaternionMap quat(q_joint.template tail<4>().data());
326 assert(math::fabs(
static_cast<Scalar
>(quat.coeffs().squaredNorm() - 1)) <= 1e-4);
328 M.rotation(quat.matrix());
329 M.translation(q_joint.template head<3>());
332 template<
typename Vector3Derived,
typename QuaternionDerived>
333 PINOCCHIO_DONT_INLINE
void calc(
334 JointDataDerived & data,
335 const typename Eigen::MatrixBase<Vector3Derived> & trans,
336 const typename Eigen::QuaternionBase<QuaternionDerived> & quat)
const
338 data.M.translation(trans);
339 data.M.rotation(quat.matrix());
342 template<
typename ConfigVector>
343 PINOCCHIO_DONT_INLINE
void
344 calc(JointDataDerived & data,
const typename Eigen::MatrixBase<ConfigVector> & qs)
const
346 typedef typename Eigen::Quaternion<Scalar, Options> Quaternion;
347 typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
349 data.joint_q = qs.template segment<NQ>(idx_q());
350 ConstQuaternionMap quat(data.joint_q.template tail<4>().data());
352 calc(data, data.joint_q.template head<3>(), quat);
355 template<
typename TangentVector>
356 PINOCCHIO_DONT_INLINE
void
357 calc(JointDataDerived & data,
const Blank,
const typename Eigen::MatrixBase<TangentVector> & vs)
360 data.joint_v = vs.template segment<NV>(idx_v());
361 data.v = data.joint_v;
364 template<
typename ConfigVector,
typename TangentVector>
365 PINOCCHIO_DONT_INLINE
void calc(
366 JointDataDerived & data,
367 const typename Eigen::MatrixBase<ConfigVector> & qs,
368 const typename Eigen::MatrixBase<TangentVector> & vs)
const
370 calc(data, qs.derived());
372 data.joint_v = vs.template segment<NV>(idx_v());
373 data.v = data.joint_v;
376 template<
typename VectorLike,
typename Matrix6Like>
378 JointDataDerived & data,
379 const Eigen::MatrixBase<VectorLike> & armature,
380 const Eigen::MatrixBase<Matrix6Like> & I,
381 const bool update_I)
const
385 data.StU.diagonal() += armature;
387 internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv);
388 data.UDinv.noalias() = I * data.Dinv;
391 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
394 static std::string classname()
396 return std::string(
"JointModelFreeFlyer");
398 std::string shortname()
const
404 template<
typename NewScalar>
409 res.setIndexes(
id(), idx_q(), idx_v(), idx_vExtended());
417 #include <boost/type_traits.hpp>
421 template<
typename Scalar,
int Options>
423 :
public integral_constant<bool, true>
427 template<
typename Scalar,
int Options>
429 :
public integral_constant<bool, true>
433 template<
typename Scalar,
int Options>
435 :
public integral_constant<bool, true>
439 template<
typename Scalar,
int Options>
441 :
public integral_constant<bool, true>
ToVectorConstReturnType toVector() const
Return the force as an Eigen vector.
Main pinocchio namespace.
JointDataTpl< Scalar, Options, JointCollectionTpl >::TangentVector_t joint_v(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConfigVisitor to get the joint velocity vector.
JointDataTpl< Scalar, Options, JointCollectionTpl >::ConfigVector_t joint_q(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConfigVisitor to get the joint configuration vector.
JointModelFreeFlyerTpl< NewScalar, Options > cast() const
Return type of the ation of a Motion onto an object of type D.
ActionMatrixType toActionMatrixInverse() const
The action matrix of .
ActionMatrixType toActionMatrix() const
The action matrix of .
Common traits structure to fully define base classes for CRTP.