5 #ifndef __pinocchio_multibody_joint_revolute_unbounded_hpp__
6 #define __pinocchio_multibody_joint_revolute_unbounded_hpp__
8 #include "pinocchio/math/fwd.hpp"
9 #include "pinocchio/math/sincos.hpp"
10 #include "pinocchio/spatial/inertia.hpp"
11 #include "pinocchio/multibody/joint/joint-base.hpp"
12 #include "pinocchio/multibody/joint/joint-revolute.hpp"
17 template<
typename Scalar,
int Options,
int axis>
20 template<
typename _Scalar,
int _Options,
int axis>
29 typedef _Scalar Scalar;
42 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
43 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
44 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
46 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
47 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
49 typedef boost::mpl::true_ is_mimicable_t;
51 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
54 template<
typename _Scalar,
int _Options,
int axis>
58 typedef _Scalar Scalar;
61 template<
typename _Scalar,
int _Options,
int axis>
65 typedef _Scalar Scalar;
68 template<
typename _Scalar,
int _Options,
int axis>
70 :
public JointDataBase<JointDataRevoluteUnboundedTpl<_Scalar, _Options, axis>>
72 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
75 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
77 ConfigVector_t joint_q;
78 TangentVector_t joint_v;
93 ,
joint_v(TangentVector_t::Zero())
94 , M((Scalar)0, (Scalar)1)
103 static std::string classname()
105 return std::string(
"JointDataRUB") + axisLabel<axis>();
107 std::string shortname()
const
114 template<
typename NewScalar,
typename Scalar,
int Options,
int axis>
120 template<
typename _Scalar,
int _Options,
int axis>
122 :
public JointModelBase<JointModelRevoluteUnboundedTpl<_Scalar, _Options, axis>>
124 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
133 using Base::idx_vExtended;
134 using Base::setIndexes;
136 typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
138 JointDataDerived createData()
const
140 return JointDataDerived();
143 const std::vector<bool> hasConfigurationLimit()
const
145 return {
false,
false};
148 const std::vector<bool> hasConfigurationLimitInTangent()
const
153 template<
typename ConfigVector>
154 void calc(JointDataDerived & data,
const typename Eigen::MatrixBase<ConfigVector> & qs)
const
156 data.joint_q = qs.template segment<NQ>(idx_q());
158 const Scalar & ca = data.joint_q[0];
159 const Scalar & sa = data.joint_q[1];
161 data.M.setValues(sa, ca);
164 template<
typename TangentVector>
166 calc(JointDataDerived & data,
const Blank,
const typename Eigen::MatrixBase<TangentVector> & vs)
169 data.joint_v[0] = vs[idx_v()];
170 data.v.angularRate() = data.joint_v[0];
173 template<
typename ConfigVector,
typename TangentVector>
175 JointDataDerived & data,
176 const typename Eigen::MatrixBase<ConfigVector> & qs,
177 const typename Eigen::MatrixBase<TangentVector> & vs)
const
179 calc(data, qs.derived());
180 data.joint_v[0] = vs[idx_v()];
181 data.v.angularRate() = data.joint_v[0];
184 template<
typename VectorLike,
typename Matrix6Like>
186 JointDataDerived & data,
187 const Eigen::MatrixBase<VectorLike> & armature,
188 const Eigen::MatrixBase<Matrix6Like> & I,
189 const bool update_I)
const
191 data.U = I.col(Inertia::ANGULAR + axis);
193 (Scalar)(1) / (I(Inertia::ANGULAR + axis, Inertia::ANGULAR + axis) + armature[0]);
194 data.UDinv.noalias() = data.U * data.Dinv[0];
197 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
200 static std::string classname()
202 return std::string(
"JointModelRUB") + axisLabel<axis>();
204 std::string shortname()
const
209 Vector3 getMotionAxis()
const
214 return Vector3::UnitX();
216 return Vector3::UnitY();
218 return Vector3::UnitZ();
220 assert(
false &&
"must never happen");
226 template<
typename NewScalar>
231 res.setIndexes(
id(), idx_q(), idx_v(), idx_vExtended());
237 typedef JointRevoluteUnboundedTpl<context::Scalar, context::Options, 0> JointRUBX;
238 typedef JointDataRevoluteUnboundedTpl<context::Scalar, context::Options, 0> JointDataRUBX;
239 typedef JointModelRevoluteUnboundedTpl<context::Scalar, context::Options, 0> JointModelRUBX;
241 typedef JointRevoluteUnboundedTpl<context::Scalar, context::Options, 1> JointRUBY;
242 typedef JointDataRevoluteUnboundedTpl<context::Scalar, context::Options, 1> JointDataRUBY;
243 typedef JointModelRevoluteUnboundedTpl<context::Scalar, context::Options, 1> JointModelRUBY;
245 typedef JointRevoluteUnboundedTpl<context::Scalar, context::Options, 2> JointRUBZ;
246 typedef JointDataRevoluteUnboundedTpl<context::Scalar, context::Options, 2> JointDataRUBZ;
247 typedef JointModelRevoluteUnboundedTpl<context::Scalar, context::Options, 2> JointModelRUBZ;
249 template<
typename Scalar,
int Options,
int axis>
256 #include <boost/type_traits.hpp>
260 template<
typename Scalar,
int Options,
int axis>
262 :
public integral_constant<bool, true>
266 template<
typename Scalar,
int Options,
int axis>
268 :
public integral_constant<bool, true>
272 template<
typename Scalar,
int Options,
int axis>
274 :
public integral_constant<bool, true>
278 template<
typename Scalar,
int Options,
int axis>
280 :
public integral_constant<bool, true>
Main pinocchio namespace.
JointDataTpl< Scalar, Options, JointCollectionTpl >::TangentVector_t joint_v(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConfigVisitor to get the joint velocity vector.
JointDataTpl< Scalar, Options, JointCollectionTpl >::ConfigVector_t joint_q(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConfigVisitor to get the joint configuration vector.
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
JointModelRevoluteUnboundedTpl< NewScalar, Options, axis > cast() const
Common traits structure to fully define base classes for CRTP.