6 #ifndef __pinocchio_joint_planar_hpp__
7 #define __pinocchio_joint_planar_hpp__
22 template<
typename Scalar,
int Options>
28 template<
typename Scalar,
int Options,
typename MotionDerived>
34 template<
typename _Scalar,
int _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;
59 template<
typename _Scalar,
int _Options>
60 struct MotionPlanarTpl
61 : MotionBase< MotionPlanarTpl<_Scalar,_Options> >
63 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
73 m_data << x_dot, y_dot, theta_dot;
76 template<
typename Vector3Like>
80 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
83 inline PlainReturnType
plain()
const
85 return PlainReturnType(
typename PlainReturnType::Vector3(
vx(),
vy(),
Scalar(0)),
89 template<
typename Derived>
97 template<
typename MotionDerived>
104 template<
typename S2,
int O2,
typename D2>
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>
120 template<
typename S2,
int O2,
typename D2>
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>
142 template<
typename M1,
typename M2>
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() ;
159 template<
typename M1>
181 return m_data == other.m_data;
190 template<
typename Scalar,
int Options,
typename MotionDerived>
191 inline typename MotionDerived::MotionPlain
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>
221 template<
typename _Scalar,
int _Options>
225 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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);
246 template<
typename Derived>
254 template<
typename Derived>
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>();
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>
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>
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>
320 const typename MotionDerived::ConstLinearType
v =
m.linear();
321 const typename MotionDerived::ConstAngularType
w =
m.angular();
322 DenseBase
res(DenseBase::Zero());
337 template<
typename MotionDerived,
typename S2,
int O2>
338 inline typename MotionDerived::MotionPlain
341 return m2.motionAction(m1);
345 template<
typename S1,
int O1,
typename S2,
int O2>
346 inline typename Eigen::Matrix<S1,6,3,O1>
351 typedef Eigen::Matrix<S1,6,3,O1> ReturnType;
355 const typename Inertia::Vector3 &
com =
Y.lever();
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>
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>
395 template<
typename S1,
int O1,
typename MotionDerived>
401 template<
typename _Scalar,
int _Options>
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;
428 template<
typename Scalar,
int Options>
430 template<
typename Scalar,
int Options>
433 template<
typename _Scalar,
int _Options>
436 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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"); }
467 template<
typename _Scalar,
int _Options>
468 struct JointModelPlanarTpl
469 :
public JointModelBase< JointModelPlanarTpl<_Scalar,_Options> >
471 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
481 JointDataDerived
createData()
const {
return JointDataDerived(); }
485 return {
true,
true,
false,
false};
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>
506 const typename Eigen::MatrixBase<ConfigVector> &
qs)
const
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>
522 const typename Eigen::MatrixBase<ConfigVector> &
qs,
523 const typename Eigen::MatrixBase<TangentVector> & vs)
const
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>
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>();
556 static std::string
classname() {
return std::string(
"JointModelPlanar");}
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__