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
73 :
public JointDataBase<JointDataRevoluteUnboundedUnalignedTpl<_Scalar, _Options>>
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
136 :
public JointModelBase<JointModelRevoluteUnboundedUnalignedTpl<_Scalar, _Options>>
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>
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
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,
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>
253 res.setIndexes(
id(), idx_q(), idx_v(), idx_vExtended());
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.
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
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.
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.