6 #ifndef __pinocchio_joint_spherical_ZYX_hpp__ 7 #define __pinocchio_joint_spherical_ZYX_hpp__ 9 #include "pinocchio/macros.hpp" 10 #include "pinocchio/multibody/joint/joint-base.hpp" 11 #include "pinocchio/multibody/joint/joint-spherical.hpp" 12 #include "pinocchio/multibody/constraint.hpp" 13 #include "pinocchio/math/sincos.hpp" 14 #include "pinocchio/math/matrix.hpp" 15 #include "pinocchio/spatial/inertia.hpp" 16 #include "pinocchio/spatial/skew.hpp" 22 template <
typename _Scalar,
int _Options>
25 typedef _Scalar Scalar;
26 enum { Options = _Options };
34 typedef Eigen::Matrix<Scalar,3,1,Options> JointForce;
35 typedef Eigen::Matrix<Scalar,6,3,Options> DenseBase;
37 typedef DenseBase MatrixReturnType;
38 typedef const DenseBase ConstMatrixReturnType;
41 template<
typename _Scalar,
int _Options>
43 :
public ConstraintBase< ConstraintSphericalZYXTpl<_Scalar,_Options> >
45 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
50 typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
54 template<
typename Matrix3Like>
57 { EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like,3,3); }
59 template<
typename Vector3Like>
60 JointMotion __mult__(
const Eigen::MatrixBase<Vector3Like> & v)
const 62 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
66 Matrix3 & operator() () {
return S_minimal; }
67 const Matrix3 & operator() ()
const {
return S_minimal; }
69 int nv_impl()
const {
return NV; }
76 template<
typename Derived>
78 Eigen::Transpose<const Matrix3>,
79 Eigen::Block<const typename ForceDense<Derived>::Vector6,3,1>
83 return ref.S_minimal.transpose() * phi.
angular();
89 typename Eigen::Transpose<const Matrix3>,
90 typename Eigen::MatrixBase<const D>::template NRowsBlockXpr<3>::Type
92 operator* (
const Eigen::MatrixBase<D> & F)
const 94 EIGEN_STATIC_ASSERT(D::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
95 return ref.S_minimal.transpose () * F.template middleRows<3>(ANGULAR);
101 DenseBase matrix_impl()
const 104 S.template middleRows<3>(LINEAR).setZero();
105 S.template middleRows<3>(ANGULAR) = S_minimal;
113 template<
typename S1,
int O1>
114 Eigen::Matrix<Scalar,6,3,Options>
123 Eigen::Matrix<Scalar,6,3,Options> result;
126 result.template middleRows<3>(ANGULAR).noalias() = m.rotation () * S_minimal;
129 cross(m.translation(),
130 result.template middleRows<3>(Motion::ANGULAR),
131 result.template middleRows<3>(LINEAR));
136 template<
typename S1,
int O1>
137 Eigen::Matrix<Scalar,6,3,Options>
140 Eigen::Matrix<Scalar,6,3,Options> result;
143 cross(m.translation(),
145 result.template middleRows<3>(ANGULAR));
146 result.template middleRows<3>(LINEAR).noalias() = -m.rotation().transpose() * result.template middleRows<3>(ANGULAR);
149 result.template middleRows<3>(ANGULAR).noalias() = m.rotation().transpose() * S_minimal;
154 template<
typename MotionDerived>
157 const typename MotionDerived::ConstLinearType v = m.linear();
158 const typename MotionDerived::ConstAngularType w = m.angular();
161 cross(v,S_minimal,res.template middleRows<3>(LINEAR));
162 cross(w,S_minimal,res.template middleRows<3>(ANGULAR));
173 template <
typename S1,
int O1,
typename S2,
int O2>
174 Eigen::Matrix<S1,6,3,O1>
180 Eigen::Matrix<S1,6,3,O1> M;
181 alphaSkew (-Y.mass(),Y.lever(),M.template middleRows<3>(Constraint::LINEAR));
182 M.template middleRows<3>(Constraint::ANGULAR) = (Y.inertia () -
183 typename Symmetric3::AlphaSkewSquare(Y.mass (), Y.lever ())).matrix();
185 return (M * S.S_minimal).eval();
190 template<
typename Matrix6Like,
typename S2,
int O2>
192 typename Eigen::internal::remove_const<typename SizeDepType<3>::ColsReturn<Matrix6Like>::ConstType>::type,
193 typename ConstraintSphericalZYXTpl<S2,O2>::Matrix3
195 operator*(
const Eigen::MatrixBase<Matrix6Like> & Y,
198 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like,6,6);
199 return Y.derived().template middleCols<3>(Inertia::ANGULAR) * S.S_minimal;
202 template<
typename S1,
int O1>
209 typedef Eigen::Matrix<S1,6,3,O1> ReturnType;
212 template<
typename S1,
int O1,
typename MotionDerived>
215 typedef Eigen::Matrix<S1,6,3,O1> ReturnType;
220 template<
typename _Scalar,
int _Options>
227 typedef _Scalar Scalar;
228 enum { Options = _Options };
237 typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
238 typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
239 typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
241 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
243 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
244 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
247 template<
typename Scalar,
int Options>
251 template<
typename Scalar,
int Options>
255 template<
typename _Scalar,
int _Options>
258 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
260 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
261 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
276 static std::string classname() {
return std::string(
"JointDataSphericalZYX"); }
277 std::string
shortname()
const {
return classname(); }
282 template<
typename _Scalar,
int _Options>
284 :
public JointModelBase< JointModelSphericalZYXTpl<_Scalar,_Options> >
286 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
288 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
294 using Base::setIndexes;
296 JointDataDerived
createData()
const {
return JointDataDerived(); }
298 template<
typename ConfigVector>
299 void calc(JointDataDerived & data,
300 const typename Eigen::MatrixBase<ConfigVector> & qs)
const 302 typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type & q = qs.template segment<NQ>(
idx_q());
304 typedef typename ConfigVector::Scalar S2;
306 S2 c0,s0;
SINCOS(q(0), &s0, &c0);
307 S2 c1,s1;
SINCOS(q(1), &s1, &c1);
308 S2 c2,s2;
SINCOS(q(2), &s2, &c2);
310 data.M.rotation () << c0 * c1,
311 c0 * s1 * s2 - s0 * c2,
312 c0 * s1 * c2 + s0 * s2,
314 s0 * s1 * s2 + c0 * c2,
315 s0 * s1 * c2 - c0 * s2,
321 << -s1, Scalar(0), Scalar(1),
322 c1 * s2, c2, Scalar(0),
323 c1 * c2, -s2, Scalar(0);
326 template<
typename ConfigVector,
typename TangentVector>
327 void calc(JointDataDerived & data,
328 const typename Eigen::MatrixBase<ConfigVector> & qs,
329 const typename Eigen::MatrixBase<TangentVector> & vs)
const 331 typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type & q = qs.template segment<NQ>(
idx_q());
333 typedef typename ConfigVector::Scalar S2;
335 S2 c0,s0;
SINCOS(q(0), &s0, &c0);
336 S2 c1,s1;
SINCOS(q(1), &s1, &c1);
337 S2 c2,s2;
SINCOS(q(2), &s2, &c2);
339 data.M.rotation () << c0 * c1,
340 c0 * s1 * s2 - s0 * c2,
341 c0 * s1 * c2 + s0 * s2,
343 s0 * s1 * s2 + c0 * c2,
344 s0 * s1 * c2 - c0 * s2,
350 << -s1, Scalar(0), Scalar(1),
351 c1 * s2, c2, Scalar(0),
352 c1 * c2, -s2, Scalar(0);
354 typename TangentVector::template ConstFixedSegmentReturnType<NV>::Type
355 & q_dot = vs.template segment<NV>(
idx_v());
357 data.v().noalias() = data.S.S_minimal * q_dot;
359 data.c()(0) = -c1 * q_dot(0) * q_dot(1);
360 data.c()(1) = -s1 * s2 * q_dot(0) * q_dot(1) + c1 * c2 * q_dot(0) * q_dot(2) - s2 * q_dot(1) * q_dot(2);
361 data.c()(2) = -s1 * c2 * q_dot(0) * q_dot(1) - c1 * s2 * q_dot(0) * q_dot(2) - c2 * q_dot(1) * q_dot(2);
364 template<
typename Matrix6Like>
365 void calc_aba(JointDataDerived & data,
366 const Eigen::MatrixBase<Matrix6Like> & I,
367 const bool update_I)
const 369 data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * data.S.S_minimal;
370 data.StU.noalias() = data.S.S_minimal.transpose() * data.U.template middleRows<3>(Motion::ANGULAR);
375 internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv);
377 data.UDinv.noalias() = data.U * data.Dinv;
380 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
383 static std::string classname() {
return std::string(
"JointModelSphericalZYX"); }
384 std::string
shortname()
const {
return classname(); }
387 template<
typename NewScalar>
400 #include <boost/type_traits.hpp> 404 template<
typename Scalar,
int Options>
406 :
public integral_constant<bool,true> {};
408 template<
typename Scalar,
int Options>
410 :
public integral_constant<bool,true> {};
412 template<
typename Scalar,
int Options>
414 :
public integral_constant<bool,true> {};
416 template<
typename Scalar,
int Options>
418 :
public integral_constant<bool,true> {};
421 #endif // ifndef __pinocchio_joint_spherical_ZYX_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 SINCOS(const Scalar &a, Scalar *sa, Scalar *ca)
Computes sin/cos values of a given input scalar.
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...
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.
JointModelSphericalZYXTpl< 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 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)
.