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 };
65 typedef SpatialAxis<_axis+LINEAR> Axis;
66 typedef typename Axis::CartesianAxis3 CartesianAxis3;
68 MotionPrismaticTpl() {}
69 MotionPrismaticTpl(
const Scalar & v) : m_v(v) {}
71 inline PlainReturnType plain()
const {
return Axis() * m_v; }
73 template<
typename OtherScalar>
74 MotionPrismaticTpl __mult__(
const OtherScalar & alpha)
const
76 return MotionPrismaticTpl(alpha*m_v);
79 template<
typename Derived>
80 void addTo(MotionDense<Derived> & other)
const
82 typedef typename MotionDense<Derived>::Scalar OtherScalar;
83 other.linear()[_axis] += (OtherScalar) m_v;
86 template<
typename MotionDerived>
87 void setTo(MotionDense<MotionDerived> & other)
const
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>
95 void se3Action_impl(
const SE3Tpl<S2,O2> & m, MotionDense<D2> & v)
const
97 v.angular().setZero();
98 v.linear().noalias() = m_v * (m.rotation().col(axis));
101 template<
typename S2,
int O2>
102 MotionPlain se3Action_impl(
const SE3Tpl<S2,O2> & m)
const
105 se3Action_impl(m,res);
109 template<
typename S2,
int O2,
typename D2>
110 void se3ActionInverse_impl(
const SE3Tpl<S2,O2> & m, MotionDense<D2> & v)
const
113 v.linear().noalias() = m_v * (m.rotation().transpose().col(axis));
116 v.angular().setZero();
119 template<
typename S2,
int O2>
120 MotionPlain se3ActionInverse_impl(
const SE3Tpl<S2,O2> & m)
const
123 se3ActionInverse_impl(m,res);
127 template<
typename M1,
typename M2>
128 void motionAction(
const MotionDense<M1> & v, MotionDense<M2> & mout)
const
131 CartesianAxis3::alphaCross(-m_v,v.angular(),mout.linear());
134 mout.angular().setZero();
137 template<
typename M1>
138 MotionPlain motionAction(
const MotionDense<M1> & v)
const
145 Scalar & linearRate() {
return m_v; }
146 const Scalar & linearRate()
const {
return m_v; }
148 bool isEqual_impl(
const MotionPrismaticTpl & other)
const
150 return m_v == other.m_v;
158 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
159 typename MotionDerived::MotionPlain
160 operator+(
const MotionPrismaticTpl<Scalar,Options,axis> & m1,
161 const MotionDense<MotionDerived> & m2)
163 typename MotionDerived::MotionPlain res(m2);
168 template<
typename MotionDerived,
typename S2,
int O2,
int axis>
170 typename MotionDerived::MotionPlain
171 operator^(
const MotionDense<MotionDerived> & m1,
const MotionPrismaticTpl<S2,O2,axis> & m2)
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
212 typedef typename Axis::CartesianAxis3 CartesianAxis3;
215 TransformPrismaticTpl(
const Scalar & displacement)
216 : m_displacement(displacement)
219 PlainType plain()
const
221 PlainType res(PlainType::Identity());
222 res.rotation().setIdentity();
223 res.translation()[axis] = m_displacement;
228 operator PlainType()
const {
return plain(); }
230 template<
typename S2,
int O2>
231 typename SE3GroupAction<TransformPrismaticTpl>::ReturnType
232 se3action(
const SE3Tpl<S2,O2> & m)
const
234 typedef typename SE3GroupAction<TransformPrismaticTpl>::ReturnType ReturnType;
236 res.translation()[axis] += m_displacement;
241 const Scalar & displacement()
const {
return m_displacement; }
242 Scalar & displacement() {
return m_displacement; }
244 ConstLinearRef translation()
const {
return CartesianAxis3()*displacement(); };
245 AngularType rotation()
const {
return AngularType(3,3); }
247 bool isEqual(
const TransformPrismaticTpl & other)
const
249 return m_displacement == other.m_displacement;
254 Scalar m_displacement;
259 template<
typename _Scalar,
int _Options,
int axis>
262 typedef _Scalar Scalar;
263 enum { Options = _Options };
269 typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
270 typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
271 typedef DenseBase MatrixReturnType;
272 typedef const DenseBase ConstMatrixReturnType;
275 template<
typename Scalar,
int Options,
int axis>
277 {
typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
279 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
281 {
typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
283 template<
typename Scalar,
int Options,
int axis,
typename ForceDerived>
287 template<
typename Scalar,
int Options,
int axis,
typename ForceSet>
289 {
typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; };
291 template<
typename _Scalar,
int _Options,
int axis>
293 :
ConstraintBase < ConstraintPrismaticTpl <_Scalar,_Options,axis> >
295 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
299 typedef SpatialAxis<LINEAR+axis> Axis;
301 ConstraintPrismaticTpl() {};
303 template<
typename Vector1Like>
304 JointMotion __mult__(
const Eigen::MatrixBase<Vector1Like> & v)
const
306 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
307 assert(v.size() == 1);
308 return JointMotion(v[0]);
311 template<
typename S2,
int O2>
312 typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType
313 se3Action(
const SE3Tpl<S2,O2> & m)
const
315 typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType res;
316 MotionRef<DenseBase> v(res);
317 v.linear() = m.rotation().col(axis);
318 v.angular().setZero();
322 template<
typename S2,
int O2>
323 typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType
324 se3ActionInverse(
const SE3Tpl<S2,O2> & m)
const
326 typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType res;
327 MotionRef<DenseBase> v(res);
328 v.linear() = m.rotation().transpose().col(axis);
329 v.angular().setZero();
333 int nv_impl()
const {
return NV; }
340 template<
typename ForceDerived>
341 typename ConstraintForceOp<ConstraintPrismaticTpl,ForceDerived>::ReturnType
343 {
return f.
linear().template segment<1>(axis); }
346 template<
typename Derived>
347 typename ConstraintForceSetOp<ConstraintPrismaticTpl,Derived>::ReturnType
348 operator*(
const Eigen::MatrixBase<Derived> & F )
351 return F.row(LINEAR+axis);
363 DenseBase matrix_impl()
const
371 template<
typename MotionDerived>
372 typename MotionAlgebraAction<ConstraintPrismaticTpl,MotionDerived>::ReturnType
373 motionAction(
const MotionDense<MotionDerived> & m)
const
375 typename MotionAlgebraAction<ConstraintPrismaticTpl,MotionDerived>::ReturnType res;
376 MotionRef<DenseBase> v(res);
381 bool isEqual(
const ConstraintPrismaticTpl &)
const {
return true; }
385 template<
typename S1,
int O1,
typename S2,
int O2,
int axis>
388 typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
394 template<
typename S1,
int O1,
typename S2,
int O2>
400 static inline ReturnType run(
const Inertia & Y,
410 res << m, S1(0), S1(0), S1(0), m*z, -m*y;
416 template<
typename S1,
int O1,
typename S2,
int O2>
422 static inline ReturnType run(
const Inertia & Y,
433 res << S1(0), m, S1(0), -m*z, S1(0), m*x;
439 template<
typename S1,
int O1,
typename S2,
int O2>
445 static inline ReturnType run(
const Inertia & Y,
456 res << S1(0), S1(0), m, m*y, -m*x, S1(0);
463 template<
typename M6Like,
typename S2,
int O2,
int axis>
466 typedef typename M6Like::ConstColXpr ReturnType;
472 template<
typename M6Like,
typename Scalar,
int Options,
int axis>
477 static inline ReturnType run(
const Eigen::MatrixBase<M6Like> & Y,
480 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
481 return Y.derived().col(Inertia::LINEAR + axis);
486 template<
typename _Scalar,
int _Options,
int _axis>
489 typedef _Scalar Scalar;
498 template<
typename _Scalar,
int _Options,
int axis>
505 typedef _Scalar Scalar;
506 enum { Options = _Options };
515 typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
516 typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
517 typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
519 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
521 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
522 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
525 template<
typename Scalar,
int Options,
int axis>
529 template<
typename Scalar,
int Options,
int axis>
533 template<
typename _Scalar,
int _Options,
int axis>
536 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
538 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
539 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
556 , UDinv(UD_t::Zero())
559 static std::string classname()
561 return std::string(
"JointDataP") + axisLabel<axis>();
563 std::string shortname()
const {
return classname(); }
567 template<
typename NewScalar,
typename Scalar,
int Options,
int axis>
573 template<
typename _Scalar,
int _Options,
int axis>
575 :
public JointModelBase< JointModelPrismaticTpl<_Scalar,_Options,axis> >
577 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
579 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
585 using Base::setIndexes;
587 JointDataDerived createData()
const {
return JointDataDerived(); }
589 template<
typename ConfigVector>
590 void calc(JointDataDerived & data,
591 const typename Eigen::MatrixBase<ConfigVector> & qs)
const
593 typedef typename ConfigVector::Scalar Scalar;
594 const Scalar & q = qs[
idx_q()];
595 data.M.displacement() = q;
598 template<
typename ConfigVector,
typename TangentVector>
599 void calc(JointDataDerived & data,
600 const typename Eigen::MatrixBase<ConfigVector> & qs,
601 const typename Eigen::MatrixBase<TangentVector> & vs)
const
603 calc(data,qs.derived());
605 typedef typename TangentVector::Scalar S2;
606 const S2 & v = vs[
idx_v()];
607 data.v.linearRate() = v;
610 template<
typename Matrix6Like>
611 void calc_aba(JointDataDerived & data,
const Eigen::MatrixBase<Matrix6Like> & I,
const bool update_I)
const
613 data.U = I.col(Inertia::LINEAR + axis);
614 data.Dinv[0] = Scalar(1)/I(Inertia::LINEAR + axis, Inertia::LINEAR + axis);
615 data.UDinv.noalias() = data.U * data.Dinv[0];
618 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
621 static std::string classname()
623 return std::string(
"JointModelP") + axisLabel<axis>();
625 std::string shortname()
const {
return classname(); }
628 template<
typename NewScalar>
639 typedef JointPrismaticTpl<double,0,0> JointPX;
640 typedef JointDataPrismaticTpl<double,0,0> JointDataPX;
641 typedef JointModelPrismaticTpl<double,0,0> JointModelPX;
643 typedef JointPrismaticTpl<double,0,1> JointPY;
644 typedef JointDataPrismaticTpl<double,0,1> JointDataPY;
645 typedef JointModelPrismaticTpl<double,0,1> JointModelPY;
647 typedef JointPrismaticTpl<double,0,2> JointPZ;
648 typedef JointDataPrismaticTpl<double,0,2> JointDataPZ;
649 typedef JointModelPrismaticTpl<double,0,2> JointModelPZ;
653 #include <boost/type_traits.hpp>
657 template<
typename Scalar,
int Options,
int axis>
659 :
public integral_constant<bool,true> {};
661 template<
typename Scalar,
int Options,
int axis>
663 :
public integral_constant<bool,true> {};
665 template<
typename Scalar,
int Options,
int axis>
667 :
public integral_constant<bool,true> {};
669 template<
typename Scalar,
int Options,
int axis>
671 :
public integral_constant<bool,true> {};
674 #endif // ifndef __pinocchio_joint_prismatic_hpp__