Go to the documentation of this file.
5 #ifndef __pinocchio_multibody_joint_composite_hpp__
6 #define __pinocchio_multibody_joint_composite_hpp__
19 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
22 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
43 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options>
U_t;
44 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options>
D_t;
45 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options>
UD_t;
53 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
60 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
67 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
69 :
public JointDataBase<JointDataCompositeTpl<_Scalar, _Options, JointCollectionTpl>>
71 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
89 ,
joint_q(ConfigVector_t::Zero(0))
90 ,
joint_v(TangentVector_t::Zero(0))
92 ,
M(Transformation_t::Identity())
104 , iMlast(joint_data.
size())
105 , pjMi(joint_data.
size())
108 ,
S(Constraint_t::Zero(
nv))
109 ,
M(Transformation_t::Identity())
110 ,
v(Motion_t::Zero())
111 ,
c(Motion_t::Zero())
112 ,
U(U_t::Zero(6,
nv))
144 return std::string(
"JointDataComposite");
152 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
159 os <<
"JointDataComposite containing following models:\n";
160 for (
typename JointDataVector::const_iterator it = jdata.
joints.begin();
161 it != jdata.
joints.end(); ++it)
162 os <<
" " <<
shortname(*it) << std::endl;
170 template<
typename S,
int O>
171 class JointCollectionTpl>
177 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
179 :
public JointModelBase<JointModelCompositeTpl<_Scalar, _Options, JointCollectionTpl>>
181 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
222 jointPlacements.reserve(
size);
235 template<
typename Jo
intModel>
258 , jointPlacements(other.jointPlacements)
277 template<
typename Jo
intModel>
295 typename JointDataDerived::JointDataVector jdata(
joints.size());
296 for (
int i = 0;
i < (int)
joints.size(); ++
i)
298 ::pinocchio::createData<Scalar, Options, JointCollectionTpl>(
joints[(
size_t)
i]);
299 return JointDataDerived(jdata,
nq(),
nv());
304 std::vector<bool>
vec;
305 for (
size_t i = 0;
i <
joints.size(); ++
i)
307 const std::vector<bool> & joint_cf_limit =
joints[
i].hasConfigurationLimit();
308 vec.insert(
vec.end(), joint_cf_limit.begin(), joint_cf_limit.end());
315 std::vector<bool>
vec;
316 for (
size_t i = 0;
i <
joints.size(); ++
i)
318 const std::vector<bool> & joint_cf_limit =
joints[
i].hasConfigurationLimitInTangent();
319 vec.insert(
vec.end(), joint_cf_limit.begin(), joint_cf_limit.end());
324 template<
typename,
int,
template<
typename S,
int O>
class,
typename>
327 template<
typename ConfigVectorType>
328 void calc(JointDataDerived &
data,
const Eigen::MatrixBase<ConfigVectorType> &
qs)
const;
330 template<
typename,
int,
template<
typename S,
int O>
class,
typename,
typename>
333 template<
typename ConfigVectorType,
typename TangentVectorType>
335 JointDataDerived &
data,
336 const Eigen::MatrixBase<ConfigVectorType> &
qs,
337 const Eigen::MatrixBase<TangentVectorType> & vs)
const;
339 template<
typename TangentVectorType>
341 JointDataDerived &
data,
343 const Eigen::MatrixBase<TangentVectorType> & vs)
const;
345 template<
typename VectorLike,
typename Matrix6Like>
347 JointDataDerived &
data,
348 const Eigen::MatrixBase<VectorLike> & armature,
349 const Eigen::MatrixBase<Matrix6Like> & I,
350 const bool update_I)
const
352 data.U.noalias() = I *
data.S.matrix();
353 data.StU.noalias() =
data.S.matrix().transpose() *
data.U;
354 data.StU.diagonal() += armature;
377 Base::setIndexes_impl(
id,
q,
v);
383 return std::string(
"JointModelComposite");
400 jointPlacements = other.jointPlacements;
421 template<
typename NewScalar>
436 res.jointPlacements.resize(jointPlacements.size());
437 for (
size_t k = 0; k < jointPlacements.size(); ++k)
439 res.joints[k] =
joints[k].template cast<NewScalar>();
440 res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
456 return a.segment(Base::i_q,
nq());
462 return a.segment(Base::i_q,
nq());
469 return a.segment(Base::i_v,
nv());
475 return a.segment(Base::i_v,
nv());
482 return A.middleCols(Base::i_v,
nv());
487 return A.middleCols(Base::i_v,
nv());
494 return a.segment(Base::i_q,
nq());
500 return a.segment(Base::i_q,
nq());
506 return a.segment(Base::i_v,
nv());
512 return a.segment(Base::i_v,
nv());
519 return A.middleCols(Base::i_v,
nv());
525 return A.middleCols(Base::i_v,
nv());
531 template<
typename,
int,
template<
typename,
int>
class>
546 for (
size_t i = 0;
i <
joints.size(); ++
i)
579 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
586 os <<
"JointModelComposite containing following models:\n";
587 for (
typename JointModelVector::const_iterator it = jmodel.
joints.begin();
588 it != jmodel.
joints.end(); ++it)
589 os <<
" " <<
shortname(*it) << std::endl;
596 #include <boost/type_traits.hpp>
600 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
601 struct has_nothrow_constructor<
603 :
public integral_constant<bool, true>
607 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
609 :
public integral_constant<bool, true>
613 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
614 struct has_nothrow_constructor<
616 :
public integral_constant<bool, true>
620 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
622 :
public integral_constant<bool, true>
630 #include "pinocchio/multibody/joint/joint-composite.hxx"
632 #endif // ifndef __pinocchio_multibody_joint_composite_hpp__
JointDataDerived createData() const
JointDataCompositeTpl(const JointDataVector &joint_data, const int nq, const int nv)
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
void setIndexes_impl(JointIndex id, int q, int v)
Update the indexes of subjoints in the stack.
JointCollectionTpl< Scalar, Options > JointCollection
SizeDepType< Eigen::Dynamic >::template SegmentReturn< D >::ConstType jointConfigSelector_impl(const Eigen::MatrixBase< D > &a) const
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > ConfigVector_t
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
JointModelCompositeTpl(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Constructor with one joint.
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...
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space.
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_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space.
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > D_t
JointModelCompositeTpl< NewScalar, Options, JointCollectionTpl > type
JointDataVector joints
Vector of joints.
friend struct JointCompositeCalcZeroOrderStep
JointModelCompositeTpl(const JointModelCompositeTpl &other)
Copy constructor.
std::vector< int > m_idx_q
Keep information of both the dimension and the position of the joints in the composition.
JointCompositeTpl< _Scalar, _Options, JointCollectionTpl > JointDerived
JointModelCompositeTpl & operator=(const JointModelCompositeTpl &other)
SizeDepType< NV >::template ColsReturn< D >::Type jointCols(Eigen::MatrixBase< D > &A) const
SizeDepType< Eigen::Dynamic >::template SegmentReturn< D >::ConstType jointVelocitySelector_impl(const Eigen::MatrixBase< D > &a) const
JointCompositeTpl< _Scalar, _Options, JointCollectionTpl > JointDerived
JointModelDerived & addJoint(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Add a joint to the vector of joints.
#define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
static std::string classname()
void updateJointIndexes()
Update the indexes of the joints contained in the composition according to the position of the joint ...
SizeDepType< Eigen::Dynamic >::template ColsReturn< D >::ConstType jointCols_impl(const Eigen::MatrixBase< D > &A) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointModelBase< JointModelCompositeTpl > Base
SizeDepType< NQ >::template SegmentReturn< D >::Type jointConfigSelector(Eigen::MatrixBase< D > &a) const
MotionTpl< Scalar, Options > Motion
SizeDepType< Eigen::Dynamic >::template SegmentReturn< D >::Type jointVelocitySelector_impl(Eigen::MatrixBase< D > &a) const
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > UD_t
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived)
JointModelVector joints
Vector of joints contained in the joint composite.
friend struct JointCompositeCalcFirstOrderStep
SizeDepType< NV >::template SegmentReturn< D >::Type jointVelocitySelector(Eigen::MatrixBase< D > &a) const
const MotionDense< Motion2 > const SE3Tpl< SE3Scalar, SE3Options > & placement
JointCompositeTpl< _Scalar, _Options, JointCollectionTpl > JointDerived
InertiaTpl< Scalar, Options > Inertia
bool isEqual(const JointModelCompositeTpl &other) const
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > TangentVector_t
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model.
JointModelCompositeTpl< NewScalar, Options, JointCollectionTpl > cast() const
JointDataTpl< Scalar, Options, JointCollectionTpl > JointDataVariant
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived)
MotionTpl< Scalar, Options > Bias_t
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...
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointDataBase< JointDataCompositeTpl > Base
SizeDepType< NV >::template ColsReturn< D >::ConstType jointCols(const Eigen::MatrixBase< D > &A) const
JointDataCompositeTpl< Scalar, Options, JointCollectionTpl > JointDataDerived
int njoints
Number of joints contained in the JointModelComposite.
SizeDepType< NQ >::template SegmentReturn< D >::ConstType jointConfigSelector(const Eigen::MatrixBase< D > &a) const
std::ostream & operator<<(std::ostream &os, const FrameTpl< Scalar, Options > &f)
static std::string classname()
int m_nq
Dimensions of the config and tangent space of the composite joint.
void calc(JointDataDerived &data, const Eigen::MatrixBase< ConfigVectorType > &qs) const
std::string shortname() const
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
bool comparison_eq(const LhsType &lhs_value, const RhsType &rhs_value)
std::string shortname() const
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > U_t
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModelVariant) JointModelVector
JointModelTpl< Scalar, Options, JointCollectionTpl > JointModelVariant
MotionTpl< Scalar, Options > Motion_t
const std::vector< bool > hasConfigurationLimitInTangent() const
const std::vector< bool > hasConfigurationLimit() const
SizeDepType< Eigen::Dynamic >::template SegmentReturn< D >::Type jointConfigSelector_impl(Eigen::MatrixBase< D > &a) const
std::vector< int > m_nvs
Dimension of the segment in the tangent vector.
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< VectorLike > &armature, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointDataVariant) JointDataVector
JointModelCompositeTpl< Scalar, Options, JointCollectionTpl > JointModelDerived
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR typedef JointCollectionTpl< Scalar, Options > JointCollection
JointModelCompositeTpl(const size_t size)
Default contructor with a defined size.
std::vector< int > m_nqs
Dimension of the segment in the config vector.
std::vector< int > m_idx_v
Index in the tangent vector.
JointModelCompositeTpl()
Default contructor.
Common traits structure to fully define base classes for CRTP.
SE3Tpl< Scalar, Options > SE3
JointCompositeTpl< _Scalar, _Options, JointCollectionTpl > JointDerived
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
virtual bool isEqual(const CollisionGeometry &other) const=0
JointMotionSubspaceTpl< Eigen::Dynamic, Scalar, Options > Constraint_t
SE3Tpl< Scalar, Options > Transformation_t
AABB & operator=(const AABB &other)=default
JointCollectionTpl< Scalar, Options > JointCollection
JointModelDerived & derived()
SizeDepType< Eigen::Dynamic >::template ColsReturn< D >::Type jointCols_impl(Eigen::MatrixBase< D > &A) const
SizeDepType< NV >::template SegmentReturn< D >::ConstType jointVelocitySelector(const Eigen::MatrixBase< D > &a) const
Main pinocchio namespace.
pinocchio
Author(s):
autogenerated on Sat Jun 22 2024 02:41:47