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;
59 template<
typename Vector6Like>
60 JointMotion __mult__(
const Eigen::MatrixBase<Vector6Like> &
vj)
const
63 return JointMotion(
vj);
66 template<
typename S1,
int O1>
69 return m.toActionMatrix();
72 template<
typename S1,
int O1>
75 return m.toActionMatrixInverse();
85 template<
typename Derived>
89 return phi.toVector();
93 template<
typename 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
145 const Eigen::MatrixBase<Matrix6Like> & Y,
const JointMotionSubspaceIdentityTpl<S2, O2> &)
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
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
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
318 typedef typename Eigen::Quaternion<
319 typename ConfigVectorLike::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(
ConfigVectorLike)::Options>
326 assert(math::fabs(
static_cast<Scalar
>(
quat.coeffs().squaredNorm() - 1)) <= 1
e-4);
328 M.rotation(
quat.matrix());
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;
355 template<
typename TangentVector>
356 PINOCCHIO_DONT_INLINE
void
357 calc(JointDataDerived & data,
const Blank,
const typename Eigen::MatrixBase<TangentVector> &
vs)
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());
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,
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());
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.