5 #ifndef __pinocchio_cartesian_product_variant_hpp__
6 #define __pinocchio_cartesian_product_variant_hpp__
8 #include "pinocchio/multibody/liegroup/operation-base.hpp"
9 #include "pinocchio/multibody/liegroup/liegroup-variant.hpp"
10 #include "pinocchio/multibody/liegroup/liegroup-variant-visitors.hpp"
17 struct CartesianProductOperationVariant;
22 typedef double Scalar;
39 , lg_nqs(0), lg_nvs(0)
60 const LieGroupVariant & lg2)
70 void append(
const LieGroupVariant & lg)
72 liegroups.push_back(lg);
73 const Index lg_nq = ::pinocchio::nq(lg); lg_nqs.push_back(lg_nq); m_nq += lg_nq;
74 const Index lg_nv = ::pinocchio::nv(lg); lg_nvs.push_back(lg_nv); m_nv += lg_nv;
76 if(liegroups.size() > 1)
78 m_name += ::pinocchio::name(lg);
80 m_neutral.conservativeResize(m_nq);
81 m_neutral.tail(lg_nq) = ::pinocchio::neutral(lg);
85 int nq()
const {
return m_nq; }
86 int nv()
const {
return m_nv; }
88 std::string name()
const {
return m_name; }
90 ConfigVector_t neutral()
const {
return m_neutral; }
92 template <
class ConfigIn_t,
class Velocity_t,
class ConfigOut_t>
93 void integrate_impl(
const Eigen::MatrixBase<ConfigIn_t> & q,
94 const Eigen::MatrixBase<Velocity_t> & v,
95 const Eigen::MatrixBase<ConfigOut_t> & qout)
const
97 assert(q.size() == m_nq);
98 assert(v.size() == m_nv);
99 assert(qout.size() == m_nq);
101 ConfigOut_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
102 Index id_q = 0, id_v = 0;
103 for(
size_t k = 0; k < liegroups.size(); ++k)
105 const Index & nq = lg_nqs[k];
106 const Index & nv = lg_nvs[k];
107 ::pinocchio::integrate(liegroups[k],
108 q.segment(id_q,lg_nqs[k]),
109 v.segment(id_v,lg_nvs[k]),
110 qout_.segment(id_q,lg_nqs[k]));
112 id_q +=
nq; id_v +=
nv;
134 std::vector<LieGroupVariant> liegroups;
136 std::vector<Index> lg_nqs, lg_nvs;
139 ConfigVector_t m_neutral;
145 #endif // ifndef __pinocchio_cartesian_product_variant_hpp__