6 #ifndef __pinocchio_joint_revolute_unaligned_hpp__ 7 #define __pinocchio_joint_revolute_unaligned_hpp__ 9 #include "pinocchio/fwd.hpp" 10 #include "pinocchio/multibody/joint/joint-base.hpp" 11 #include "pinocchio/multibody/constraint.hpp" 12 #include "pinocchio/spatial/inertia.hpp" 13 #include "pinocchio/math/matrix.hpp" 21 template<
typename Scalar,
int Options>
27 template<
typename Scalar,
int Options,
typename MotionDerived>
33 template<
typename _Scalar,
int _Options>
36 typedef _Scalar Scalar;
37 enum { Options = _Options };
38 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
39 typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
40 typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
41 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
42 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
43 typedef Vector3 AngularType;
44 typedef Vector3 LinearType;
45 typedef const Vector3 ConstAngularType;
46 typedef const Vector3 ConstLinearType;
47 typedef Matrix6 ActionMatrixType;
56 template<
typename _Scalar,
int _Options>
58 :
MotionBase< MotionRevoluteUnalignedTpl<_Scalar,_Options> >
60 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
65 template<
typename Vector3Like,
typename OtherScalar>
67 const OtherScalar & w)
78 template<
typename OtherScalar>
84 template<
typename MotionDerived>
87 v.angular() += axis*w;
90 template<
typename Derived>
93 other.linear().setZero();
94 other.angular().noalias() = axis*w;
97 template<
typename S2,
int O2,
typename D2>
101 v.angular().noalias() = w * m.rotation() * axis;
104 v.linear().noalias() = m.translation().cross(v.angular());
107 template<
typename S2,
int O2>
111 se3Action_impl(m,res);
115 template<
typename S2,
int O2,
typename D2>
121 v3_tmp.noalias() = axis.cross(m.translation());
123 v.linear().noalias() = m.rotation().transpose() * v3_tmp;
126 v.angular().noalias() = m.rotation().transpose() * axis;
130 template<
typename S2,
int O2>
134 se3ActionInverse_impl(m,res);
138 template<
typename M1,
typename M2>
142 mout.linear().noalias() = v.linear().cross(axis);
146 mout.angular().noalias() = v.angular().cross(axis);
150 template<
typename M1>
164 template<
typename S1,
int O1,
typename MotionDerived>
165 inline typename MotionDerived::MotionPlain
169 typename MotionDerived::MotionPlain res(m2);
174 template<
typename MotionDerived,
typename S2,
int O2>
175 inline typename MotionDerived::MotionPlain
179 return m2.motionAction(m1);
184 template<
typename _Scalar,
int _Options>
187 typedef _Scalar Scalar;
188 enum { Options = _Options };
194 typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
195 typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
196 typedef DenseBase MatrixReturnType;
197 typedef const DenseBase ConstMatrixReturnType;
199 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
202 template<
typename Scalar,
int Options>
204 {
typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
206 template<
typename Scalar,
int Options,
typename MotionDerived>
208 {
typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
210 template<
typename Scalar,
int Options,
typename ForceDerived>
214 typedef Eigen::Matrix<typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<ForceDerived>::ConstAngularType),1,1,Options> ReturnType;
217 template<
typename Scalar,
int Options,
typename ForceSet>
222 typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type
226 template<
typename _Scalar,
int _Options>
228 :
ConstraintBase< ConstraintRevoluteUnalignedTpl<_Scalar,_Options> >
230 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
239 template<
typename Vector3Like>
244 template<
typename Vector1Like>
245 JointMotion __mult__(
const Eigen::MatrixBase<Vector1Like> & v)
const 247 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
248 return JointMotion(axis,v[0]);
251 template<
typename S1,
int O1>
259 res.template segment<3>(ANGULAR).noalias() = m.rotation() * axis;
260 res.template segment<3>(LINEAR).noalias() = m.translation().cross(res.template segment<3>(ANGULAR));
264 template<
typename S1,
int O1>
271 res.template segment<3>(ANGULAR).noalias() = m.rotation().transpose() * axis;
272 res.template segment<3>(LINEAR).noalias() = -m.rotation().transpose() * m.translation().cross(axis);
276 int nv_impl()
const {
return NV; }
283 template<
typename ForceDerived>
284 typename ConstraintForceOp<ConstraintRevoluteUnalignedTpl,ForceDerived>::ReturnType
287 typedef typename ConstraintForceOp<ConstraintRevoluteUnalignedTpl,ForceDerived>::ReturnType ReturnType;
289 res[0] = ref.axis.dot(f.
angular());
294 template<
typename ForceSet>
295 typename ConstraintForceSetOp<ConstraintRevoluteUnalignedTpl,ForceSet>::ReturnType
296 operator*(
const Eigen::MatrixBase<ForceSet> & F)
298 EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
300 return ref.axis.transpose() * F.template middleRows<3>(ANGULAR);
313 DenseBase matrix_impl()
const 316 S << Vector3::Zero(), axis;
320 template<
typename MotionDerived>
324 const typename MotionDerived::ConstLinearType v = m.linear();
325 const typename MotionDerived::ConstAngularType w = m.angular();
328 res << v.cross(axis), w.cross(axis);
338 template<
typename S1,
int O1,
typename S2,
int O2>
341 typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
347 template<
typename S1,
int O1,
typename S2,
int O2>
353 static inline ReturnType run(
const Inertia & Y,
354 const Constraint & cru)
359 const typename Inertia::Scalar & m = Y.mass();
360 const typename Inertia::Vector3 & c = Y.lever();
361 const typename Inertia::Symmetric3 & I = Y.inertia();
363 res.template segment<3>(Inertia::LINEAR) = -m*c.cross(cru.axis);
364 res.template segment<3>(Inertia::ANGULAR).noalias() = I*cru.axis;
365 res.template segment<3>(Inertia::ANGULAR) += c.cross(res.template segment<3>(Inertia::LINEAR));
372 template<
typename M6Like,
typename Scalar,
int Options>
376 typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst;
379 typedef typename Constraint::Vector3 Vector3;
380 typedef const typename MatrixMatrixProduct<M6LikeColsNonConst,Vector3>::type ReturnType;
386 template<
typename M6Like,
typename Scalar,
int Options>
392 static inline ReturnType run(
const Eigen::MatrixBase<M6Like> & Y,
393 const Constraint & cru)
395 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
396 return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.axis;
403 template<
typename _Scalar,
int _Options>
410 typedef _Scalar Scalar;
411 enum { Options = _Options };
420 typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
421 typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
422 typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
424 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
426 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
427 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
431 template<
typename Scalar,
int Options>
435 template<
typename Scalar,
int Options>
439 template<
typename _Scalar,
int _Options>
441 :
public JointDataBase< JointDataRevoluteUnalignedTpl<_Scalar,_Options> >
443 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
445 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
446 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
460 template<
typename Vector3Like>
464 , v(axis,(Scalar)NAN)
465 , U(), Dinv(), UDinv()
468 static std::string classname() {
return std::string(
"JointDataRevoluteUnaligned"); }
469 std::string
shortname()
const {
return classname(); }
474 template<
typename _Scalar,
int _Options>
476 :
public JointModelBase< JointModelRevoluteUnalignedTpl<_Scalar,_Options> >
478 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
480 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
481 typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;
487 using Base::setIndexes;
497 assert(isUnitary(axis) &&
"Rotation axis is not unitary");
500 template<
typename Vector3Like>
504 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
505 assert(isUnitary(axis) &&
"Rotation axis is not unitary");
508 JointDataDerived
createData()
const {
return JointDataDerived(axis); }
513 return Base::isEqual(other) && axis == other.
axis;
516 template<
typename ConfigVector>
517 void calc(JointDataDerived & data,
518 const typename Eigen::MatrixBase<ConfigVector> & qs)
const 520 typedef typename ConfigVector::Scalar OtherScalar;
521 typedef Eigen::AngleAxis<Scalar> AngleAxis;
523 const OtherScalar & q = qs[
idx_q()];
528 template<
typename ConfigVector,
typename TangentVector>
529 void calc(JointDataDerived & data,
530 const typename Eigen::MatrixBase<ConfigVector> & qs,
531 const typename Eigen::MatrixBase<TangentVector> & vs)
const 533 calc(data,qs.derived());
535 data.v.w = (Scalar)vs[
idx_v()];
538 template<
typename Matrix6Like>
539 void calc_aba(JointDataDerived & data,
const Eigen::MatrixBase<Matrix6Like> & I,
const bool update_I)
const 541 data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis;
542 data.Dinv[0] = (Scalar)(1)/axis.dot(data.U.template segment<3>(Motion::ANGULAR));
543 data.UDinv.noalias() = data.U * data.Dinv;
546 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
549 static std::string classname() {
return std::string(
"JointModelRevoluteUnaligned"); }
550 std::string
shortname()
const {
return classname(); }
553 template<
typename NewScalar>
557 ReturnType res(axis.template cast<NewScalar>());
572 #include <boost/type_traits.hpp> 576 template<
typename Scalar,
int Options>
578 :
public integral_constant<bool,true> {};
580 template<
typename Scalar,
int Options>
582 :
public integral_constant<bool,true> {};
584 template<
typename Scalar,
int Options>
586 :
public integral_constant<bool,true> {};
588 template<
typename Scalar,
int Options>
590 :
public integral_constant<bool,true> {};
594 #endif // ifndef __pinocchio_joint_revolute_unaligned_hpp__
Forward declaration of the multiplication operation return type. Should be overloaded, otherwise it will procude a compilation error.
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...
Vector3 axis
3d main axis of the joint.
Return type of the Constraint::Transpose * ForceSet operation.
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.
Source from #include <cppad/example/cppad_eigen.hpp>
ConstAngularType angular() const
Return the angular part of the force vector.
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.
void toRotationMatrix(const Eigen::MatrixBase< Vector3 > &axis, const Scalar &cos_value, const Scalar &sin_value, const Eigen::MatrixBase< Matrix3 > &res)
Computes a rotation matrix from a vector and values of sin and cos orientations values.
JointModelRevoluteUnalignedTpl< NewScalar, Options > cast() const
Return type of the Constraint::Transpose * Force operation.
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)