5 #ifndef __pinocchio_multibody_joint_revolute_unbounded_unaligned_hpp__
6 #define __pinocchio_multibody_joint_revolute_unbounded_unaligned_hpp__
8 #include "pinocchio/fwd.hpp"
9 #include "pinocchio/spatial/inertia.hpp"
10 #include "pinocchio/math/rotation.hpp"
11 #include "pinocchio/math/matrix.hpp"
13 #include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp"
18 template<
typename Scalar,
int Options = 0>
21 template<
typename _Scalar,
int _Options>
30 typedef _Scalar Scalar;
36 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
37 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
45 typedef Eigen::Matrix<Scalar, 6, NV, Options> F_t;
48 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
49 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
50 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
52 typedef boost::mpl::true_ is_mimicable_t;
54 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
57 template<
typename _Scalar,
int _Options>
61 typedef _Scalar Scalar;
64 template<
typename _Scalar,
int _Options>
68 typedef _Scalar Scalar;
71 template<
typename _Scalar,
int _Options>
73 :
public JointDataBase<JointDataRevoluteUnboundedUnalignedTpl<_Scalar, _Options>>
75 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
78 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
80 ConfigVector_t joint_q;
81 TangentVector_t joint_v;
96 ,
joint_v(TangentVector_t::Zero())
97 , M(Transformation_t::Identity())
98 , S(Constraint_t::Vector3::Zero())
99 , v(Constraint_t::Vector3::Zero(), (Scalar)0)
102 , UDinv(UD_t::Zero())
107 template<
typename Vector3Like>
109 :
joint_q(Scalar(1), Scalar(0))
110 ,
joint_v(TangentVector_t::Zero())
111 , M(Transformation_t::Identity())
116 , UDinv(UD_t::Zero())
121 static std::string classname()
123 return std::string(
"JointDataRevoluteUnboundedUnalignedTpl");
125 std::string shortname()
const
134 template<
typename _Scalar,
int _Options>
136 :
public JointModelBase<JointModelRevoluteUnboundedUnalignedTpl<_Scalar, _Options>>
138 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
141 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
147 using Base::idx_vExtended;
148 using Base::setIndexes;
151 :
axis(Vector3::UnitX())
162 template<
typename Vector3Like>
166 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
170 JointDataDerived createData()
const
172 return JointDataDerived(
axis);
175 const std::vector<bool> hasConfigurationLimit()
const
177 return {
false,
false};
180 const std::vector<bool> hasConfigurationLimitInTangent()
const
188 return Base::isEqual(other) && internal::comparison_eq(
axis, other.
axis);
191 template<
typename ConfigVector>
192 void calc(JointDataDerived & data,
const typename Eigen::MatrixBase<ConfigVector> & qs)
const
194 data.joint_q = qs.template segment<NQ>(idx_q());
196 const Scalar & ca = data.joint_q(0);
197 const Scalar & sa = data.joint_q(1);
202 template<
typename TangentVector>
204 calc(JointDataDerived & data,
const Blank,
const typename Eigen::MatrixBase<TangentVector> & vs)
207 data.joint_v[0] = vs[idx_v()];
208 data.v.angularRate() = data.joint_v[0];
211 template<
typename ConfigVector,
typename TangentVector>
213 JointDataDerived & data,
214 const typename Eigen::MatrixBase<ConfigVector> & qs,
215 const typename Eigen::MatrixBase<TangentVector> & vs)
const
217 calc(data, qs.derived());
218 data.joint_v[0] = vs[idx_v()];
219 data.v.angularRate() = data.joint_v[0];
222 template<
typename VectorLike,
typename Matrix6Like>
224 JointDataDerived & data,
225 const Eigen::MatrixBase<VectorLike> & armature,
226 const Eigen::MatrixBase<Matrix6Like> & I,
227 const bool update_I)
const
229 data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) *
axis;
231 Scalar(1) / (
axis.dot(data.U.template segment<3>(Motion::ANGULAR)) + armature[0]);
232 data.UDinv.noalias() = data.U * data.Dinv;
235 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
238 static std::string classname()
240 return std::string(
"JointModelRevoluteUnboundedUnaligned");
242 std::string shortname()
const
248 template<
typename NewScalar>
252 ReturnType res(
axis.template cast<NewScalar>());
253 res.setIndexes(
id(), idx_q(), idx_v(), idx_vExtended());
264 template<
typename Scalar,
int Options>
271 #include <boost/type_traits.hpp>
275 template<
typename Scalar,
int Options>
276 struct has_nothrow_constructor<
278 :
public integral_constant<bool, true>
282 template<
typename Scalar,
int Options>
284 :
public integral_constant<bool, true>
288 template<
typename Scalar,
int Options>
289 struct has_nothrow_constructor<
291 :
public integral_constant<bool, true>
295 template<
typename Scalar,
int Options>
297 :
public integral_constant<bool, true>
Main pinocchio namespace.
bool isUnitary(const Eigen::MatrixBase< MatrixLike > &mat, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Check whether the input matrix is Unitary within the given precision.
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.
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
void toRotationMatrix(const Eigen::MatrixBase< Vector3 > &axis, const Scalar &cos_value, const Scalar &sin_value, const Eigen::MatrixBase< Matrix3 > &res)
Computes a rotation matrix from a vector and values of sin and cos orientations values.
JointModelRevoluteUnboundedUnalignedTpl< NewScalar, Options > cast() const
Vector3 axis
axis of rotation of the joint.
Common traits structure to fully define base classes for CRTP.