Go to the documentation of this file.
5 #ifndef __pinocchio_joint_composite_hpp__
6 #define __pinocchio_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>
42 typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options>
U_t;
43 typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options>
D_t;
44 typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options>
UD_t;
52 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
56 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>
62 :
public JointDataBase< JointDataCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
64 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
83 ,
M(Transformation_t::Identity())
92 :
joints(joint_data), iMlast(joint_data.
size()), pjMi(joint_data.
size())
93 ,
S(Constraint_t::Zero(
nv))
94 ,
M(Transformation_t::Identity())
124 static std::string
classname() {
return std::string(
"JointDataComposite"); }
129 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
134 os <<
"JointDataComposite containing following models:\n" ;
135 for (
typename JointDataVector::const_iterator it = jdata.
joints.begin();
136 it != jdata.
joints.end(); ++it)
137 os <<
" " <<
shortname(*it) << std::endl;
142 template<
typename NewScalar,
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
148 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
150 :
public JointModelBase< JointModelCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
152 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
203 template<
typename Jo
intModel>
223 , jointPlacements(other.jointPlacements)
240 template<
typename Jo
intModel>
257 typename JointDataDerived::JointDataVector jdata(
joints.size());
258 for (
int i = 0;
i < (int)
joints.size(); ++
i)
259 jdata[(
size_t)
i] = ::pinocchio::createData<Scalar,Options,JointCollectionTpl>(
joints[(
size_t)
i]);
260 return JointDataDerived(jdata,
nq(),
nv());
265 std::vector<bool>
vec;
266 for (
size_t i = 0;
i <
joints.size(); ++
i)
268 const std::vector<bool> & joint_cf_limit =
joints[
i].hasConfigurationLimit();
270 joint_cf_limit.begin(),
271 joint_cf_limit.end());
278 std::vector<bool>
vec;
279 for (
size_t i = 0;
i <
joints.size(); ++
i)
281 const std::vector<bool> & joint_cf_limit =
joints[
i].hasConfigurationLimitInTangent();
283 joint_cf_limit.begin(),
284 joint_cf_limit.end());
289 template<
typename,
int,
template<
typename S,
int O>
class,
typename>
292 template<
typename ConfigVectorType>
293 void calc(JointDataDerived &
data,
const Eigen::MatrixBase<ConfigVectorType> &
qs)
const;
295 template<
typename,
int,
template<
typename S,
int O>
class,
typename,
typename>
298 template<
typename ConfigVectorType,
typename TangentVectorType>
300 const Eigen::MatrixBase<ConfigVectorType> &
qs,
301 const Eigen::MatrixBase<TangentVectorType> & vs)
const;
303 template<
typename Matrix6Like>
305 const Eigen::MatrixBase<Matrix6Like> & I,
306 const bool update_I)
const
308 data.U.noalias() = I *
data.S.matrix();
309 data.StU.noalias() =
data.S.matrix().transpose() *
data.U;
329 Base::setIndexes_impl(
id,
q,
v);
333 static std::string
classname() {
return std::string(
"JointModelComposite"); }
346 jointPlacements = other.jointPlacements;
356 &&
nq() == other.nq()
357 &&
nv() == other.nv()
363 && jointPlacements == other.jointPlacements
368 template<
typename NewScalar>
383 res.jointPlacements.resize(jointPlacements.size());
384 for(
size_t k = 0; k < jointPlacements.size(); ++k)
386 res.joints[k] =
joints[k].template cast<NewScalar>();
387 res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
414 jointCols(
const Eigen::MatrixBase<D>& A)
const {
return A.middleCols(Base::i_v,
nv()); }
417 jointCols(Eigen::MatrixBase<D>& A)
const {
return A.middleCols(Base::i_v,
nv()); }
444 template<
typename,
int,
template<
typename,
int>
class>
459 for(
size_t i = 0;
i <
joints.size(); ++
i)
492 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
497 os <<
"JointModelComposite containing following models:\n" ;
498 for (
typename JointModelVector::const_iterator it = jmodel.
joints.begin();
499 it != jmodel.
joints.end(); ++it)
500 os <<
" " <<
shortname(*it) << std::endl;
507 #include <boost/type_traits.hpp>
511 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
513 :
public integral_constant<bool,true> {};
515 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
517 :
public integral_constant<bool,true> {};
519 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
521 :
public integral_constant<bool,true> {};
523 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
525 :
public integral_constant<bool,true> {};
531 #include "pinocchio/multibody/joint/joint-composite.hxx"
533 #endif // ifndef __pinocchio_joint_composite_hpp__
JointDataDerived createData() const
#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.
MotionTpl< Scalar, Options > Motion_t
SizeDepType< Eigen::Dynamic >::template SegmentReturn< D >::ConstType jointConfigSelector_impl(const Eigen::MatrixBase< D > &a) const
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > D_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...
JointCollectionTpl< Scalar, Options > JointCollection
JointModelCompositeTpl(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Constructor with one joint.
JointModelTpl< Scalar, Options, JointCollectionTpl > JointModel
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...
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
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.
JointDataVector joints
Vector of joints.
JointModelCompositeTpl< NewScalar, Options, JointCollectionTpl > cast() const
ConstraintTpl< Eigen::Dynamic, Scalar, Options > Constraint_t
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.
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR typedef JointCollectionTpl< Scalar, Options > JointCollection
JointDataCompositeTpl(const JointDataVector &joint_data, const int, const int nv)
JointModelCompositeTpl & operator=(const JointModelCompositeTpl &other)
SizeDepType< NV >::template ColsReturn< D >::Type jointCols(Eigen::MatrixBase< D > &A) const
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > U_t
SizeDepType< Eigen::Dynamic >::template SegmentReturn< D >::ConstType jointVelocitySelector_impl(const Eigen::MatrixBase< D > &a) const
InertiaTpl< Scalar, Options > Inertia
JointModelDerived & addJoint(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Add a joint to the vector of joints.
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector
#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
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > ConfigVector_t
JointModelCompositeTpl< NewScalar, Options, JointCollectionTpl > type
SizeDepType< Eigen::Dynamic >::template SegmentReturn< D >::Type jointVelocitySelector_impl(Eigen::MatrixBase< D > &a) const
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
MotionTpl< Scalar, Options > Motion
bool isEqual(const JointModelCompositeTpl &other) const
SE3Tpl< Scalar, Options > SE3
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model.
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived)
JointDataTpl< Scalar, Options, JointCollectionTpl > JointDataVariant
JointModelCompositeTpl< Scalar, Options, JointCollectionTpl > JointModelDerived
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
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
std::string shortname() const
JointCollectionTpl< Scalar, Options > JointCollection
MotionTpl< Scalar, Options > Bias_t
const std::vector< bool > hasConfigurationLimitInTangent() const
JointCompositeTpl< Scalar, Options, JointCollectionTpl > JointDerived
JointCompositeTpl< Scalar, Options, JointCollectionTpl > JointDerived
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.
JointCompositeTpl< _Scalar, _Options, JointCollectionTpl > JointDerived
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointDataVariant) JointDataVector
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.
JointDataCompositeTpl< Scalar, Options, JointCollectionTpl > JointDataDerived
Common traits structure to fully define base classes for CRTP.
SE3Tpl< Scalar, Options > Transformation_t
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
JointCompositeTpl< _Scalar, _Options, JointCollectionTpl > JointDerived
JointModel JointModelVariant
AABB & operator=(const AABB &other)=default
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > UD_t
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.
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > TangentVector_t
pinocchio
Author(s):
autogenerated on Tue Feb 13 2024 03:43:58