6 #ifndef __pinocchio_joint_spherical_hpp__ 7 #define __pinocchio_joint_spherical_hpp__ 9 #include "pinocchio/macros.hpp" 10 #include "pinocchio/multibody/joint/joint-base.hpp" 11 #include "pinocchio/multibody/constraint.hpp" 12 #include "pinocchio/math/sincos.hpp" 13 #include "pinocchio/spatial/inertia.hpp" 14 #include "pinocchio/spatial/skew.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< MotionSphericalTpl<_Scalar,_Options> >
61 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
67 template<
typename Vector3Like>
72 Vector3 & operator() () {
return w; }
73 const Vector3 & operator() ()
const {
return w; }
80 template<
typename MotionDerived>
86 template<
typename Derived>
89 other.linear().setZero();
103 template<
typename MotionDerived>
106 return other.angular() == w && other.linear().isZero(0);
109 template<
typename S2,
int O2,
typename D2>
113 v.angular().noalias() = m.rotation() * w;
116 v.linear().noalias() = m.translation().cross(v.angular());
119 template<
typename S2,
int O2>
123 se3Action_impl(m,res);
127 template<
typename S2,
int O2,
typename D2>
133 v3_tmp.noalias() = w.cross(m.translation());
134 v.linear().noalias() = m.rotation().transpose() * v3_tmp;
137 v.angular().noalias() = m.rotation().transpose() * w;
140 template<
typename S2,
int O2>
144 se3ActionInverse_impl(m,res);
148 template<
typename M1,
typename M2>
152 mout.linear().noalias() = v.linear().cross(w);
155 mout.angular().noalias() = v.angular().cross(w);
158 template<
typename M1>
170 template<
typename S1,
int O1,
typename MotionDerived>
171 inline typename MotionDerived::MotionPlain
174 return typename MotionDerived::MotionPlain(m2.linear(),m2.angular() + m1.w);
179 template<
typename _Scalar,
int _Options>
182 typedef _Scalar Scalar;
183 enum { Options = _Options };
189 typedef Eigen::Matrix<Scalar,3,1,Options> JointForce;
190 typedef Eigen::Matrix<Scalar,6,3,Options> DenseBase;
191 typedef DenseBase MatrixReturnType;
192 typedef const DenseBase ConstMatrixReturnType;
195 template<
typename _Scalar,
int _Options>
197 :
public ConstraintBase< ConstraintSphericalTpl<_Scalar,_Options> >
199 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
207 int nv_impl()
const {
return NV; }
209 template<
typename Vector3Like>
210 JointMotion __mult__(
const Eigen::MatrixBase<Vector3Like> & w)
const 212 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
218 template<
typename Derived>
224 template<
typename MatrixDerived>
226 operator*(
const Eigen::MatrixBase<MatrixDerived> & F)
const 229 return F.derived().template middleRows<3>(Inertia::ANGULAR);
235 DenseBase matrix_impl()
const 238 S.template block <3,3>(LINEAR,0).setZero();
239 S.template block <3,3>(ANGULAR,0).setIdentity();
243 template<
typename S1,
int O1>
244 Eigen::Matrix<S1,6,3,O1> se3Action(
const SE3Tpl<S1,O1> & m)
const 246 Eigen::Matrix<S1,6,3,O1> X_subspace;
247 cross(m.translation(),m.rotation(),X_subspace.template middleRows<3>(LINEAR));
248 X_subspace.template middleRows<3>(ANGULAR) = m.rotation();
253 template<
typename S1,
int O1>
254 Eigen::Matrix<S1,6,3,O1> se3ActionInverse(
const SE3Tpl<S1,O1> & m)
const 256 Eigen::Matrix<S1,6,3,O1> X_subspace;
257 AxisX::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(0));
258 AxisY::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(1));
259 AxisZ::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(2));
261 X_subspace.template middleRows<3>(LINEAR).noalias()
262 = m.rotation().transpose() * X_subspace.template middleRows<3>(ANGULAR);
263 X_subspace.template middleRows<3>(ANGULAR) = m.rotation().transpose();
268 template<
typename MotionDerived>
271 const typename MotionDerived::ConstLinearType v = m.linear();
272 const typename MotionDerived::ConstAngularType w = m.angular();
275 skew(v,res.template middleRows<3>(LINEAR));
276 skew(w,res.template middleRows<3>(ANGULAR));
283 template<
typename MotionDerived,
typename S2,
int O2>
284 inline typename MotionDerived::MotionPlain
288 return m2.motionAction(m1);
292 template<
typename S1,
int O1,
typename S2,
int O2>
293 inline Eigen::Matrix<S2,6,3,O2>
298 typedef typename Inertia::Symmetric3
Symmetric3;
299 Eigen::Matrix<S2,6,3,O2> M;
301 M.template block<3,3>(Inertia::LINEAR,0) =
alphaSkew(-Y.mass(), Y.lever());
302 M.template block<3,3>(Inertia::ANGULAR,0) = (Y.inertia() -
typename Symmetric3::AlphaSkewSquare(Y.mass(), Y.lever())).matrix();
307 template<
typename M6Like,
typename S2,
int O2>
309 operator*(
const Eigen::MatrixBase<M6Like> & Y,
313 return Y.derived().template middleCols<3>(Constraint::ANGULAR);
316 template<
typename S1,
int O1>
318 {
typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
320 template<
typename S1,
int O1,
typename MotionDerived>
322 {
typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
326 template<
typename _Scalar,
int _Options>
333 typedef _Scalar Scalar;
334 enum { Options = _Options };
343 typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
344 typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
345 typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
347 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
349 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
350 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
353 template<
typename Scalar,
int Options>
357 template<
typename Scalar,
int Options>
361 template<
typename _Scalar,
int _Options>
364 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
367 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
368 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
382 static std::string classname() {
return std::string(
"JointDataSpherical"); }
383 std::string
shortname()
const {
return classname(); }
388 template<
typename _Scalar,
int _Options>
390 :
public JointModelBase< JointModelSphericalTpl<_Scalar,_Options> >
392 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
395 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
401 using Base::setIndexes;
403 JointDataDerived
createData()
const {
return JointDataDerived(); }
405 template<
typename ConfigVectorLike>
406 inline void forwardKinematics(Transformation_t & M,
const Eigen::MatrixBase<ConfigVectorLike> & q_joint)
const 408 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike);
409 typedef typename Eigen::Quaternion<typename ConfigVectorLike::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;
410 typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
412 ConstQuaternionMap quat(q_joint.derived().data());
414 assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= 1e-4);
416 M.rotation(quat.matrix());
417 M.translation().setZero();
420 template<
typename QuaternionDerived>
421 void calc(JointDataDerived & data,
422 const typename Eigen::QuaternionBase<QuaternionDerived> & quat)
const 424 data.M.rotation(quat.matrix());
427 template<
typename ConfigVector>
429 void calc(JointDataDerived & data,
430 const typename Eigen::PlainObjectBase<ConfigVector> & qs)
const 432 typedef typename Eigen::Quaternion<typename ConfigVector::Scalar,ConfigVector::Options> Quaternion;
433 typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
435 ConstQuaternionMap quat(qs.template segment<NQ>(
idx_q()).data());
439 template<
typename ConfigVector>
441 void calc(JointDataDerived & data,
442 const typename Eigen::MatrixBase<ConfigVector> & qs)
const 444 typedef typename Eigen::Quaternion<Scalar,Options> Quaternion;
446 const Quaternion quat(qs.template segment<NQ>(
idx_q()));
450 template<
typename ConfigVector,
typename TangentVector>
451 void calc(JointDataDerived & data,
452 const typename Eigen::MatrixBase<ConfigVector> & qs,
453 const typename Eigen::MatrixBase<TangentVector> & vs)
const 455 calc(data,qs.derived());
457 data.v() = vs.template segment<NV>(
idx_v());
460 template<
typename Matrix6Like>
461 void calc_aba(JointDataDerived & data,
462 const Eigen::MatrixBase<Matrix6Like> & I,
463 const bool update_I)
const 465 data.U = I.template block<6,3>(0,Inertia::ANGULAR);
470 internal::PerformStYSInversion<Scalar>::run(data.U.template middleRows<3>(Inertia::ANGULAR),data.Dinv);
472 data.UDinv.template middleRows<3>(Inertia::ANGULAR).setIdentity();
473 data.UDinv.template middleRows<3>(Inertia::LINEAR).noalias() = data.U.template block<3,3>(Inertia::LINEAR, 0) * data.Dinv;
477 Matrix6Like & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I);
478 I_.template block<3,3>(Inertia::LINEAR,Inertia::LINEAR)
479 -= data.UDinv.template middleRows<3>(Inertia::LINEAR) * I_.template block<3,3> (Inertia::ANGULAR, Inertia::LINEAR);
480 I_.template block<6,3>(0,Inertia::ANGULAR).setZero();
481 I_.template block<3,3>(Inertia::ANGULAR,Inertia::LINEAR).setZero();
485 static std::string classname() {
return std::string(
"JointModelSpherical"); }
486 std::string
shortname()
const {
return classname(); }
489 template<
typename NewScalar>
503 #include <boost/type_traits.hpp> 507 template<
typename Scalar,
int Options>
509 :
public integral_constant<bool,true> {};
511 template<
typename Scalar,
int Options>
513 :
public integral_constant<bool,true> {};
515 template<
typename Scalar,
int Options>
517 :
public integral_constant<bool,true> {};
519 template<
typename Scalar,
int Options>
521 :
public integral_constant<bool,true> {};
524 #endif // ifndef __pinocchio_joint_spherical_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.
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.
ConstAngularType angular() const
Return the angular part of the force vector.
JointModelSphericalTpl< NewScalar, Options > cast() const
Main pinocchio namespace.
void alphaSkew(const Scalar alpha, const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector multiplied by a given scalar. i.e. the antisymmetric matrix representation of the cross product operator ( )
Common traits structure to fully define base classes for CRTP.
void skew(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector, i.e. the antisymmetric matrix representation o...
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)
.