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,4,4,Options> Matrix4;
42 typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
43 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
44 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
45 typedef Vector3 AngularType;
46 typedef Vector3 LinearType;
47 typedef const Vector3 ConstAngularType;
48 typedef const Vector3 ConstLinearType;
49 typedef Matrix6 ActionMatrixType;
50 typedef Matrix4 HomogeneousMatrixType;
59 template<
typename _Scalar,
int _Options>
61 :
MotionBase< MotionPlanarTpl<_Scalar,_Options> >
63 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
70 MotionPlanarTpl(
const Scalar & x_dot,
const Scalar & y_dot,
71 const Scalar & theta_dot)
73 m_data << x_dot, y_dot, theta_dot;
76 template<
typename Vector3Like>
77 MotionPlanarTpl(
const Eigen::MatrixBase<Vector3Like> & vj)
80 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
83 inline PlainReturnType plain()
const
85 return PlainReturnType(
typename PlainReturnType::Vector3(vx(),vy(),Scalar(0)),
86 typename PlainReturnType::Vector3(Scalar(0),Scalar(0),wz()));
89 template<
typename Derived>
90 void addTo(MotionDense<Derived> & other)
const
92 other.linear()[0] += vx();
93 other.linear()[1] += vy();
94 other.angular()[2] += wz();
97 template<
typename MotionDerived>
98 void setTo(MotionDense<MotionDerived> & other)
const
100 other.linear() << vx(), vy(), (Scalar)0;
101 other.angular() << (Scalar)0, (Scalar)0, wz();
104 template<
typename S2,
int O2,
typename D2>
105 void se3Action_impl(
const SE3Tpl<S2,O2> & m, MotionDense<D2> & v)
const
107 v.angular().noalias() = m.rotation().col(2) * wz();
108 v.linear().noalias() = m.rotation().template leftCols<2>() * m_data.template head<2>();
109 v.linear() += m.translation().cross(v.angular());
112 template<
typename S2,
int O2>
113 MotionPlain se3Action_impl(
const SE3Tpl<S2,O2> & m)
const
116 se3Action_impl(m,res);
120 template<
typename S2,
int O2,
typename D2>
121 void se3ActionInverse_impl(
const SE3Tpl<S2,O2> & m, MotionDense<D2> & v)
const
126 AxisZ::alphaCross(wz(),m.translation(),v3_tmp);
127 v3_tmp[0] += vx(); v3_tmp[1] += vy();
128 v.linear().noalias() = m.rotation().transpose() * v3_tmp;
131 v.angular().noalias() = m.rotation().transpose().col(2) * wz();
134 template<
typename S2,
int O2>
135 MotionPlain se3ActionInverse_impl(
const SE3Tpl<S2,O2> & m)
const
138 se3ActionInverse_impl(m,res);
142 template<
typename M1,
typename M2>
143 void motionAction(
const MotionDense<M1> & v, MotionDense<M2> & mout)
const
146 AxisZ::alphaCross(-wz(),v.linear(),mout.linear());
148 typename M1::ConstAngularType w_in = v.angular();
149 typename M2::LinearType v_out = mout.linear();
151 v_out[0] -= w_in[2] * vy();
152 v_out[1] += w_in[2] * vx();
153 v_out[2] += -w_in[1] * vx() + w_in[0] * vy() ;
156 AxisZ::alphaCross(-wz(),v.angular(),mout.angular());
159 template<
typename M1>
160 MotionPlain motionAction(
const MotionDense<M1> & v)
const
167 const Scalar & vx()
const {
return m_data[0]; }
168 Scalar & vx() {
return m_data[0]; }
170 const Scalar & vy()
const {
return m_data[1]; }
171 Scalar & vy() {
return m_data[1]; }
173 const Scalar & wz()
const {
return m_data[2]; }
174 Scalar & wz() {
return m_data[2]; }
176 const Vector3 & data()
const {
return m_data; }
177 Vector3 & data() {
return m_data; }
179 bool isEqual_impl(
const MotionPlanarTpl & other)
const
181 return m_data == other.m_data;
190 template<
typename Scalar,
int Options,
typename MotionDerived>
191 inline typename MotionDerived::MotionPlain
192 operator+(
const MotionPlanarTpl<Scalar,Options> & m1,
const MotionDense<MotionDerived> & m2)
194 typename MotionDerived::MotionPlain result(m2);
195 result.linear()[0] += m1.vx();
196 result.linear()[1] += m1.vy();
198 result.angular()[2] += m1.wz();
205 template<
typename _Scalar,
int _Options>
208 typedef _Scalar Scalar;
209 enum { Options = _Options };
215 typedef Eigen::Matrix<Scalar,3,1,Options> JointForce;
216 typedef Eigen::Matrix<Scalar,6,3,Options> DenseBase;
217 typedef DenseBase MatrixReturnType;
218 typedef const DenseBase ConstMatrixReturnType;
221 template<
typename _Scalar,
int _Options>
225 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
230 ConstraintPlanarTpl() {};
232 template<
typename Vector3Like>
233 JointMotion __mult__(
const Eigen::MatrixBase<Vector3Like> & vj)
const
235 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
236 return JointMotion(vj);
239 int nv_impl()
const {
return NV; }
246 template<
typename Derived>
254 template<
typename Derived>
255 friend typename Eigen::Matrix <typename Eigen::MatrixBase<Derived>::Scalar, 3, Derived::ColsAtCompileTime>
258 typedef typename Eigen::MatrixBase<Derived>::Scalar Scalar;
259 typedef Eigen::Matrix<Scalar, 3, Derived::ColsAtCompileTime> MatrixReturnType;
262 MatrixReturnType result(3, F.cols ());
263 result.template topRows<2>() = F.template topRows<2>();
264 result.template bottomRows<1>() = F.template bottomRows<1>();
272 DenseBase matrix_impl()
const
275 S.template block<3,3>(Inertia::LINEAR, 0).setZero ();
276 S.template block<3,3>(Inertia::ANGULAR, 0).setZero ();
277 S(Inertia::LINEAR + 0,0) = Scalar(1);
278 S(Inertia::LINEAR + 1,1) = Scalar(1);
279 S(Inertia::ANGULAR + 2,2) = Scalar(1);
283 template<
typename S1,
int O1>
284 DenseBase se3Action(
const SE3Tpl<S1,O1> & m)
const
286 DenseBase X_subspace;
289 X_subspace.template block <3,2>(Motion::LINEAR, 0) = m.rotation ().template leftCols <2> ();
290 X_subspace.template block <3,1>(Motion::LINEAR, 2).noalias()
291 = m.translation().cross(m.rotation ().template rightCols<1>());
294 X_subspace.template block <3,2>(Motion::ANGULAR, 0).setZero ();
295 X_subspace.template block <3,1>(Motion::ANGULAR, 2) = m.rotation ().template rightCols<1>();
300 template<
typename S1,
int O1>
301 DenseBase se3ActionInverse(
const SE3Tpl<S1,O1> & m)
const
303 DenseBase X_subspace;
306 X_subspace.template block <3,2>(Motion::LINEAR, 0) = m.rotation().transpose().template leftCols <2>();
307 X_subspace.template block <3,1>(Motion::ANGULAR,2).noalias() = m.rotation().transpose() * m.translation();
308 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>());
311 X_subspace.template block <3,2>(Motion::ANGULAR, 0).setZero();
312 X_subspace.template block <3,1>(Motion::ANGULAR, 2) = m.rotation().transpose().template rightCols<1>();
317 template<
typename MotionDerived>
318 DenseBase motionAction(
const MotionDense<MotionDerived> & m)
const
320 const typename MotionDerived::ConstLinearType v = m.linear();
321 const typename MotionDerived::ConstAngularType w = m.angular();
322 DenseBase res(DenseBase::Zero());
324 res(0,1) = -w[2]; res(0,2) = v[1];
325 res(1,0) = w[2]; res(1,2) = -v[0];
326 res(2,0) = -w[1]; res(2,1) = w[0];
333 bool isEqual(
const ConstraintPlanarTpl &)
const {
return true; }
337 template<
typename MotionDerived,
typename S2,
int O2>
338 inline typename MotionDerived::MotionPlain
339 operator^(
const MotionDense<MotionDerived> & m1,
const MotionPlanarTpl<S2,O2> & m2)
341 return m2.motionAction(m1);
345 template<
typename S1,
int O1,
typename S2,
int O2>
346 inline typename Eigen::Matrix<S1,6,3,O1>
347 operator*(
const InertiaTpl<S1,O1> & Y,
const ConstraintPlanarTpl<S2,O2> &)
349 typedef InertiaTpl<S1,O1> Inertia;
350 typedef typename Inertia::Scalar Scalar;
351 typedef Eigen::Matrix<S1,6,3,O1> ReturnType;
354 const Scalar mass = Y.mass();
355 const typename Inertia::Vector3 & com = Y.lever();
356 const typename Inertia::Symmetric3 & inertia = Y.inertia();
358 M.template topLeftCorner<3,3>().setZero();
359 M.template topLeftCorner<2,2>().diagonal().fill(mass);
361 const typename Inertia::Vector3 mc(mass * com);
362 M.template rightCols<1>().template head<2>() << -mc(1), mc(0);
364 M.template bottomLeftCorner<3,2>() << (Scalar)0, -mc(2), mc(2), (Scalar)0, -mc(1), mc(0);
365 M.template rightCols<1>().template tail<3>() = inertia.data().template tail<3>();
366 M.template rightCols<1>()[3] -= mc(0)*com(2);
367 M.template rightCols<1>()[4] -= mc(1)*com(2);
368 M.template rightCols<1>()[5] += mass*(com(0)*com(0) + com(1)*com(1));
376 template<
typename M6Like,
typename S2,
int O2>
377 inline Eigen::Matrix<S2,6,3,O2>
378 operator*(
const Eigen::MatrixBase<M6Like> & Y,
379 const ConstraintPlanarTpl<S2,O2> &)
381 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
382 typedef Eigen::Matrix<S2,6,3,O2> Matrix63;
385 IS.template leftCols<2>() = Y.template leftCols<2>();
386 IS.template rightCols<1>() = Y.template rightCols<1>();
391 template<
typename S1,
int O1>
393 {
typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
395 template<
typename S1,
int O1,
typename MotionDerived>
397 {
typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
401 template<
typename _Scalar,
int _Options>
408 enum { Options = _Options };
409 typedef _Scalar Scalar;
418 typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
419 typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
420 typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
422 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
424 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
425 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
428 template<
typename Scalar,
int Options>
430 template<
typename Scalar,
int Options>
433 template<
typename _Scalar,
int _Options>
436 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
438 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
439 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
454 : M(Transformation_t::Identity())
455 , v(Motion_t::Vector3::Zero())
458 , UDinv(UD_t::Zero())
461 static std::string classname() {
return std::string(
"JointDataPlanar"); }
462 std::string shortname()
const {
return classname(); }
466 PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelPlanarTpl);
467 template<
typename _Scalar,
int _Options>
468 struct JointModelPlanarTpl
469 :
public JointModelBase< JointModelPlanarTpl<_Scalar,_Options> >
471 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
472 typedef JointPlanarTpl<_Scalar,_Options> JointDerived;
473 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
475 typedef JointModelBase<JointModelPlanarTpl> Base;
479 using Base::setIndexes;
481 JointDataDerived createData()
const {
return JointDataDerived(); }
483 const std::vector<bool> hasConfigurationLimit()
const
485 return {
true,
true,
false,
false};
488 const std::vector<bool> hasConfigurationLimitInTangent()
const
490 return {
true,
true,
false};
493 template<
typename ConfigVector>
494 inline void forwardKinematics(Transformation_t & M,
const Eigen::MatrixBase<ConfigVector> & q_joint)
const
497 & c_theta = q_joint(2),
498 & s_theta = q_joint(3);
500 M.rotation().template topLeftCorner<2,2>() << c_theta, -s_theta, s_theta, c_theta;
501 M.translation().template head<2>() = q_joint.template head<2>();
504 template<
typename ConfigVector>
505 void calc(JointDataDerived & data,
506 const typename Eigen::MatrixBase<ConfigVector> & qs)
const
508 typedef typename ConfigVector::Scalar Scalar;
509 typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type & q = qs.template segment<NQ>(
idx_q());
515 data.M.rotation().template topLeftCorner<2,2>() << c_theta, -s_theta, s_theta, c_theta;
516 data.M.translation().template head<2>() = q.template head<2>();
520 template<
typename ConfigVector,
typename TangentVector>
521 void calc(JointDataDerived & data,
522 const typename Eigen::MatrixBase<ConfigVector> & qs,
523 const typename Eigen::MatrixBase<TangentVector> & vs)
const
525 calc(data,qs.derived());
527 typename TangentVector::template ConstFixedSegmentReturnType<NV>::Type & q_dot = vs.template segment<NV>(
idx_v ());
529 data.v.vx() = q_dot(0);
530 data.v.vy() = q_dot(1);
531 data.v.wz() = q_dot(2);
534 template<
typename Matrix6Like>
535 void calc_aba(JointDataDerived & data,
536 const Eigen::MatrixBase<Matrix6Like> & I,
537 const bool update_I)
const
539 data.U.template leftCols<2>() = I.template leftCols<2>();
540 data.U.template rightCols<1>() = I.template rightCols<1>();
542 data.StU.template leftCols<2>() = data.U.template topRows<2>().transpose();
543 data.StU.template rightCols<1>() = data.U.template bottomRows<1>();
548 internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv);
550 data.UDinv.noalias() = data.U * data.Dinv;
553 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).noalias() -= data.UDinv * data.U.transpose();
556 static std::string classname() {
return std::string(
"JointModelPlanar");}
557 std::string shortname()
const {
return classname(); }
560 template<
typename NewScalar>
573 #include <boost/type_traits.hpp>
577 template<
typename Scalar,
int Options>
579 :
public integral_constant<bool,true> {};
581 template<
typename Scalar,
int Options>
583 :
public integral_constant<bool,true> {};
585 template<
typename Scalar,
int Options>
587 :
public integral_constant<bool,true> {};
589 template<
typename Scalar,
int Options>
591 :
public integral_constant<bool,true> {};
594 #endif // ifndef __pinocchio_joint_planar_hpp__