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) rate;
86 template<
typename MotionDerived>
89 for(Eigen::DenseIndex k = 0; k < 3; ++k)
90 other.linear()[k] = k == axis ? rate : (Scalar)0;
91 other.angular().setZero();
94 template<
typename S2,
int O2,
typename D2>
97 v.angular().setZero();
98 v.linear().noalias() = rate * (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() = rate * (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(-rate,v.angular(),mout.linear());
134 mout.angular().setZero();
137 template<
typename M1>
149 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
150 typename MotionDerived::MotionPlain
154 typename MotionDerived::MotionPlain res(m2);
159 template<
typename MotionDerived,
typename S2,
int O2,
int axis>
161 typename MotionDerived::MotionPlain
164 return m2.motionAction(m1);
169 template<
typename _Scalar,
int _Options,
int _axis>
178 typedef _Scalar Scalar;
180 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
181 typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
182 typedef typename Matrix3::IdentityReturnType AngularType;
183 typedef AngularType AngularRef;
184 typedef AngularType ConstAngularRef;
185 typedef Vector3 LinearType;
186 typedef const Vector3 LinearRef;
187 typedef const Vector3 ConstLinearRef;
192 template<
typename Scalar,
int Options,
int axis>
196 template<
typename _Scalar,
int _Options,
int axis>
199 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
204 typedef typename Axis::CartesianAxis3 CartesianAxis3;
208 : m_displacement(displacement)
211 PlainType plain()
const 213 PlainType res(PlainType::Identity());
214 res.rotation().setIdentity();
215 res.translation()[axis] = m_displacement;
220 operator PlainType()
const {
return plain(); }
222 template<
typename S2,
int O2>
228 res.translation()[axis] += m_displacement;
233 const Scalar & displacement()
const {
return m_displacement; }
234 Scalar & displacement() {
return m_displacement; }
236 ConstLinearRef translation()
const {
return CartesianAxis3()*displacement(); };
237 AngularType rotation()
const {
return AngularType(3,3); }
241 Scalar m_displacement;
246 template<
typename _Scalar,
int _Options,
int axis>
249 typedef _Scalar Scalar;
250 enum { Options = _Options };
256 typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
257 typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
258 typedef DenseBase MatrixReturnType;
259 typedef const DenseBase ConstMatrixReturnType;
262 template<
typename Scalar,
int Options,
int axis>
264 {
typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
266 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
268 {
typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
270 template<
typename Scalar,
int Options,
int axis,
typename ForceDerived>
274 template<
typename Scalar,
int Options,
int axis,
typename ForceSet>
276 {
typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; };
278 template<
typename _Scalar,
int _Options,
int axis>
280 :
ConstraintBase < ConstraintPrismaticTpl <_Scalar,_Options,axis> >
282 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
290 template<
typename Vector1Like>
291 JointMotion __mult__(
const Eigen::MatrixBase<Vector1Like> & v)
const 293 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
294 assert(v.size() == 1);
295 return JointMotion(v[0]);
298 template<
typename S2,
int O2>
304 v.linear() = m.rotation().col(axis);
305 v.angular().setZero();
309 template<
typename S2,
int O2>
315 v.linear() = m.rotation().transpose().col(axis);
316 v.angular().setZero();
320 int nv_impl()
const {
return NV; }
327 template<
typename ForceDerived>
328 typename ConstraintForceOp<ConstraintPrismaticTpl,ForceDerived>::ReturnType
330 {
return f.
linear().template segment<1>(axis); }
333 template<
typename Derived>
334 typename ConstraintForceSetOp<ConstraintPrismaticTpl,Derived>::ReturnType
335 operator*(
const Eigen::MatrixBase<Derived> & F )
338 return F.row(LINEAR+axis);
350 DenseBase matrix_impl()
const 358 template<
typename MotionDerived>
370 template<
typename S1,
int O1,
typename S2,
int O2,
int axis>
373 typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
379 template<
typename S1,
int O1,
typename S2,
int O2>
385 static inline ReturnType run(
const Inertia & Y,
395 res << m, S1(0), S1(0), S1(0), m*z, -m*y;
401 template<
typename S1,
int O1,
typename S2,
int O2>
407 static inline ReturnType run(
const Inertia & Y,
418 res << S1(0), m, S1(0), -m*z, S1(0), m*x;
424 template<
typename S1,
int O1,
typename S2,
int O2>
430 static inline ReturnType run(
const Inertia & Y,
441 res << S1(0), S1(0), m, m*y, -m*x, S1(0);
448 template<
typename M6Like,
typename S2,
int O2,
int axis>
451 typedef typename M6Like::ConstColXpr ReturnType;
457 template<
typename M6Like,
typename Scalar,
int Options,
int axis>
462 static inline ReturnType run(
const Eigen::MatrixBase<M6Like> & Y,
465 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
466 return Y.derived().col(Inertia::LINEAR + axis);
471 template<
typename _Scalar,
int _Options,
int _axis>
474 typedef _Scalar Scalar;
483 template<
typename _Scalar,
int _Options,
int axis>
490 typedef _Scalar Scalar;
491 enum { Options = _Options };
500 typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
501 typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
502 typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
504 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
506 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
507 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
510 template<
typename Scalar,
int Options,
int axis>
514 template<
typename Scalar,
int Options,
int axis>
518 template<
typename _Scalar,
int _Options,
int axis>
521 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
523 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
524 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
538 static std::string classname() {
return std::string(
"JointDataPrismatic"); }
539 std::string
shortname()
const {
return classname(); }
543 template<
typename NewScalar,
typename Scalar,
int Options,
int axis>
549 template<
typename _Scalar,
int _Options,
int axis>
551 :
public JointModelBase< JointModelPrismaticTpl<_Scalar,_Options,axis> >
553 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
555 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
561 using Base::setIndexes;
563 JointDataDerived
createData()
const {
return JointDataDerived(); }
565 template<
typename ConfigVector>
566 void calc(JointDataDerived & data,
567 const typename Eigen::MatrixBase<ConfigVector> & qs)
const 569 typedef typename ConfigVector::Scalar Scalar;
570 const Scalar & q = qs[
idx_q()];
571 data.M.displacement() = q;
574 template<
typename ConfigVector,
typename TangentVector>
575 void calc(JointDataDerived & data,
576 const typename Eigen::MatrixBase<ConfigVector> & qs,
577 const typename Eigen::MatrixBase<TangentVector> & vs)
const 579 calc(data,qs.derived());
581 typedef typename TangentVector::Scalar S2;
582 const S2 & v = vs[
idx_v()];
586 template<
typename Matrix6Like>
587 void calc_aba(JointDataDerived & data,
const Eigen::MatrixBase<Matrix6Like> & I,
const bool update_I)
const 589 data.U = I.col(Inertia::LINEAR + axis);
590 data.Dinv[0] = 1./I(Inertia::LINEAR + axis, Inertia::LINEAR + axis);
591 data.UDinv.noalias() = data.U * data.Dinv[0];
594 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
597 static std::string classname()
599 return std::string(
"JointModelP") + axisLabel<axis>();
601 std::string
shortname()
const {
return classname(); }
604 template<
typename NewScalar>
629 #include <boost/type_traits.hpp> 633 template<
typename Scalar,
int Options,
int axis>
635 :
public integral_constant<bool,true> {};
637 template<
typename Scalar,
int Options,
int axis>
639 :
public integral_constant<bool,true> {};
641 template<
typename Scalar,
int Options,
int axis>
643 :
public integral_constant<bool,true> {};
645 template<
typename Scalar,
int Options,
int axis>
647 :
public integral_constant<bool,true> {};
650 #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)
.