5 #ifndef __pinocchio_joint_revolute_unbounded_unaligned_hpp__
6 #define __pinocchio_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"
20 template<
typename _Scalar,
int _Options>
27 typedef _Scalar Scalar;
28 enum { Options = _Options };
30 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
31 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
39 typedef Eigen::Matrix<Scalar,6,NV,Options> F_t;
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 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
49 template<
typename Scalar,
int Options>
53 template<
typename Scalar,
int Options>
57 template<
typename _Scalar,
int _Options>
59 :
public JointDataBase< JointDataRevoluteUnboundedUnalignedTpl<_Scalar,_Options> >
61 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
64 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
77 : M(Transformation_t::Identity())
78 , S(Constraint_t::Vector3::Zero())
79 , v(Constraint_t::Vector3::Zero(),(Scalar)0)
85 template<
typename Vector3Like>
86 JointDataRevoluteUnboundedUnalignedTpl(
const Eigen::MatrixBase<Vector3Like> & axis)
87 : M(Transformation_t::Identity())
95 static std::string classname() {
return std::string(
"JointDataRevoluteUnboundedUnalignedTpl"); }
96 std::string shortname()
const {
return classname(); }
100 PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelRevoluteUnboundedUnalignedTpl);
102 template<
typename _Scalar,
int _Options>
103 struct JointModelRevoluteUnboundedUnalignedTpl
104 :
public JointModelBase< JointModelRevoluteUnboundedUnalignedTpl<_Scalar,_Options> >
106 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
107 typedef JointRevoluteUnboundedUnalignedTpl<_Scalar,_Options> JointDerived;
108 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
109 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
111 typedef JointModelBase<JointModelRevoluteUnboundedUnalignedTpl> Base;
115 using Base::setIndexes;
117 JointModelRevoluteUnboundedUnalignedTpl():
axis(Vector3::UnitX()) {}
119 JointModelRevoluteUnboundedUnalignedTpl(
const Scalar & x,
128 template<
typename Vector3Like>
129 JointModelRevoluteUnboundedUnalignedTpl(
const Eigen::MatrixBase<Vector3Like> &
axis)
132 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
136 JointDataDerived createData()
const {
return JointDataDerived(
axis); }
138 const std::vector<bool> hasConfigurationLimit()
const
140 return {
false,
false};
143 const std::vector<bool> hasConfigurationLimitInTangent()
const
149 bool isEqual(
const JointModelRevoluteUnboundedUnalignedTpl & other)
const
151 return Base::isEqual(other) &&
axis == other.axis;
154 template<
typename ConfigVector>
155 void calc(JointDataDerived & data,
156 const typename Eigen::MatrixBase<ConfigVector> & qs)
const
158 typedef typename ConfigVector::Scalar OtherScalar;
159 typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type
160 & q = qs.template segment<NQ>(
idx_q());
162 const OtherScalar & ca = q(0);
163 const OtherScalar & sa = q(1);
168 template<
typename ConfigVector,
typename TangentVector>
169 void calc(JointDataDerived & data,
170 const typename Eigen::MatrixBase<ConfigVector> & qs,
171 const typename Eigen::MatrixBase<TangentVector> & vs)
const
173 calc(data,qs.derived());
174 data.v.angularRate() =
static_cast<Scalar
>(vs[
idx_v()]);
177 template<
typename Matrix6Like>
178 void calc_aba(JointDataDerived & data,
179 const Eigen::MatrixBase<Matrix6Like> & I,
180 const bool update_I)
const
182 data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) *
axis;
183 data.Dinv[0] = (Scalar)(1)/
axis.dot(data.U.template segment<3>(Motion::ANGULAR));
184 data.UDinv.noalias() = data.U * data.Dinv;
187 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
190 static std::string classname() {
return std::string(
"JointModelRevoluteUnboundedUnaligned"); }
191 std::string shortname()
const {
return classname(); }
194 template<
typename NewScalar>
198 ReturnType res(
axis.template cast<NewScalar>());
212 #include <boost/type_traits.hpp>
216 template<
typename Scalar,
int Options>
218 :
public integral_constant<bool,true> {};
220 template<
typename Scalar,
int Options>
222 :
public integral_constant<bool,true> {};
224 template<
typename Scalar,
int Options>
226 :
public integral_constant<bool,true> {};
228 template<
typename Scalar,
int Options>
230 :
public integral_constant<bool,true> {};
234 #endif // ifndef __pinocchio_joint_revolute_unbounded_unaligned_hpp__