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>
JointModelPrismaticTpl< NewScalar, Options, axis > cast() const
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
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)
.