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)
70 : m_x_dot(x_dot), m_y_dot(y_dot), m_theta_dot(theta_dot)
73 template<
typename Vector3Like>
75 : m_x_dot(vj[0]), m_y_dot(vj[1]), m_theta_dot(vj[2])
77 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
82 return PlainReturnType(
typename PlainReturnType::Vector3(m_x_dot,m_y_dot,Scalar(0)),
83 typename PlainReturnType::Vector3(Scalar(0),Scalar(0),m_theta_dot));
86 template<
typename Derived>
89 other.linear()[0] += m_x_dot;
90 other.linear()[1] += m_y_dot;
91 other.angular()[2] += m_theta_dot;
94 template<
typename MotionDerived>
97 other.linear() << m_x_dot, m_y_dot, (Scalar)0;
98 other.angular() << (Scalar)0, (Scalar)0, m_theta_dot;
101 template<
typename S2,
int O2,
typename D2>
104 v.angular().noalias() = m.rotation().col(2) * m_theta_dot;
105 v.linear().noalias() = m.translation().cross(v.angular());
106 v.linear() += m.rotation().col(0) * m_x_dot;
107 v.linear() += m.rotation().col(1) * m_y_dot;
110 template<
typename S2,
int O2>
114 se3Action_impl(m,res);
118 template<
typename S2,
int O2,
typename D2>
124 AxisZ::alphaCross(m_theta_dot,m.translation(),v3_tmp);
125 v3_tmp[0] += m_x_dot; v3_tmp[1] += m_y_dot;
126 v.linear().noalias() = m.rotation().transpose() * v3_tmp;
129 v.angular().noalias() = m.rotation().transpose().col(2) * m_theta_dot;
132 template<
typename S2,
int O2>
136 se3ActionInverse_impl(m,res);
140 template<
typename M1,
typename M2>
144 AxisZ::alphaCross(-m_theta_dot,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] * m_y_dot;
150 v_out[1] += w_in[2] * m_x_dot;
151 v_out[2] += -w_in[1] * m_x_dot + w_in[0] * m_y_dot ;
154 AxisZ::alphaCross(-m_theta_dot,v.angular(),mout.angular());
157 template<
typename M1>
172 template<
typename Scalar,
int Options,
typename MotionDerived>
173 inline typename MotionDerived::MotionPlain
176 typename MotionDerived::MotionPlain result(m2);
177 result.linear()[0] += m1.m_x_dot;
178 result.linear()[1] += m1.m_y_dot;
180 result.angular()[2] += m1.m_theta_dot;
187 template<
typename _Scalar,
int _Options>
190 typedef _Scalar Scalar;
191 enum { Options = _Options };
197 typedef Eigen::Matrix<Scalar,3,1,Options> JointForce;
198 typedef Eigen::Matrix<Scalar,6,3,Options> DenseBase;
199 typedef DenseBase MatrixReturnType;
200 typedef const DenseBase ConstMatrixReturnType;
203 template<
typename _Scalar,
int _Options>
207 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
214 template<
typename Vector3Like>
215 JointMotion __mult__(
const Eigen::MatrixBase<Vector3Like> & vj)
const 217 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
221 int nv_impl()
const {
return NV; }
228 template<
typename Derived>
236 template<
typename Derived>
237 friend typename Eigen::Matrix <typename Eigen::MatrixBase<Derived>::Scalar, 3, Derived::ColsAtCompileTime>
240 typedef typename Eigen::MatrixBase<Derived>::Scalar Scalar;
241 typedef Eigen::Matrix<Scalar, 3, Derived::ColsAtCompileTime> MatrixReturnType;
244 MatrixReturnType result(3, F.cols ());
245 result.template topRows<2>() = F.template topRows<2>();
246 result.template bottomRows<1>() = F.template bottomRows<1>();
254 DenseBase matrix_impl()
const 257 S.template block<3,3>(Inertia::LINEAR, 0).setZero ();
258 S.template block<3,3>(Inertia::ANGULAR, 0).setZero ();
259 S(Inertia::LINEAR + 0,0) = Scalar(1);
260 S(Inertia::LINEAR + 1,1) = Scalar(1);
261 S(Inertia::ANGULAR + 2,2) = Scalar(1);
265 template<
typename S1,
int O1>
268 DenseBase X_subspace;
271 X_subspace.template block <3,2>(Motion::LINEAR, 0) = m.rotation ().template leftCols <2> ();
272 X_subspace.template block <3,1>(Motion::LINEAR, 2) = m.translation().cross(m.rotation ().template rightCols<1>());
275 X_subspace.template block <3,2>(Motion::ANGULAR, 0).setZero ();
276 X_subspace.template block <3,1>(Motion::ANGULAR, 2) = m.rotation ().template rightCols<1>();
281 template<
typename S1,
int O1>
284 DenseBase X_subspace;
287 X_subspace.template block <3,2>(Motion::LINEAR, 0) = m.rotation().transpose().template leftCols <2>();
288 X_subspace.template block <3,1>(Motion::ANGULAR,2) = m.rotation().transpose() * m.translation();
289 X_subspace.template block <3,1>(Motion::LINEAR, 2) = -X_subspace.template block <3,1>(Motion::ANGULAR,2).cross(m.rotation().transpose().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().transpose().template rightCols<1>();
298 template<
typename MotionDerived>
301 const typename MotionDerived::ConstLinearType v = m.linear();
302 const typename MotionDerived::ConstAngularType w = m.angular();
303 DenseBase res(DenseBase::Zero());
305 res(0,1) = -w[2]; res(0,2) = v[1];
306 res(1,0) = w[2]; res(1,2) = -v[0];
307 res(2,0) = -w[1]; res(2,1) = w[0];
315 template<
typename MotionDerived,
typename S2,
int O2>
316 inline typename MotionDerived::MotionPlain
319 return m2.motionAction(m1);
323 template<
typename S1,
int O1,
typename S2,
int O2>
324 inline typename Eigen::Matrix<S1,6,3,O1>
328 typedef typename Inertia::Scalar Scalar;
329 typedef Eigen::Matrix<S1,6,3,O1> ReturnType;
332 const Scalar mass = Y.mass();
333 const typename Inertia::Vector3 & com = Y.lever();
334 const typename Inertia::Symmetric3 & inertia = Y.inertia();
336 M.template topLeftCorner<3,3>().setZero();
337 M.template topLeftCorner<2,2>().diagonal().fill(mass);
339 const typename Inertia::Vector3 mc(mass * com);
340 M.template rightCols<1>().
template head<2>() << -mc(1), mc(0);
342 M.template bottomLeftCorner<3,2>() << (Scalar)0, -mc(2), mc(2), (Scalar)0, -mc(1), mc(0);
343 M.template rightCols<1>().
template tail<3>() = inertia.data().template tail<3>();
344 M.template rightCols<1>()[3] -= mc(0)*com(2);
345 M.template rightCols<1>()[4] -= mc(1)*com(2);
346 M.template rightCols<1>()[5] += mass*(com(0)*com(0) + com(1)*com(1));
354 template<
typename M6Like,
typename S2,
int O2>
355 inline Eigen::Matrix<S2,6,3,O2>
356 operator*(
const Eigen::MatrixBase<M6Like> & Y,
359 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
360 typedef Eigen::Matrix<S2,6,3,O2> Matrix63;
363 IS.template leftCols<2>() = Y.template leftCols<2>();
364 IS.template rightCols<1>() = Y.template rightCols<1>();
369 template<
typename S1,
int O1>
371 {
typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
373 template<
typename S1,
int O1,
typename MotionDerived>
375 {
typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
379 template<
typename _Scalar,
int _Options>
386 enum { Options = _Options };
387 typedef _Scalar Scalar;
396 typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
397 typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
398 typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
400 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
402 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
403 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
406 template<
typename Scalar,
int Options>
408 template<
typename Scalar,
int Options>
411 template<
typename _Scalar,
int _Options>
414 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
416 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
417 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
433 static std::string classname() {
return std::string(
"JointDataPlanar"); }
434 std::string
shortname()
const {
return classname(); }
439 template<
typename _Scalar,
int _Options>
443 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
445 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
451 using Base::setIndexes;
453 JointDataDerived
createData()
const {
return JointDataDerived(); }
455 template<
typename ConfigVector>
456 inline void forwardKinematics(Transformation_t & M,
const Eigen::MatrixBase<ConfigVector> & q_joint)
const 459 & c_theta = q_joint(2),
460 & s_theta = q_joint(3);
462 M.rotation().template topLeftCorner<2,2>() << c_theta, -s_theta, s_theta, c_theta;
463 M.translation().template head<2>() = q_joint.template head<2>();
466 template<
typename ConfigVector>
467 void calc(JointDataDerived & data,
468 const typename Eigen::MatrixBase<ConfigVector> & qs)
const 470 typedef typename ConfigVector::Scalar Scalar;
471 typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type & q = qs.template segment<NQ>(
idx_q());
477 data.M.rotation().template topLeftCorner<2,2>() << c_theta, -s_theta, s_theta, c_theta;
478 data.M.translation().template head<2>() = q.template head<2>();
482 template<
typename ConfigVector,
typename TangentVector>
483 void calc(JointDataDerived & data,
484 const typename Eigen::MatrixBase<ConfigVector> & qs,
485 const typename Eigen::MatrixBase<TangentVector> & vs)
const 487 calc(data,qs.derived());
489 typename TangentVector::template ConstFixedSegmentReturnType<NV>::Type & q_dot = vs.template segment<NV>(
idx_v ());
491 data.v.m_x_dot = q_dot(0);
492 data.v.m_y_dot = q_dot(1);
493 data.v.m_theta_dot = q_dot(2);
496 template<
typename Matrix6Like>
497 void calc_aba(JointDataDerived & data,
498 const Eigen::MatrixBase<Matrix6Like> & I,
499 const bool update_I)
const 501 data.U.template leftCols<2>() = I.template leftCols<2>();
502 data.U.template rightCols<1>() = I.template rightCols<1>();
504 data.StU.template leftCols<2>() = data.U.template topRows<2>().transpose();
505 data.StU.template rightCols<1>() = data.U.template bottomRows<1>();
510 internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv);
512 data.UDinv.noalias() = data.U * data.Dinv;
515 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).noalias() -= data.UDinv * data.U.transpose();
518 static std::string classname() {
return std::string(
"JointModelPlanar");}
519 std::string
shortname()
const {
return classname(); }
522 template<
typename NewScalar>
535 #include <boost/type_traits.hpp> 539 template<
typename Scalar,
int Options>
541 :
public integral_constant<bool,true> {};
543 template<
typename Scalar,
int Options>
545 :
public integral_constant<bool,true> {};
547 template<
typename Scalar,
int Options>
549 :
public integral_constant<bool,true> {};
551 template<
typename Scalar,
int Options>
553 :
public integral_constant<bool,true> {};
556 #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...
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)
.