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
69 const Scalar & theta_dot)
71 m_data << x_dot, y_dot, theta_dot;
74 template<
typename Vector3Like>
78 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
83 return PlainReturnType(
typename PlainReturnType::Vector3(vx(),vy(),Scalar(0)),
84 typename PlainReturnType::Vector3(Scalar(0),Scalar(0),wz()));
87 template<
typename Derived>
90 other.linear()[0] += vx();
91 other.linear()[1] += vy();
92 other.angular()[2] += wz();
95 template<
typename MotionDerived>
98 other.linear() << vx(), vy(), (Scalar)0;
99 other.angular() << (Scalar)0, (Scalar)0, wz();
102 template<
typename S2,
int O2,
typename D2>
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>
114 se3Action_impl(m,res);
118 template<
typename S2,
int O2,
typename D2>
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>
136 se3ActionInverse_impl(m,res);
140 template<
typename M1,
typename M2>
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>
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; }
179 return m_data == other.m_data;
188 template<
typename Scalar,
int Options,
typename MotionDerived>
189 inline typename MotionDerived::MotionPlain
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
230 template<
typename Vector3Like>
231 JointMotion __mult__(
const Eigen::MatrixBase<Vector3Like> & vj)
const 233 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
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>
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>
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>
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];
335 template<
typename MotionDerived,
typename S2,
int O2>
336 inline typename MotionDerived::MotionPlain
339 return m2.motionAction(m1);
343 template<
typename S1,
int O1,
typename S2,
int O2>
344 inline typename Eigen::Matrix<S1,6,3,O1>
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,
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(); }
465 template<
typename _Scalar,
int _Options>
469 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
471 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
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__
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 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.
JointModelPlanarTpl< NewScalar, Options > cast() const
ConstAngularType angular() const
Return the angular part of the force vector.
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
void cross(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3xIn > &Min, const Eigen::MatrixBase< Matrix3xOut > &Mout)
Applies the cross product onto the columns of M.
Main pinocchio namespace.
Common traits structure to fully define base classes for CRTP.
ConstLinearType linear() const
Return the linear part of the force vector.
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)
.