Go to the documentation of this file.
6 #ifndef __pinocchio_joint_free_flyer_hpp__
7 #define __pinocchio_joint_free_flyer_hpp__
22 template<
typename _Scalar,
int _Options>
27 typedef Eigen::Matrix<Scalar,6,6,Options>
Matrix6;
34 typedef Eigen::Matrix<Scalar,6,6,Options>
DenseBase;
40 template<
typename _Scalar,
int _Options>
44 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
49 template<
typename Vector6Like>
50 JointMotion
__mult__(
const Eigen::MatrixBase<Vector6Like> & vj)
const
52 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like,6);
53 return JointMotion(vj);
56 template<
typename S1,
int O1>
60 return m.toActionMatrix();
64 template<
typename S1,
int O1>
68 return m.toActionMatrixInverse();
75 template<
typename Derived>
81 template<
typename MatrixDerived>
83 operator*(
const Eigen::MatrixBase<MatrixDerived> & F)
90 MatrixReturnType
matrix_impl()
const {
return DenseBase::Identity(); }
92 template<
typename MotionDerived>
93 typename MotionDerived::ActionMatrixType
95 {
return v.toActionMatrix(); }
101 template<
typename Scalar,
int Options,
typename Vector6Like>
102 MotionRef<const Vector6Like>
104 const Eigen::MatrixBase<Vector6Like> &
v)
106 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like,6);
113 template<
typename S1,
int O1,
typename S2,
int O2>
114 inline typename InertiaTpl<S1,O1>::Matrix6
121 template<
typename Matrix6Like,
typename S2,
int O2>
128 template<
typename S1,
int O1>
132 template<
typename S1,
int O1,
typename MotionDerived>
138 template<
typename _Scalar,
int _Options>
155 typedef Eigen::Matrix<Scalar,6,NV,Options>
U_t;
156 typedef Eigen::Matrix<Scalar,NV,NV,Options>
D_t;
157 typedef Eigen::Matrix<Scalar,6,NV,Options>
UD_t;
165 template<
typename Scalar,
int Options>
169 template<
typename Scalar,
int Options>
173 template<
typename _Scalar,
int _Options>
176 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
192 :
M(Transformation_t::Identity())
193 ,
v(Motion_t::Zero())
196 ,
UDinv(UD_t::Identity())
199 static std::string
classname() {
return std::string(
"JointDataFreeFlyer"); }
205 template<
typename _Scalar,
int _Options>
206 struct JointModelFreeFlyerTpl
207 :
public JointModelBase< JointModelFreeFlyerTpl<_Scalar,_Options> >
209 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
219 JointDataDerived
createData()
const {
return JointDataDerived(); }
223 return {
true,
true,
true,
false,
false,
false,
false};
228 return {
true,
true,
true,
false,
false,
false};
231 template<
typename ConfigVectorLike>
232 inline void forwardKinematics(Transformation_t & M,
const Eigen::MatrixBase<ConfigVectorLike> & q_joint)
const
234 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike);
236 typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
238 ConstQuaternionMap
quat(q_joint.template tail<4>().data());
240 assert(math::fabs(
static_cast<Scalar>(
quat.coeffs().squaredNorm()-1)) <= 1e-4);
242 M.rotation(
quat.matrix());
243 M.translation(q_joint.template head<3>());
246 template<
typename Vector3Derived,
typename QuaternionDerived>
249 const typename Eigen::MatrixBase<Vector3Derived> & trans,
250 const typename Eigen::QuaternionBase<QuaternionDerived> & quat)
const
252 data.M.translation(trans);
256 template<
typename ConfigVector>
259 const typename Eigen::PlainObjectBase<ConfigVector> &
qs)
const
261 typedef typename Eigen::Quaternion<typename ConfigVector::Scalar,ConfigVector::Options> Quaternion;
262 typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
264 typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type
q =
qs.template segment<NQ>(
idx_q());
265 ConstQuaternionMap
quat(
q.template tail<4>().data());
270 template<
typename ConfigVector>
273 const typename Eigen::MatrixBase<ConfigVector> &
qs)
const
275 typedef typename Eigen::Quaternion<Scalar,Options> Quaternion;
277 typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type
q =
qs.template segment<NQ>(
idx_q());
278 const Quaternion
quat(
q.template tail<4>());
283 template<
typename ConfigVector,
typename TangentVector>
286 const typename Eigen::MatrixBase<ConfigVector> &
qs,
287 const typename Eigen::MatrixBase<TangentVector> & vs)
const
291 data.v = vs.template segment<NV>(
idx_v());
294 template<
typename Matrix6Like>
296 const Eigen::MatrixBase<Matrix6Like> & I,
297 const bool update_I)
const
310 static std::string
classname() {
return std::string(
"JointModelFreeFlyer"); }
314 template<
typename NewScalar>
327 #include <boost/type_traits.hpp>
331 template<
typename Scalar,
int Options>
333 :
public integral_constant<bool,true> {};
335 template<
typename Scalar,
int Options>
337 :
public integral_constant<bool,true> {};
339 template<
typename Scalar,
int Options>
341 :
public integral_constant<bool,true> {};
343 template<
typename Scalar,
int Options>
345 :
public integral_constant<bool,true> {};
348 #endif // ifndef __pinocchio_joint_free_flyer_hpp__
JointFreeFlyerTpl< Scalar, Options > JointDerived
std::string shortname() const
MotionTpl< Scalar, Options > JointMotion
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
JointModelBase< JointModelFreeFlyerTpl > Base
JointModelFreeFlyerTpl< NewScalar, Options > cast() const
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)
 
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
SE3Tpl< S1, O1 >::ActionMatrixType ReturnType
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...
SE3Tpl< Scalar, Options > Transformation_t
Eigen::Matrix< Scalar, NV, NV, Options > D_t
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
void forwardKinematics(Transformation_t &M, const Eigen::MatrixBase< ConfigVectorLike > &q_joint) const
PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(const Eigen
MatrixReturnType matrix_impl() const
Eigen::Matrix< Scalar, 6, NV, Options > UD_t
void setIndexes(JointIndex id, int q, int v)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointFreeFlyerTpl< _Scalar, _Options > JointDerived
Return type of the ation of a Motion onto an object of type D.
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs) const
TransposeConst transpose() const
#define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
static std::string classname()
PINOCCHIO_EIGEN_REF_CONST_TYPE(MatrixDerived) operator*(const Eigen
Matrix6::IdentityReturnType ConstMatrixReturnType
bool isEqual(const ConstraintIdentityTpl &) const
Matrix6::IdentityReturnType MatrixReturnType
MotionZeroTpl< Scalar, Options > Bias_t
Eigen::Matrix< Scalar, 6, 1, Options > JointForce
SE3Tpl< S1, O1 >::ActionMatrixType se3Action(const SE3Tpl< S1, O1 > &m) const
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl)
JointFreeFlyerTpl< Scalar, Options > JointDerived
MotionDerived::ActionMatrixType motionAction(const MotionBase< MotionDerived > &v) const
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived)
SE3Tpl< S1, O1 >::ActionMatrixType ReturnType
const std::vector< bool > hasConfigurationLimitInTangent() const
MotionTpl< double, 0 > Motion
SE3Tpl< S1, O1 >::ActionMatrixType se3ActionInverse(const SE3Tpl< S1, O1 > &m) const
MotionTpl< Scalar, Options > Motion_t
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointFreeFlyerTpl< _Scalar, _Options > JointDerived
Eigen::Matrix< Scalar, 6, 6, Options > Matrix6
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::PlainObjectBase< ConfigVector > &qs) const
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
std::string shortname() const
Eigen::Matrix< Scalar, 6, 6, Options > DenseBase
JointModelFreeFlyerTpl< Scalar, Options > JointModelDerived
Eigen::Matrix< Scalar, NV, 1, Options > TangentVector_t
ToVectorConstReturnType toVector() const
Return the force as an Eigen vector.
static std::string classname()
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived)
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR Constraint_t S
JointDataFreeFlyerTpl< Scalar, Options > JointDataDerived
const std::vector< bool > hasConfigurationLimit() const
Common traits structure to fully define base classes for CRTP.
Eigen::Matrix< Scalar, 6, NV, Options > U_t
ConstraintIdentityTpl< Scalar, Options > Constraint_t
PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(const ModelTpl< Scalar
Integrate a configuration vector for the specified model for a tangent vector during one unit time.
JointMotion __mult__(const Eigen::MatrixBase< Vector6Like > &vj) const
ForceDense< Derived >::ToVectorConstReturnType operator*(const ForceDense< Derived > &phi)
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix< Scalar, NQ, 1, Options > ConfigVector_t
JointDataDerived createData() const
Main pinocchio namespace.
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::MatrixBase< Vector3Derived > &trans, const typename Eigen::QuaternionBase< QuaternionDerived > &quat) const
pinocchio
Author(s):
autogenerated on Tue Feb 13 2024 03:43:58