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>
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;
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
77 : M(Transformation_t::Identity())
78 , S(Constraint_t::Vector3::Zero())
79 , v(Constraint_t::Vector3::Zero(),(
Scalar)0)
85 template<
typename Vector3Like>
87 : M(Transformation_t::Identity())
95 static std::string
classname() {
return std::string(
"JointDataRevoluteUnboundedUnalignedTpl"); }
96 std::string
shortname()
const {
return classname(); }
102 template<
typename _Scalar,
int _Options>
104 :
public JointModelBase< JointModelRevoluteUnboundedUnalignedTpl<_Scalar,_Options> >
106 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
109 typedef Eigen::Matrix<Scalar,3,1,Options>
Vector3;
128 template<
typename Vector3Like>
132 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
133 assert(
isUnitary(axis) &&
"Rotation axis is not unitary");
140 return {
false,
false};
154 template<
typename ConfigVector>
156 const typename Eigen::MatrixBase<ConfigVector> &
qs)
const 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>
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>
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;
190 static std::string
classname() {
return std::string(
"JointModelRevoluteUnboundedUnaligned"); }
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__ JointModelRevoluteUnboundedUnalignedTpl< NewScalar, Options > cast() const
static std::string classname()
JointModelRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis)
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
int idx_v(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxVVisitor to get the index in the full model tangent space corre...
Eigen::Matrix< Scalar, NV, NV, Options > D_t
JointModelBase< JointModelRevoluteUnboundedUnalignedTpl > Base
JointModelRevoluteUnboundedUnalignedTpl(const Scalar &x, const Scalar &y, const Scalar &z)
MotionRevoluteUnalignedTpl< Scalar, Options > Motion_t
std::string shortname() const
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR Transformation_t M
bool isEqual(const JointModelRevoluteUnboundedUnalignedTpl &other) const
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain...
#define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
JointDataRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis)
ConstraintRevoluteUnalignedTpl< Scalar, Options > Constraint_t
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
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.
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
virtual bool isEqual(const CollisionGeometry &other) const=0
Eigen::Matrix< Scalar, NV, 1, Options > TangentVector_t
JointModelRevoluteUnboundedUnalignedTpl< Scalar, Options > JointModelDerived
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
JointDataRevoluteUnboundedUnalignedTpl()
MotionZeroTpl< Scalar, Options > Bias_t
JointDataRevoluteUnboundedUnalignedTpl< Scalar, Options > JointDataDerived
#define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteUnboundedUnalignedTpl< _Scalar, _Options > JointDerived
JointDataDerived createData() const
std::string shortname() const
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Eigen::Matrix< Scalar, 6, NV, Options > UD_t
static std::string classname()
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl)
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
Vector3 axis
3d main axis of the joint.
Main pinocchio namespace.
void setIndexes(JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointIndex id, int q, int v)
Visit a JointModelTpl through JointSetIndexesVisitor to set the indexes of the joint in the kinematic...
JointRevoluteUnboundedUnalignedTpl< Scalar, Options > JointDerived
Eigen::Matrix< Scalar, NQ, 1, Options > ConfigVector_t
const std::vector< bool > hasConfigurationLimit() const
Eigen::Matrix< Scalar, 6, NV, Options > U_t
Common traits structure to fully define base classes for CRTP.
Eigen::Matrix< Scalar, 6, NV, Options > F_t
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.
JointRevoluteUnboundedUnalignedTpl< Scalar, Options > JointDerived
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteUnboundedUnalignedTpl< _Scalar, _Options > JointDerived
const std::vector< bool > hasConfigurationLimitInTangent() const
JointModelRevoluteUnboundedUnalignedTpl()
SE3Tpl< Scalar, Options > Transformation_t
#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint)