6 #ifndef __pinocchio_joint_prismatic_hpp__ 7 #define __pinocchio_joint_prismatic_hpp__ 9 #include "pinocchio/macros.hpp" 10 #include "pinocchio/multibody/joint/joint-base.hpp" 11 #include "pinocchio/multibody/constraint.hpp" 12 #include "pinocchio/spatial/inertia.hpp" 13 #include "pinocchio/spatial/spatial-axis.hpp" 14 #include "pinocchio/utils/axis-label.hpp" 21 template<
typename Scalar,
int Options,
int axis>
27 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
33 template<
typename _Scalar,
int _Options,
int _axis>
36 typedef _Scalar Scalar;
37 enum { Options = _Options };
38 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
39 typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
40 typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
41 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
42 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
43 typedef Vector3 AngularType;
44 typedef Vector3 LinearType;
45 typedef const Vector3 ConstAngularType;
46 typedef const Vector3 ConstLinearType;
47 typedef Matrix6 ActionMatrixType;
56 template<
typename _Scalar,
int _Options,
int _axis>
58 :
MotionBase < MotionPrismaticTpl<_Scalar,_Options,_axis> >
60 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63 enum { axis = _axis };
66 typedef typename Axis::CartesianAxis3 CartesianAxis3;
73 template<
typename OtherScalar>
79 template<
typename Derived>
83 other.linear()[_axis] += (OtherScalar) m_v;
86 template<
typename MotionDerived>
89 for(Eigen::DenseIndex k = 0; k < 3; ++k)
90 other.linear()[k] = k == axis ? m_v : (Scalar)0;
91 other.angular().setZero();
94 template<
typename S2,
int O2,
typename D2>
97 v.angular().setZero();
98 v.linear().noalias() = m_v * (m.rotation().col(axis));
101 template<
typename S2,
int O2>
105 se3Action_impl(m,res);
109 template<
typename S2,
int O2,
typename D2>
113 v.linear().noalias() = m_v * (m.rotation().transpose().col(axis));
116 v.angular().setZero();
119 template<
typename S2,
int O2>
123 se3ActionInverse_impl(m,res);
127 template<
typename M1,
typename M2>
131 CartesianAxis3::alphaCross(-m_v,v.angular(),mout.linear());
134 mout.angular().setZero();
137 template<
typename M1>
145 Scalar & linearRate() {
return m_v; }
146 const Scalar & linearRate()
const {
return m_v; }
150 return m_v == other.m_v;
158 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
159 typename MotionDerived::MotionPlain
163 typename MotionDerived::MotionPlain res(m2);
168 template<
typename MotionDerived,
typename S2,
int O2,
int axis>
170 typename MotionDerived::MotionPlain
173 return m2.motionAction(m1);
178 template<
typename _Scalar,
int _Options,
int _axis>
187 typedef _Scalar Scalar;
189 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
190 typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
191 typedef typename Matrix3::IdentityReturnType AngularType;
192 typedef AngularType AngularRef;
193 typedef AngularType ConstAngularRef;
194 typedef Vector3 LinearType;
195 typedef const Vector3 LinearRef;
196 typedef const Vector3 ConstLinearRef;
201 template<
typename Scalar,
int Options,
int axis>
205 template<
typename _Scalar,
int _Options,
int axis>
208 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
213 typedef typename Axis::CartesianAxis3 CartesianAxis3;
217 : m_displacement(displacement)
220 PlainType plain()
const 222 PlainType res(PlainType::Identity());
223 res.rotation().setIdentity();
224 res.translation()[axis] = m_displacement;
229 operator PlainType()
const {
return plain(); }
231 template<
typename S2,
int O2>
237 res.translation()[axis] += m_displacement;
242 const Scalar & displacement()
const {
return m_displacement; }
243 Scalar & displacement() {
return m_displacement; }
245 ConstLinearRef translation()
const {
return CartesianAxis3()*displacement(); };
246 AngularType rotation()
const {
return AngularType(3,3); }
250 return m_displacement == other.m_displacement;
255 Scalar m_displacement;
260 template<
typename _Scalar,
int _Options,
int axis>
263 typedef _Scalar Scalar;
264 enum { Options = _Options };
270 typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
271 typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
272 typedef DenseBase MatrixReturnType;
273 typedef const DenseBase ConstMatrixReturnType;
276 template<
typename Scalar,
int Options,
int axis>
278 {
typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
280 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
282 {
typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
284 template<
typename Scalar,
int Options,
int axis,
typename ForceDerived>
288 template<
typename Scalar,
int Options,
int axis,
typename ForceSet>
290 {
typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; };
292 template<
typename _Scalar,
int _Options,
int axis>
294 :
ConstraintBase < ConstraintPrismaticTpl <_Scalar,_Options,axis> >
296 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
304 template<
typename Vector1Like>
305 JointMotion __mult__(
const Eigen::MatrixBase<Vector1Like> & v)
const 307 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
308 assert(v.size() == 1);
309 return JointMotion(v[0]);
312 template<
typename S2,
int O2>
318 v.linear() = m.rotation().col(axis);
319 v.angular().setZero();
323 template<
typename S2,
int O2>
329 v.linear() = m.rotation().transpose().col(axis);
330 v.angular().setZero();
334 int nv_impl()
const {
return NV; }
341 template<
typename ForceDerived>
342 typename ConstraintForceOp<ConstraintPrismaticTpl,ForceDerived>::ReturnType
344 {
return f.
linear().template segment<1>(axis); }
347 template<
typename Derived>
348 typename ConstraintForceSetOp<ConstraintPrismaticTpl,Derived>::ReturnType
349 operator*(
const Eigen::MatrixBase<Derived> & F )
352 return F.row(LINEAR+axis);
364 DenseBase matrix_impl()
const 372 template<
typename MotionDerived>
386 template<
typename S1,
int O1,
typename S2,
int O2,
int axis>
389 typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
395 template<
typename S1,
int O1,
typename S2,
int O2>
401 static inline ReturnType run(
const Inertia & Y,
411 res << m, S1(0), S1(0), S1(0), m*z, -m*y;
417 template<
typename S1,
int O1,
typename S2,
int O2>
423 static inline ReturnType run(
const Inertia & Y,
434 res << S1(0), m, S1(0), -m*z, S1(0), m*x;
440 template<
typename S1,
int O1,
typename S2,
int O2>
446 static inline ReturnType run(
const Inertia & Y,
457 res << S1(0), S1(0), m, m*y, -m*x, S1(0);
464 template<
typename M6Like,
typename S2,
int O2,
int axis>
467 typedef typename M6Like::ConstColXpr ReturnType;
473 template<
typename M6Like,
typename Scalar,
int Options,
int axis>
478 static inline ReturnType run(
const Eigen::MatrixBase<M6Like> & Y,
481 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
482 return Y.derived().col(Inertia::LINEAR + axis);
487 template<
typename _Scalar,
int _Options,
int _axis>
490 typedef _Scalar Scalar;
499 template<
typename _Scalar,
int _Options,
int axis>
506 typedef _Scalar Scalar;
507 enum { Options = _Options };
516 typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
517 typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
518 typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
520 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
522 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
523 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
526 template<
typename Scalar,
int Options,
int axis>
530 template<
typename Scalar,
int Options,
int axis>
534 template<
typename _Scalar,
int _Options,
int axis>
537 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
539 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
540 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
557 , UDinv(UD_t::Zero())
560 static std::string classname()
562 return std::string(
"JointDataP") + axisLabel<axis>();
564 std::string
shortname()
const {
return classname(); }
568 template<
typename NewScalar,
typename Scalar,
int Options,
int axis>
574 template<
typename _Scalar,
int _Options,
int axis>
576 :
public JointModelBase< JointModelPrismaticTpl<_Scalar,_Options,axis> >
578 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
580 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
586 using Base::setIndexes;
588 JointDataDerived
createData()
const {
return JointDataDerived(); }
590 template<
typename ConfigVector>
591 void calc(JointDataDerived & data,
592 const typename Eigen::MatrixBase<ConfigVector> & qs)
const 594 typedef typename ConfigVector::Scalar Scalar;
595 const Scalar & q = qs[
idx_q()];
596 data.M.displacement() = q;
599 template<
typename ConfigVector,
typename TangentVector>
600 void calc(JointDataDerived & data,
601 const typename Eigen::MatrixBase<ConfigVector> & qs,
602 const typename Eigen::MatrixBase<TangentVector> & vs)
const 604 calc(data,qs.derived());
606 typedef typename TangentVector::Scalar S2;
607 const S2 & v = vs[
idx_v()];
608 data.v.linearRate() = v;
611 template<
typename Matrix6Like>
612 void calc_aba(JointDataDerived & data,
const Eigen::MatrixBase<Matrix6Like> & I,
const bool update_I)
const 614 data.U = I.col(Inertia::LINEAR + axis);
615 data.Dinv[0] = 1./I(Inertia::LINEAR + axis, Inertia::LINEAR + axis);
616 data.UDinv.noalias() = data.U * data.Dinv[0];
619 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
622 static std::string classname()
624 return std::string(
"JointModelP") + axisLabel<axis>();
626 std::string
shortname()
const {
return classname(); }
629 template<
typename NewScalar>
654 #include <boost/type_traits.hpp> 658 template<
typename Scalar,
int Options,
int axis>
660 :
public integral_constant<bool,true> {};
662 template<
typename Scalar,
int Options,
int axis>
664 :
public integral_constant<bool,true> {};
666 template<
typename Scalar,
int Options,
int axis>
668 :
public integral_constant<bool,true> {};
670 template<
typename Scalar,
int Options,
int axis>
672 :
public integral_constant<bool,true> {};
675 #endif // ifndef __pinocchio_joint_prismatic_hpp__
Forward declaration of the multiplication operation return type. Should be overloaded, otherwise it will procude a compilation error.
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
int idx_v(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxVVisitor to get the index in the full model tangent space corre...
Return type of the Constraint::Transpose * ForceSet operation.
Return type of the ation of a Motion onto an object of type D.
JointDataTpl< Scalar, Options, JointCollectionTpl > createData(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through CreateData visitor to create a JointDataTpl.
Source from #include <cppad/example/cppad_eigen.hpp>
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
JointModelPrismaticTpl< NewScalar, Options, axis > cast() const
Main pinocchio namespace.
Common traits structure to fully define base classes for CRTP.
ConstLinearType linear() const
Return the linear part of the force vector.
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
Return type of the Constraint::Transpose * Force operation.
void calc_aba(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata, const Eigen::MatrixBase< Matrix6Type > &I, const bool update_I)
Visit a JointModelTpl and the corresponding JointDataTpl through JointCalcAbaVisitor to...
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)