6 #ifndef __pinocchio_joint_prismatic_unaligned_hpp__ 7 #define __pinocchio_joint_prismatic_unaligned_hpp__ 9 #include "pinocchio/macros.hpp" 10 #include "pinocchio/multibody/joint/joint-base.hpp" 11 #include "pinocchio/multibody/joint/joint-translation.hpp" 12 #include "pinocchio/multibody/constraint.hpp" 13 #include "pinocchio/spatial/inertia.hpp" 14 #include "pinocchio/math/matrix.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 < MotionPrismaticUnalignedTpl<_Scalar,_Options> >
61 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
66 template<
typename Vector3Like,
typename S2>
69 : axis(axis), rate(rate)
70 { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }
75 PlainReturnType::Vector3::Zero());
78 template<
typename OtherScalar>
84 template<
typename Derived>
87 other.linear() += axis * rate;
90 template<
typename Derived>
93 other.linear().noalias() = axis*rate;
94 other.angular().setZero();
97 template<
typename S2,
int O2,
typename D2>
100 v.linear().noalias() = rate * (m.rotation() * axis);
101 v.angular().setZero();
104 template<
typename S2,
int O2>
108 se3Action_impl(m,res);
112 template<
typename S2,
int O2,
typename D2>
116 v.linear().noalias() = rate * (m.rotation().transpose() * axis);
119 v.angular().setZero();
122 template<
typename S2,
int O2>
126 se3ActionInverse_impl(m,res);
130 template<
typename M1,
typename M2>
134 mout.linear().noalias() = v.angular().cross(axis);
135 mout.linear() *= rate;
138 mout.angular().setZero();
141 template<
typename M1>
154 template<
typename Scalar,
int Options,
typename MotionDerived>
155 inline typename MotionDerived::MotionPlain
158 typedef typename MotionDerived::MotionPlain ReturnType;
159 return ReturnType(m1.rate*m1.axis + m2.linear(), m2.angular());
162 template<
typename MotionDerived,
typename S2,
int O2>
163 inline typename MotionDerived::MotionPlain
167 return m2.motionAction(m1);
172 template<
typename _Scalar,
int _Options>
175 typedef _Scalar Scalar;
176 enum { Options = _Options };
182 typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
183 typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
184 typedef DenseBase MatrixReturnType;
185 typedef const DenseBase ConstMatrixReturnType;
187 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
190 template<
typename Scalar,
int Options>
192 {
typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
194 template<
typename Scalar,
int Options,
typename MotionDerived>
196 {
typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
198 template<
typename Scalar,
int Options,
typename ForceDerived>
202 typedef Eigen::Matrix<typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<ForceDerived>::ConstAngularType),1,1,Options> ReturnType;
205 template<
typename Scalar,
int Options,
typename ForceSet>
210 typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type
214 template<
typename _Scalar,
int _Options>
216 :
ConstraintBase< ConstraintPrismaticUnalignedTpl<_Scalar,_Options> >
218 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
227 template<
typename Vector3Like>
230 { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }
232 template<
typename Vector1Like>
233 JointMotion __mult__(
const Eigen::MatrixBase<Vector1Like> & v)
const 235 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
236 return JointMotion(axis,v[0]);
239 template<
typename S1,
int O1>
245 v.linear().noalias() = m.rotation()*axis;
246 v.angular().setZero();
250 template<
typename S1,
int O1>
256 v.linear().noalias() = m.rotation().transpose()*axis;
257 v.angular().setZero();
261 int nv_impl()
const {
return NV; }
268 template<
typename ForceDerived>
269 typename ConstraintForceOp<ConstraintPrismaticUnalignedTpl,ForceDerived>::ReturnType
272 typedef typename ConstraintForceOp<ConstraintPrismaticUnalignedTpl,ForceDerived>::ReturnType ReturnType;
274 res[0] = ref.axis.dot(f.
linear());
279 template<
typename ForceSet>
280 typename ConstraintForceSetOp<ConstraintPrismaticUnalignedTpl,ForceSet>::ReturnType
281 operator*(
const Eigen::MatrixBase<ForceSet> & F)
283 EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
285 return ref.axis.transpose() * F.template middleRows<3>(LINEAR);
298 DenseBase matrix_impl()
const 301 S << axis, Vector3::Zero();
305 template<
typename MotionDerived>
309 res << v.angular().cross(axis), Vector3::Zero();
319 template<
typename S1,
int O1,
typename S2,
int O2>
322 typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
328 template<
typename S1,
int O1,
typename S2,
int O2>
335 static inline ReturnType run(
const Inertia & Y,
336 const Constraint & cpu)
340 const S1 & m = Y.mass();
341 const typename Inertia::Vector3 & c = Y.lever();
343 res.template head<3>().noalias() = m*cpu.axis;
344 res.template tail<3>().noalias() = c.cross(res.template head<3>());
351 template<
typename M6Like,
typename Scalar,
int Options>
355 typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst;
358 typedef typename Constraint::Vector3 Vector3;
359 typedef const typename MatrixMatrixProduct<M6LikeColsNonConst,Vector3>::type ReturnType;
365 template<
typename M6Like,
typename Scalar,
int Options>
370 static inline ReturnType run(
const Eigen::MatrixBase<M6Like> & Y,
371 const Constraint & cru)
373 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
374 return Y.derived().template middleCols<3>(Constraint::LINEAR) * cru.axis;
381 template<
typename _Scalar,
int _Options>
388 typedef _Scalar Scalar;
389 enum { Options = _Options };
398 typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
399 typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
400 typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
402 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
404 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
405 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
408 template<
typename Scalar,
int Options>
412 template<
typename _Scalar,
int _Options>
414 :
public JointDataBase< JointDataPrismaticUnalignedTpl<_Scalar,_Options> >
416 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
418 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
419 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
433 template<
typename Vector3Like>
437 , v(axis,(Scalar)NAN)
438 , U(), Dinv(), UDinv()
441 static std::string classname() {
return std::string(
"JointDataPrismaticUnaligned"); }
442 std::string
shortname()
const {
return classname(); }
446 template<
typename Scalar,
int Options>
451 template<
typename _Scalar,
int _Options>
453 :
public JointModelBase< JointModelPrismaticUnalignedTpl<_Scalar,_Options> >
455 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
457 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
463 using Base::setIndexes;
465 typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;
474 assert(isUnitary(axis) &&
"Translation axis is not unitary");
477 template<
typename Vector3Like>
481 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
482 assert(isUnitary(axis) &&
"Translation axis is not unitary");
485 JointDataDerived
createData()
const {
return JointDataDerived(axis); }
490 return Base::isEqual(other) && axis == other.
axis;
493 template<
typename ConfigVector>
494 void calc(JointDataDerived & data,
495 const typename Eigen::MatrixBase<ConfigVector> & qs)
const 497 typedef typename ConfigVector::Scalar Scalar;
498 const Scalar & q = qs[
idx_q()];
500 data.M.translation().noalias() = axis * q;
503 template<
typename ConfigVector,
typename TangentVector>
504 void calc(JointDataDerived & data,
505 const typename Eigen::MatrixBase<ConfigVector> & qs,
506 const typename Eigen::MatrixBase<TangentVector> & vs)
const 508 calc(data,qs.derived());
510 typedef typename TangentVector::Scalar S2;
511 const S2 & v = vs[
idx_v()];
515 template<
typename Matrix6Like>
516 void calc_aba(JointDataDerived & data,
const Eigen::MatrixBase<Matrix6Like> & I,
const bool update_I)
const 518 data.U.noalias() = I.template block<6,3> (0,Inertia::LINEAR) * axis;
519 data.Dinv[0] = Scalar(1)/axis.dot(data.U.template segment <3> (Inertia::LINEAR));
520 data.UDinv.noalias() = data.U * data.Dinv;
523 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
526 static std::string classname() {
return std::string(
"JointModelPrismaticUnaligned"); }
527 std::string
shortname()
const {
return classname(); }
530 template<
typename NewScalar>
534 ReturnType res(axis.template cast<NewScalar>());
549 #include <boost/type_traits.hpp> 553 template<
typename Scalar,
int Options>
555 :
public integral_constant<bool,true> {};
557 template<
typename Scalar,
int Options>
559 :
public integral_constant<bool,true> {};
561 template<
typename Scalar,
int Options>
563 :
public integral_constant<bool,true> {};
565 template<
typename Scalar,
int Options>
567 :
public integral_constant<bool,true> {};
571 #endif // ifndef __pinocchio_joint_prismatic_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...
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>
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
JointModelPrismaticUnalignedTpl< NewScalar, Options > cast() const
Main pinocchio namespace.
Common traits structure to fully define base classes for CRTP.
ConstLinearType linear() const
Return the linear part of the force vector.
Vector3 axis
3d main axis of the joint.
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)
.