6 #ifndef __pinocchio_joint_spherical_ZYX_hpp__
7 #define __pinocchio_joint_spherical_ZYX_hpp__
22 template <
typename _Scalar,
int _Options>
35 typedef Eigen::Matrix<Scalar,6,3,Options>
DenseBase;
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);
63 return JointMotion(
m_S *
v);
76 template<
typename Derived>
78 Eigen::Transpose<const Matrix3>,
79 Eigen::Block<const typename ForceDense<Derived>::Vector6,3,1>
89 typename Eigen::Transpose<const Matrix3>,
90 typename Eigen::MatrixBase<const D>::template NRowsBlockXpr<3>::Type
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);
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>
123 Eigen::Matrix<Scalar,6,3,Options> result;
126 result.template middleRows<3>(ANGULAR).noalias() =
m.rotation () *
m_S;
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;
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>
157 const typename MotionDerived::ConstLinearType
v =
m.linear();
158 const typename MotionDerived::ConstAngularType
w =
m.angular();
181 template <
typename S1,
int O1,
typename S2,
int O2>
182 Eigen::Matrix<S1,6,3,O1>
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();
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,
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>
220 template<
typename S1,
int O1,
typename MotionDerived>
228 template<
typename _Scalar,
int _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;
255 template<
typename Scalar,
int Options>
259 template<
typename Scalar,
int Options>
263 template<
typename _Scalar,
int _Options>
266 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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"); }
298 template<
typename _Scalar,
int _Options>
299 struct JointModelSphericalZYXTpl
300 :
public JointModelBase< JointModelSphericalZYXTpl<_Scalar,_Options> >
302 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
312 JointDataDerived
createData()
const {
return JointDataDerived(); }
316 return {
true,
true,
true};
321 return {
true,
true,
true};
324 template<
typename ConfigVector>
326 const typename Eigen::MatrixBase<ConfigVector> &
qs)
const
328 typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type &
q =
qs.template segment<NQ>(
idx_q());
332 S2 c0,s0;
SINCOS(
q(0), &s0, &c0);
333 S2 c1,s1;
SINCOS(
q(1), &s1, &c1);
336 data.M.rotation () << c0 * c1,
337 c0 * s1 * s2 - s0 *
c2,
338 c0 * s1 *
c2 + s0 * s2,
340 s0 * s1 * s2 + c0 *
c2,
341 s0 * s1 *
c2 - c0 * s2,
346 data.S.angularSubspace()
352 template<
typename ConfigVector,
typename TangentVector>
354 const typename Eigen::MatrixBase<ConfigVector> &
qs,
355 const typename Eigen::MatrixBase<TangentVector> & vs)
const
357 typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type &
q =
qs.template segment<NQ>(
idx_q());
361 S2 c0,s0;
SINCOS(
q(0), &s0, &c0);
362 S2 c1,s1;
SINCOS(
q(1), &s1, &c1);
365 data.M.rotation () << c0 * c1,
366 c0 * s1 * s2 - s0 *
c2,
367 c0 * s1 *
c2 + s0 * s2,
369 s0 * s1 * s2 + c0 *
c2,
370 s0 * s1 *
c2 - c0 * s2,
375 data.S.angularSubspace()
380 typename TangentVector::template ConstFixedSegmentReturnType<NV>::Type
381 & q_dot = vs.template segment<NV>(
idx_v());
383 data.v().noalias() =
data.S.angularSubspace() * q_dot;
385 data.c()(0) = -c1 * q_dot(0) * q_dot(1);
386 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);
387 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);
390 template<
typename Matrix6Like>
392 const Eigen::MatrixBase<Matrix6Like> & I,
393 const bool update_I)
const
395 data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) *
data.S.angularSubspace();
396 data.StU.noalias() =
data.S.angularSubspace().transpose() *
data.U.template middleRows<3>(Motion::ANGULAR);
409 static std::string
classname() {
return std::string(
"JointModelSphericalZYX"); }
413 template<
typename NewScalar>
426 #include <boost/type_traits.hpp>
430 template<
typename Scalar,
int Options>
432 :
public integral_constant<bool,true> {};
434 template<
typename Scalar,
int Options>
436 :
public integral_constant<bool,true> {};
438 template<
typename Scalar,
int Options>
440 :
public integral_constant<bool,true> {};
442 template<
typename Scalar,
int Options>
444 :
public integral_constant<bool,true> {};
447 #endif // ifndef __pinocchio_joint_spherical_ZYX_hpp__