6 #ifndef __pinocchio_joint_prismatic_unaligned_hpp__
7 #define __pinocchio_joint_prismatic_unaligned_hpp__
9 #include "pinocchio/macros.hpp"
10 #include "pinocchio/multibody/joint/joint-base.hpp"
11 #include "pinocchio/multibody/joint/joint-translation.hpp"
12 #include "pinocchio/multibody/constraint.hpp"
13 #include "pinocchio/spatial/inertia.hpp"
14 #include "pinocchio/math/matrix.hpp"
22 template<
typename Scalar,
int Options>
28 template<
typename Scalar,
int Options,
typename MotionDerived>
34 template<
typename _Scalar,
int _Options>
37 typedef _Scalar Scalar;
38 enum { Options = _Options };
39 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
40 typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
41 typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
42 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
43 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
44 typedef Vector3 AngularType;
45 typedef Vector3 LinearType;
46 typedef const Vector3 ConstAngularType;
47 typedef const Vector3 ConstLinearType;
48 typedef Matrix6 ActionMatrixType;
57 template<
typename _Scalar,
int _Options>
59 :
MotionBase < MotionPrismaticUnalignedTpl<_Scalar,_Options> >
61 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
66 template<
typename Vector3Like,
typename S2>
67 MotionPrismaticUnalignedTpl(
const Eigen::MatrixBase<Vector3Like> & axis,
69 : m_axis(axis), m_v(v)
70 { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }
72 inline PlainReturnType plain()
const
74 return PlainReturnType(m_axis*m_v,
75 PlainReturnType::Vector3::Zero());
78 template<
typename OtherScalar>
79 MotionPrismaticUnalignedTpl __mult__(
const OtherScalar & alpha)
const
81 return MotionPrismaticUnalignedTpl(m_axis,alpha*m_v);
84 template<
typename Derived>
85 void addTo(MotionDense<Derived> & other)
const
87 other.linear() += m_axis * m_v;
90 template<
typename Derived>
91 void setTo(MotionDense<Derived> & other)
const
93 other.linear().noalias() = m_axis*m_v;
94 other.angular().setZero();
97 template<
typename S2,
int O2,
typename D2>
98 void se3Action_impl(
const SE3Tpl<S2,O2> & m, MotionDense<D2> & v)
const
100 v.linear().noalias() = m_v * (m.rotation() * m_axis);
101 v.angular().setZero();
104 template<
typename S2,
int O2>
105 MotionPlain se3Action_impl(
const SE3Tpl<S2,O2> & m)
const
108 se3Action_impl(m,res);
112 template<
typename S2,
int O2,
typename D2>
113 void se3ActionInverse_impl(
const SE3Tpl<S2,O2> & m, MotionDense<D2> & v)
const
116 v.linear().noalias() = m_v * (m.rotation().transpose() * m_axis);
119 v.angular().setZero();
122 template<
typename S2,
int O2>
123 MotionPlain se3ActionInverse_impl(
const SE3Tpl<S2,O2> & m)
const
126 se3ActionInverse_impl(m,res);
130 template<
typename M1,
typename M2>
131 void motionAction(
const MotionDense<M1> & v, MotionDense<M2> & mout)
const
134 mout.linear().noalias() = v.angular().cross(m_axis);
135 mout.linear() *= m_v;
138 mout.angular().setZero();
141 template<
typename M1>
142 MotionPlain motionAction(
const MotionDense<M1> & v)
const
149 bool isEqual_impl(
const MotionPrismaticUnalignedTpl & other)
const
151 return m_axis == other.m_axis && m_v == other.m_v;
154 const Scalar & linearRate()
const {
return m_v; }
155 Scalar & linearRate() {
return m_v; }
157 const Vector3 & axis()
const {
return m_axis; }
158 Vector3 & axis() {
return m_axis; }
166 template<
typename Scalar,
int Options,
typename MotionDerived>
167 inline typename MotionDerived::MotionPlain
168 operator+(
const MotionPrismaticUnalignedTpl<Scalar,Options> & m1,
const MotionDense<MotionDerived> & m2)
170 typedef typename MotionDerived::MotionPlain ReturnType;
171 return ReturnType(m1.linearRate() * m1.axis() + m2.linear(), m2.angular());
174 template<
typename MotionDerived,
typename S2,
int O2>
175 inline typename MotionDerived::MotionPlain
176 operator^(
const MotionDense<MotionDerived> & m1,
177 const MotionPrismaticUnalignedTpl<S2,O2> & m2)
179 return m2.motionAction(m1);
184 template<
typename _Scalar,
int _Options>
187 typedef _Scalar Scalar;
188 enum { Options = _Options };
194 typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
195 typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
196 typedef DenseBase MatrixReturnType;
197 typedef const DenseBase ConstMatrixReturnType;
199 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
202 template<
typename Scalar,
int Options>
204 {
typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
206 template<
typename Scalar,
int Options,
typename MotionDerived>
208 {
typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
210 template<
typename Scalar,
int Options,
typename ForceDerived>
217 template<
typename Scalar,
int Options,
typename ForceSet>
222 typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type
226 template<
typename _Scalar,
int _Options>
228 :
ConstraintBase< ConstraintPrismaticUnalignedTpl<_Scalar,_Options> >
230 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
235 typedef typename traits<ConstraintPrismaticUnalignedTpl>::Vector3 Vector3;
237 ConstraintPrismaticUnalignedTpl() {}
239 template<
typename Vector3Like>
240 ConstraintPrismaticUnalignedTpl(
const Eigen::MatrixBase<Vector3Like> & axis)
242 { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }
244 template<
typename Vector1Like>
245 JointMotion __mult__(
const Eigen::MatrixBase<Vector1Like> & v)
const
247 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
248 return JointMotion(m_axis,v[0]);
251 template<
typename S1,
int O1>
252 typename SE3GroupAction<ConstraintPrismaticUnalignedTpl>::ReturnType
253 se3Action(
const SE3Tpl<S1,O1> & m)
const
255 typename SE3GroupAction<ConstraintPrismaticUnalignedTpl>::ReturnType res;
256 MotionRef<DenseBase> v(res);
257 v.linear().noalias() = m.rotation()*m_axis;
258 v.angular().setZero();
262 template<
typename S1,
int O1>
263 typename SE3GroupAction<ConstraintPrismaticUnalignedTpl>::ReturnType
264 se3ActionInverse(
const SE3Tpl<S1,O1> & m)
const
266 typename SE3GroupAction<ConstraintPrismaticUnalignedTpl>::ReturnType res;
267 MotionRef<DenseBase> v(res);
268 v.linear().noalias() = m.rotation().transpose()*m_axis;
269 v.angular().setZero();
273 int nv_impl()
const {
return NV; }
280 template<
typename ForceDerived>
281 typename ConstraintForceOp<ConstraintPrismaticUnalignedTpl,ForceDerived>::ReturnType
284 typedef typename ConstraintForceOp<ConstraintPrismaticUnalignedTpl,ForceDerived>::ReturnType ReturnType;
286 res[0] = ref.axis().dot(f.
linear());
291 template<
typename ForceSet>
292 typename ConstraintForceSetOp<ConstraintPrismaticUnalignedTpl,ForceSet>::ReturnType
293 operator*(
const Eigen::MatrixBase<ForceSet> & F)
295 EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
297 return ref.axis().transpose() * F.template middleRows<3>(LINEAR);
310 DenseBase matrix_impl()
const
313 S.template segment<3>(LINEAR) = m_axis;
314 S.template segment<3>(ANGULAR).setZero();
318 template<
typename MotionDerived>
319 DenseBase motionAction(
const MotionDense<MotionDerived> & v)
const
322 res.template segment<3>(LINEAR).noalias() = v.angular().cross(m_axis);
323 res.template segment<3>(ANGULAR).setZero();
328 const Vector3 & axis()
const {
return m_axis; }
329 Vector3 & axis() {
return m_axis; }
331 bool isEqual(
const ConstraintPrismaticUnalignedTpl & other)
const
333 return m_axis == other.m_axis;
342 template<
typename S1,
int O1,
typename S2,
int O2>
345 typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
351 template<
typename S1,
int O1,
typename S2,
int O2>
358 static inline ReturnType run(
const Inertia & Y,
363 const S1 & m = Y.mass();
364 const typename Inertia::Vector3 & c = Y.lever();
366 res.template segment<3>(Constraint::LINEAR).noalias() = m*cpu.axis();
367 res.template segment<3>(Constraint::ANGULAR).noalias() = c.cross(res.template segment<3>(Constraint::LINEAR));
374 template<
typename M6Like,
typename Scalar,
int Options>
378 typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst;
381 typedef typename Constraint::Vector3 Vector3;
388 template<
typename M6Like,
typename Scalar,
int Options>
393 static inline ReturnType run(
const Eigen::MatrixBase<M6Like> & Y,
396 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
397 return Y.derived().template middleCols<3>(Constraint::LINEAR) * cru.axis();
404 template<
typename _Scalar,
int _Options>
411 typedef _Scalar Scalar;
412 enum { Options = _Options };
421 typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
422 typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
423 typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
425 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
427 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
428 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
431 template<
typename Scalar,
int Options>
435 template<
typename _Scalar,
int _Options>
437 :
public JointDataBase< JointDataPrismaticUnalignedTpl<_Scalar,_Options> >
439 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
441 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
442 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
455 : M(Transformation_t::Vector3::Zero())
456 , S(Constraint_t::Vector3::Zero())
457 , v(Constraint_t::Vector3::Zero(),(Scalar)0)
460 , UDinv(UD_t::Zero())
463 template<
typename Vector3Like>
464 JointDataPrismaticUnalignedTpl(
const Eigen::MatrixBase<Vector3Like> & axis)
467 , v(axis,(Scalar)NAN)
468 , U(), Dinv(), UDinv()
471 static std::string classname() {
return std::string(
"JointDataPrismaticUnaligned"); }
472 std::string shortname()
const {
return classname(); }
476 template<
typename Scalar,
int Options>
481 template<
typename _Scalar,
int _Options>
483 :
public JointModelBase< JointModelPrismaticUnalignedTpl<_Scalar,_Options> >
485 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
487 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
493 using Base::setIndexes;
495 typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;
498 JointModelPrismaticUnalignedTpl(
const Scalar & x,
504 assert(
isUnitary(
axis) &&
"Translation axis is not unitary");
507 template<
typename Vector3Like>
508 JointModelPrismaticUnalignedTpl(
const Eigen::MatrixBase<Vector3Like> &
axis)
511 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
512 assert(
isUnitary(
axis) &&
"Translation axis is not unitary");
515 JointDataDerived createData()
const {
return JointDataDerived(
axis); }
518 bool isEqual(
const JointModelPrismaticUnalignedTpl & other)
const
520 return Base::isEqual(other) &&
axis == other.axis;
523 template<
typename ConfigVector>
524 void calc(JointDataDerived & data,
525 const typename Eigen::MatrixBase<ConfigVector> & qs)
const
527 typedef typename ConfigVector::Scalar Scalar;
528 const Scalar & q = qs[
idx_q()];
530 data.M.translation().noalias() =
axis * q;
533 template<
typename ConfigVector,
typename TangentVector>
534 void calc(JointDataDerived & data,
535 const typename Eigen::MatrixBase<ConfigVector> & qs,
536 const typename Eigen::MatrixBase<TangentVector> & vs)
const
538 calc(data,qs.derived());
540 typedef typename TangentVector::Scalar S2;
541 const S2 & v = vs[
idx_v()];
542 data.v.linearRate() = v;
545 template<
typename Matrix6Like>
546 void calc_aba(JointDataDerived & data,
const Eigen::MatrixBase<Matrix6Like> & I,
const bool update_I)
const
548 data.U.noalias() = I.template block<6,3> (0,Inertia::LINEAR) *
axis;
549 data.Dinv[0] = Scalar(1)/
axis.dot(data.U.template segment <3> (Inertia::LINEAR));
550 data.UDinv.noalias() = data.U * data.Dinv;
553 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
556 static std::string classname() {
return std::string(
"JointModelPrismaticUnaligned"); }
557 std::string shortname()
const {
return classname(); }
560 template<
typename NewScalar>
564 ReturnType res(
axis.template cast<NewScalar>());
579 #include <boost/type_traits.hpp>
583 template<
typename Scalar,
int Options>
585 :
public integral_constant<bool,true> {};
587 template<
typename Scalar,
int Options>
589 :
public integral_constant<bool,true> {};
591 template<
typename Scalar,
int Options>
593 :
public integral_constant<bool,true> {};
595 template<
typename Scalar,
int Options>
597 :
public integral_constant<bool,true> {};
601 #endif // ifndef __pinocchio_joint_prismatic_unaligned_hpp__