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>
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)
.