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;
52 ConstraintSphericalZYXTpl() {}
54 template<
typename Matrix3Like>
55 ConstraintSphericalZYXTpl(
const Eigen::MatrixBase<Matrix3Like> & subspace)
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);
63 return JointMotion(m_S * v);
66 Matrix3 & operator() () {
return m_S; }
67 const Matrix3 & operator() ()
const {
return m_S; }
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.m_S.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.m_S.transpose () * F.template middleRows<3>(ANGULAR);
101 DenseBase matrix_impl()
const
104 S.template middleRows<3>(LINEAR).setZero();
105 S.template middleRows<3>(ANGULAR) = m_S;
113 template<
typename S1,
int O1>
114 Eigen::Matrix<Scalar,6,3,Options>
115 se3Action(
const SE3Tpl<S1,O1> & m)
const
123 Eigen::Matrix<Scalar,6,3,Options> result;
126 result.template middleRows<3>(ANGULAR).noalias() = m.rotation () * m_S;
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>
138 se3ActionInverse(
const SE3Tpl<S1,O1> & m)
const
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() * m_S;
154 template<
typename MotionDerived>
155 DenseBase motionAction(
const MotionDense<MotionDerived> & m)
const
157 const typename MotionDerived::ConstLinearType v = m.linear();
158 const typename MotionDerived::ConstAngularType w = m.angular();
161 cross(v,m_S,res.template middleRows<3>(LINEAR));
162 cross(w,m_S,res.template middleRows<3>(ANGULAR));
167 Matrix3 & angularSubspace() {
return m_S; }
168 const Matrix3 & angularSubspace()
const {
return m_S; }
170 bool isEqual(
const ConstraintSphericalZYXTpl & other)
const
172 return m_S == other.m_S;
181 template <
typename S1,
int O1,
typename S2,
int O2>
182 Eigen::Matrix<S1,6,3,O1>
184 const ConstraintSphericalZYXTpl<S2,O2> & S)
186 typedef typename InertiaTpl<S1,O1>::Symmetric3 Symmetric3;
187 typedef ConstraintSphericalZYXTpl<S2,O2> Constraint;
188 Eigen::Matrix<S1,6,3,O1> M;
189 alphaSkew (-Y.mass(),Y.lever(),M.template middleRows<3>(Constraint::LINEAR));
190 M.template middleRows<3>(Constraint::ANGULAR) = (Y.inertia () -
191 typename Symmetric3::AlphaSkewSquare(Y.mass (), Y.lever ())).matrix();
193 return (M * S.angularSubspace()).eval();
198 template<
typename Matrix6Like,
typename S2,
int O2>
199 const typename MatrixMatrixProduct<
200 typename Eigen::internal::remove_const<typename SizeDepType<3>::ColsReturn<Matrix6Like>::ConstType>::type,
201 typename ConstraintSphericalZYXTpl<S2,O2>::Matrix3
203 operator*(
const Eigen::MatrixBase<Matrix6Like> & Y,
204 const ConstraintSphericalZYXTpl<S2,O2> & S)
206 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like,6,6);
207 return Y.derived().template middleCols<3>(Inertia::ANGULAR) * S.angularSubspace();
210 template<
typename S1,
int O1>
217 typedef Eigen::Matrix<S1,6,3,O1> ReturnType;
220 template<
typename S1,
int O1,
typename MotionDerived>
223 typedef Eigen::Matrix<S1,6,3,O1> ReturnType;
228 template<
typename _Scalar,
int _Options>
235 typedef _Scalar Scalar;
236 enum { Options = _Options };
245 typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
246 typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
247 typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
249 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
251 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
252 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
255 template<
typename Scalar,
int Options>
259 template<
typename Scalar,
int Options>
263 template<
typename _Scalar,
int _Options>
266 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
268 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
269 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
283 : S(Constraint_t::Matrix3::Zero())
284 , M(Transformation_t::Identity())
285 , v(Motion_t::Vector3::Zero())
286 , c(Bias_t::Vector3::Zero())
289 , UDinv(UD_t::Zero())
292 static std::string classname() {
return std::string(
"JointDataSphericalZYX"); }
293 std::string shortname()
const {
return classname(); }
297 PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelSphericalZYXTpl);
298 template<
typename _Scalar,
int _Options>
299 struct JointModelSphericalZYXTpl
300 :
public JointModelBase< JointModelSphericalZYXTpl<_Scalar,_Options> >
302 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
303 typedef JointSphericalZYXTpl<_Scalar,_Options> JointDerived;
304 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
306 typedef JointModelBase<JointModelSphericalZYXTpl> Base;
310 using Base::setIndexes;
312 JointDataDerived createData()
const {
return JointDataDerived(); }
314 template<
typename ConfigVector>
315 void calc(JointDataDerived & data,
316 const typename Eigen::MatrixBase<ConfigVector> & qs)
const
318 typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type & q = qs.template segment<NQ>(
idx_q());
320 typedef typename ConfigVector::Scalar S2;
322 S2 c0,s0;
SINCOS(q(0), &s0, &c0);
323 S2 c1,s1;
SINCOS(q(1), &s1, &c1);
324 S2 c2,s2;
SINCOS(q(2), &s2, &c2);
326 data.M.rotation () << c0 * c1,
327 c0 * s1 * s2 - s0 * c2,
328 c0 * s1 * c2 + s0 * s2,
330 s0 * s1 * s2 + c0 * c2,
331 s0 * s1 * c2 - c0 * s2,
336 data.S.angularSubspace()
337 << -s1, Scalar(0), Scalar(1),
338 c1 * s2, c2, Scalar(0),
339 c1 * c2, -s2, Scalar(0);
342 template<
typename ConfigVector,
typename TangentVector>
343 void calc(JointDataDerived & data,
344 const typename Eigen::MatrixBase<ConfigVector> & qs,
345 const typename Eigen::MatrixBase<TangentVector> & vs)
const
347 typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type & q = qs.template segment<NQ>(
idx_q());
349 typedef typename ConfigVector::Scalar S2;
351 S2 c0,s0;
SINCOS(q(0), &s0, &c0);
352 S2 c1,s1;
SINCOS(q(1), &s1, &c1);
353 S2 c2,s2;
SINCOS(q(2), &s2, &c2);
355 data.M.rotation () << c0 * c1,
356 c0 * s1 * s2 - s0 * c2,
357 c0 * s1 * c2 + s0 * s2,
359 s0 * s1 * s2 + c0 * c2,
360 s0 * s1 * c2 - c0 * s2,
365 data.S.angularSubspace()
366 << -s1, Scalar(0), Scalar(1),
367 c1 * s2, c2, Scalar(0),
368 c1 * c2, -s2, Scalar(0);
370 typename TangentVector::template ConstFixedSegmentReturnType<NV>::Type
371 & q_dot = vs.template segment<NV>(
idx_v());
373 data.v().noalias() = data.S.angularSubspace() * q_dot;
375 data.c()(0) = -c1 * q_dot(0) * q_dot(1);
376 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);
377 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);
380 template<
typename Matrix6Like>
381 void calc_aba(JointDataDerived & data,
382 const Eigen::MatrixBase<Matrix6Like> & I,
383 const bool update_I)
const
385 data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * data.S.angularSubspace();
386 data.StU.noalias() = data.S.angularSubspace().transpose() * data.U.template middleRows<3>(Motion::ANGULAR);
391 internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv);
393 data.UDinv.noalias() = data.U * data.Dinv;
396 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
399 static std::string classname() {
return std::string(
"JointModelSphericalZYX"); }
400 std::string shortname()
const {
return classname(); }
403 template<
typename NewScalar>
416 #include <boost/type_traits.hpp>
420 template<
typename Scalar,
int Options>
422 :
public integral_constant<bool,true> {};
424 template<
typename Scalar,
int Options>
426 :
public integral_constant<bool,true> {};
428 template<
typename Scalar,
int Options>
430 :
public integral_constant<bool,true> {};
432 template<
typename Scalar,
int Options>
434 :
public integral_constant<bool,true> {};
437 #endif // ifndef __pinocchio_joint_spherical_ZYX_hpp__