Commit a212763e authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Implement CartesianProductOperationVariant::dIntegrate_product_impl

parent 644cf97d
......@@ -131,6 +131,15 @@ namespace pinocchio
const Eigen::MatrixBase<JacobianOut_t> & J,
const AssignmentOperatorType op=SETTO) const;
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrate_product_impl(const Config_t & q,
const Tangent_t & v,
const JacobianIn_t & Jin,
JacobianOut_t & Jout,
bool dIntegrateOnTheLeft,
const ArgumentPosition arg,
const AssignmentOperatorType op) const;
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
......
......@@ -161,6 +161,43 @@ dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & q,
}
}
template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
dIntegrate_product_impl(const Config_t & q,
const Tangent_t & v,
const JacobianIn_t & Jin,
JacobianOut_t & Jout,
bool dIntegrateOnTheLeft,
const ArgumentPosition arg,
const AssignmentOperatorType op) const
{
Index id_q = 0, id_v = 0;
for(size_t k = 0; k < liegroups.size(); ++k)
{
const Index & nq = lg_nqs[k];
const Index & nv = lg_nvs[k];
if(dIntegrateOnTheLeft)
::pinocchio::dIntegrate(liegroups[k],
q.segment(id_q,nq),
v.segment(id_v,nv),
SELF,
Jin.middleRows(id_v,nv),
Jout.middleRows(id_v,nv),
arg, op);
else
::pinocchio::dIntegrate(liegroups[k],
q.segment(id_q,nq),
v.segment(id_v,nv),
Jin.middleCols(id_v,nv),
SELF,
Jout.middleCols(id_v,nv),
arg, op);
id_q += nq; id_v += nv;
}
}
template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
......
......@@ -123,6 +123,26 @@ namespace pinocchio
const ArgumentPosition arg,
const AssignmentOperatorType op = SETTO);
template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrate(const LieGroupGenericTpl<LieGroupCollection> & lg,
const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & J_in,
int self,
const Eigen::MatrixBase<JacobianOut_t> & J_out,
const ArgumentPosition arg,
const AssignmentOperatorType op = SETTO);
template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrate(const LieGroupGenericTpl<LieGroupCollection> & lg,
const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
int self,
const Eigen::MatrixBase<JacobianIn_t> & J_in,
const Eigen::MatrixBase<JacobianOut_t> & J_out,
const ArgumentPosition arg,
const AssignmentOperatorType op = SETTO);
template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg,
const Eigen::MatrixBase<ConfigL_t> & q0,
......
......@@ -430,6 +430,84 @@ namespace pinocchio
#undef PINOCCHIO_LG_VISITOR
template <class M1_t, class M2_t, class M3_t, class M4_t, bool dIntegrateOnTheLeft>
struct LieGroupDIntegrateProductVisitor
: visitor::LieGroupVisitorBase< LieGroupDIntegrateProductVisitor<M1_t, M2_t, M3_t, M4_t, dIntegrateOnTheLeft> >
{
typedef boost::fusion::vector<const M1_t &,
const M2_t &,
const M3_t &,
M4_t &,
const ArgumentPosition,
const AssignmentOperatorType> ArgsType;
LIE_GROUP_VISITOR(LieGroupDIntegrateProductVisitor);
template<typename LieGroupDerived>
static void algo(const LieGroupBase<LieGroupDerived> & lg,
const M1_t& q,
const M2_t& v,
const M3_t& J_in,
M4_t& J_out,
const ArgumentPosition arg,
const AssignmentOperatorType op)
{
assert((arg==ARG0||arg==ARG1) && "arg should be either ARG0 or ARG1");
switch (arg) {
case ARG0:
if(dIntegrateOnTheLeft) lg.dIntegrate_dq(q, v, SELF, J_in, J_out, op);
else lg.dIntegrate_dq(q, v, J_in, SELF, J_out, op);
return;
case ARG1:
if(dIntegrateOnTheLeft) lg.dIntegrate_dv(q, v, SELF, J_in, J_out, op);
else lg.dIntegrate_dv(q, v, J_in, SELF, J_out, op);
return;
default:
return;
}
}
};
template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrate(const LieGroupGenericTpl<LieGroupCollection> & lg,
const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & J_in,
int self,
const Eigen::MatrixBase<JacobianOut_t> & J_out,
const ArgumentPosition arg,
const AssignmentOperatorType op)
{
PINOCCHIO_UNUSED_VARIABLE(self);
PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, q, nq(lg));
PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg));
typedef LieGroupDIntegrateProductVisitor<Config_t, Tangent_t, JacobianIn_t, JacobianOut_t, false> Operation;
Operation::run(lg,typename Operation::ArgsType(
q.derived(),v.derived(),J_in.derived(),
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out), arg, op));
}
template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrate(const LieGroupGenericTpl<LieGroupCollection> & lg,
const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
int self,
const Eigen::MatrixBase<JacobianIn_t> & J_in,
const Eigen::MatrixBase<JacobianOut_t> & J_out,
const ArgumentPosition arg,
const AssignmentOperatorType op)
{
PINOCCHIO_UNUSED_VARIABLE(self);
PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, q, nq(lg));
PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg));
typedef LieGroupDIntegrateProductVisitor<Config_t, Tangent_t, JacobianIn_t, JacobianOut_t, true> Operation;
Operation::run(lg,typename Operation::ArgsType(
q.derived(),v.derived(),J_in.derived(),
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out), arg, op));
}
template <class M1_t, class M2_t, class M3_t, class M4_t>
struct LieGroupDIntegrateTransportVisitor
: visitor::LieGroupVisitorBase< LieGroupDIntegrateTransportVisitor<M1_t, M2_t, M3_t, M4_t> >
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment