6 #ifndef __pinocchio_joint_planar_hpp__
7 #define __pinocchio_joint_planar_hpp__
9 #include "pinocchio/macros.hpp"
10 #include "pinocchio/multibody/joint/joint-base.hpp"
11 #include "pinocchio/spatial/cartesian-axis.hpp"
12 #include "pinocchio/multibody/constraint.hpp"
13 #include "pinocchio/spatial/motion.hpp"
14 #include "pinocchio/spatial/inertia.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< MotionPlanarTpl<_Scalar,_Options> >
61 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
68 MotionPlanarTpl(
const Scalar & x_dot,
const Scalar & y_dot,
69 const Scalar & theta_dot)
71 m_data << x_dot, y_dot, theta_dot;
74 template<
typename Vector3Like>
75 MotionPlanarTpl(
const Eigen::MatrixBase<Vector3Like> & vj)
78 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
81 inline PlainReturnType plain()
const
83 return PlainReturnType(
typename PlainReturnType::Vector3(vx(),vy(),Scalar(0)),
84 typename PlainReturnType::Vector3(Scalar(0),Scalar(0),wz()));
87 template<
typename Derived>
88 void addTo(MotionDense<Derived> & other)
const
90 other.linear()[0] += vx();
91 other.linear()[1] += vy();
92 other.angular()[2] += wz();
95 template<
typename MotionDerived>
96 void setTo(MotionDense<MotionDerived> & other)
const
98 other.linear() << vx(), vy(), (Scalar)0;
99 other.angular() << (Scalar)0, (Scalar)0, wz();
102 template<
typename S2,
int O2,
typename D2>
103 void se3Action_impl(
const SE3Tpl<S2,O2> & m, MotionDense<D2> & v)
const
105 v.angular().noalias() = m.rotation().col(2) * wz();
106 v.linear().noalias() = m.rotation().template leftCols<2>() * m_data.template head<2>();
107 v.linear() += m.translation().cross(v.angular());
110 template<
typename S2,
int O2>
111 MotionPlain se3Action_impl(
const SE3Tpl<S2,O2> & m)
const
114 se3Action_impl(m,res);
118 template<
typename S2,
int O2,
typename D2>
119 void se3ActionInverse_impl(
const SE3Tpl<S2,O2> & m, MotionDense<D2> & v)
const
124 AxisZ::alphaCross(wz(),m.translation(),v3_tmp);
125 v3_tmp[0] += vx(); v3_tmp[1] += vy();
126 v.linear().noalias() = m.rotation().transpose() * v3_tmp;
129 v.angular().noalias() = m.rotation().transpose().col(2) * wz();
132 template<
typename S2,
int O2>
133 MotionPlain se3ActionInverse_impl(
const SE3Tpl<S2,O2> & m)
const
136 se3ActionInverse_impl(m,res);
140 template<
typename M1,
typename M2>
141 void motionAction(
const MotionDense<M1> & v, MotionDense<M2> & mout)
const
144 AxisZ::alphaCross(-wz(),v.linear(),mout.linear());
146 typename M1::ConstAngularType w_in = v.angular();
147 typename M2::LinearType v_out = mout.linear();
149 v_out[0] -= w_in[2] * vy();
150 v_out[1] += w_in[2] * vx();
151 v_out[2] += -w_in[1] * vx() + w_in[0] * vy() ;
154 AxisZ::alphaCross(-wz(),v.angular(),mout.angular());
157 template<
typename M1>
158 MotionPlain motionAction(
const MotionDense<M1> & v)
const
165 const Scalar & vx()
const {
return m_data[0]; }
166 Scalar & vx() {
return m_data[0]; }
168 const Scalar & vy()
const {
return m_data[1]; }
169 Scalar & vy() {
return m_data[1]; }
171 const Scalar & wz()
const {
return m_data[2]; }
172 Scalar & wz() {
return m_data[2]; }
174 const Vector3 & data()
const {
return m_data; }
175 Vector3 & data() {
return m_data; }
177 bool isEqual_impl(
const MotionPlanarTpl & other)
const
179 return m_data == other.m_data;
188 template<
typename Scalar,
int Options,
typename MotionDerived>
189 inline typename MotionDerived::MotionPlain
190 operator+(
const MotionPlanarTpl<Scalar,Options> & m1,
const MotionDense<MotionDerived> & m2)
192 typename MotionDerived::MotionPlain result(m2);
193 result.linear()[0] += m1.vx();
194 result.linear()[1] += m1.vy();
196 result.angular()[2] += m1.wz();
203 template<
typename _Scalar,
int _Options>
206 typedef _Scalar Scalar;
207 enum { Options = _Options };
213 typedef Eigen::Matrix<Scalar,3,1,Options> JointForce;
214 typedef Eigen::Matrix<Scalar,6,3,Options> DenseBase;
215 typedef DenseBase MatrixReturnType;
216 typedef const DenseBase ConstMatrixReturnType;
219 template<
typename _Scalar,
int _Options>
223 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
228 ConstraintPlanarTpl() {};
230 template<
typename Vector3Like>
231 JointMotion __mult__(
const Eigen::MatrixBase<Vector3Like> & vj)
const
233 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
234 return JointMotion(vj);
237 int nv_impl()
const {
return NV; }
244 template<
typename Derived>
252 template<
typename Derived>
253 friend typename Eigen::Matrix <typename Eigen::MatrixBase<Derived>::Scalar, 3, Derived::ColsAtCompileTime>
256 typedef typename Eigen::MatrixBase<Derived>::Scalar Scalar;
257 typedef Eigen::Matrix<Scalar, 3, Derived::ColsAtCompileTime> MatrixReturnType;
260 MatrixReturnType result(3, F.cols ());
261 result.template topRows<2>() = F.template topRows<2>();
262 result.template bottomRows<1>() = F.template bottomRows<1>();
270 DenseBase matrix_impl()
const
273 S.template block<3,3>(Inertia::LINEAR, 0).setZero ();
274 S.template block<3,3>(Inertia::ANGULAR, 0).setZero ();
275 S(Inertia::LINEAR + 0,0) = Scalar(1);
276 S(Inertia::LINEAR + 1,1) = Scalar(1);
277 S(Inertia::ANGULAR + 2,2) = Scalar(1);
281 template<
typename S1,
int O1>
282 DenseBase se3Action(
const SE3Tpl<S1,O1> & m)
const
284 DenseBase X_subspace;
287 X_subspace.template block <3,2>(Motion::LINEAR, 0) = m.rotation ().template leftCols <2> ();
288 X_subspace.template block <3,1>(Motion::LINEAR, 2).noalias()
289 = m.translation().cross(m.rotation ().template rightCols<1>());
292 X_subspace.template block <3,2>(Motion::ANGULAR, 0).setZero ();
293 X_subspace.template block <3,1>(Motion::ANGULAR, 2) = m.rotation ().template rightCols<1>();
298 template<
typename S1,
int O1>
299 DenseBase se3ActionInverse(
const SE3Tpl<S1,O1> & m)
const
301 DenseBase X_subspace;
304 X_subspace.template block <3,2>(Motion::LINEAR, 0) = m.rotation().transpose().template leftCols <2>();
305 X_subspace.template block <3,1>(Motion::ANGULAR,2).noalias() = m.rotation().transpose() * m.translation();
306 X_subspace.template block <3,1>(Motion::LINEAR, 2).noalias() = -X_subspace.template block <3,1>(Motion::ANGULAR,2).cross(m.rotation().transpose().template rightCols<1>());
309 X_subspace.template block <3,2>(Motion::ANGULAR, 0).setZero();
310 X_subspace.template block <3,1>(Motion::ANGULAR, 2) = m.rotation().transpose().template rightCols<1>();
315 template<
typename MotionDerived>
316 DenseBase motionAction(
const MotionDense<MotionDerived> & m)
const
318 const typename MotionDerived::ConstLinearType v = m.linear();
319 const typename MotionDerived::ConstAngularType w = m.angular();
320 DenseBase res(DenseBase::Zero());
322 res(0,1) = -w[2]; res(0,2) = v[1];
323 res(1,0) = w[2]; res(1,2) = -v[0];
324 res(2,0) = -w[1]; res(2,1) = w[0];
331 bool isEqual(
const ConstraintPlanarTpl &)
const {
return true; }
335 template<
typename MotionDerived,
typename S2,
int O2>
336 inline typename MotionDerived::MotionPlain
337 operator^(
const MotionDense<MotionDerived> & m1,
const MotionPlanarTpl<S2,O2> & m2)
339 return m2.motionAction(m1);
343 template<
typename S1,
int O1,
typename S2,
int O2>
344 inline typename Eigen::Matrix<S1,6,3,O1>
345 operator*(
const InertiaTpl<S1,O1> & Y,
const ConstraintPlanarTpl<S2,O2> &)
347 typedef InertiaTpl<S1,O1> Inertia;
348 typedef typename Inertia::Scalar Scalar;
349 typedef Eigen::Matrix<S1,6,3,O1> ReturnType;
352 const Scalar mass = Y.mass();
353 const typename Inertia::Vector3 & com = Y.lever();
354 const typename Inertia::Symmetric3 & inertia = Y.inertia();
356 M.template topLeftCorner<3,3>().setZero();
357 M.template topLeftCorner<2,2>().diagonal().fill(mass);
359 const typename Inertia::Vector3 mc(mass * com);
360 M.template rightCols<1>().template head<2>() << -mc(1), mc(0);
362 M.template bottomLeftCorner<3,2>() << (Scalar)0, -mc(2), mc(2), (Scalar)0, -mc(1), mc(0);
363 M.template rightCols<1>().template tail<3>() = inertia.data().template tail<3>();
364 M.template rightCols<1>()[3] -= mc(0)*com(2);
365 M.template rightCols<1>()[4] -= mc(1)*com(2);
366 M.template rightCols<1>()[5] += mass*(com(0)*com(0) + com(1)*com(1));
374 template<
typename M6Like,
typename S2,
int O2>
375 inline Eigen::Matrix<S2,6,3,O2>
376 operator*(
const Eigen::MatrixBase<M6Like> & Y,
377 const ConstraintPlanarTpl<S2,O2> &)
379 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
380 typedef Eigen::Matrix<S2,6,3,O2> Matrix63;
383 IS.template leftCols<2>() = Y.template leftCols<2>();
384 IS.template rightCols<1>() = Y.template rightCols<1>();
389 template<
typename S1,
int O1>
391 {
typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
393 template<
typename S1,
int O1,
typename MotionDerived>
395 {
typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
399 template<
typename _Scalar,
int _Options>
406 enum { Options = _Options };
407 typedef _Scalar Scalar;
416 typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
417 typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
418 typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
420 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
422 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
423 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
426 template<
typename Scalar,
int Options>
428 template<
typename Scalar,
int Options>
431 template<
typename _Scalar,
int _Options>
434 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
436 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
437 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
452 : M(Transformation_t::Identity())
453 , v(Motion_t::Vector3::Zero())
456 , UDinv(UD_t::Zero())
459 static std::string classname() {
return std::string(
"JointDataPlanar"); }
460 std::string shortname()
const {
return classname(); }
464 PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelPlanarTpl);
465 template<
typename _Scalar,
int _Options>
466 struct JointModelPlanarTpl
467 :
public JointModelBase< JointModelPlanarTpl<_Scalar,_Options> >
469 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
470 typedef JointPlanarTpl<_Scalar,_Options> JointDerived;
471 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
473 typedef JointModelBase<JointModelPlanarTpl> Base;
477 using Base::setIndexes;
479 JointDataDerived createData()
const {
return JointDataDerived(); }
481 template<
typename ConfigVector>
482 inline void forwardKinematics(Transformation_t & M,
const Eigen::MatrixBase<ConfigVector> & q_joint)
const
485 & c_theta = q_joint(2),
486 & s_theta = q_joint(3);
488 M.rotation().template topLeftCorner<2,2>() << c_theta, -s_theta, s_theta, c_theta;
489 M.translation().template head<2>() = q_joint.template head<2>();
492 template<
typename ConfigVector>
493 void calc(JointDataDerived & data,
494 const typename Eigen::MatrixBase<ConfigVector> & qs)
const
496 typedef typename ConfigVector::Scalar Scalar;
497 typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type & q = qs.template segment<NQ>(
idx_q());
503 data.M.rotation().template topLeftCorner<2,2>() << c_theta, -s_theta, s_theta, c_theta;
504 data.M.translation().template head<2>() = q.template head<2>();
508 template<
typename ConfigVector,
typename TangentVector>
509 void calc(JointDataDerived & data,
510 const typename Eigen::MatrixBase<ConfigVector> & qs,
511 const typename Eigen::MatrixBase<TangentVector> & vs)
const
513 calc(data,qs.derived());
515 typename TangentVector::template ConstFixedSegmentReturnType<NV>::Type & q_dot = vs.template segment<NV>(
idx_v ());
517 data.v.vx() = q_dot(0);
518 data.v.vy() = q_dot(1);
519 data.v.wz() = q_dot(2);
522 template<
typename Matrix6Like>
523 void calc_aba(JointDataDerived & data,
524 const Eigen::MatrixBase<Matrix6Like> & I,
525 const bool update_I)
const
527 data.U.template leftCols<2>() = I.template leftCols<2>();
528 data.U.template rightCols<1>() = I.template rightCols<1>();
530 data.StU.template leftCols<2>() = data.U.template topRows<2>().transpose();
531 data.StU.template rightCols<1>() = data.U.template bottomRows<1>();
536 internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv);
538 data.UDinv.noalias() = data.U * data.Dinv;
541 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).noalias() -= data.UDinv * data.U.transpose();
544 static std::string classname() {
return std::string(
"JointModelPlanar");}
545 std::string shortname()
const {
return classname(); }
548 template<
typename NewScalar>
561 #include <boost/type_traits.hpp>
565 template<
typename Scalar,
int Options>
567 :
public integral_constant<bool,true> {};
569 template<
typename Scalar,
int Options>
571 :
public integral_constant<bool,true> {};
573 template<
typename Scalar,
int Options>
575 :
public integral_constant<bool,true> {};
577 template<
typename Scalar,
int Options>
579 :
public integral_constant<bool,true> {};
582 #endif // ifndef __pinocchio_joint_planar_hpp__