Verified Commit 2b6215c3 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

liegroup: add new signature for dIntegrate

This new signature remove the tamplate argument ArgumentPosition and put it as a standard argument.
This modification is needed to expose dIntegrate for all the joints
parent 5ada87b2
......@@ -108,7 +108,29 @@ SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
template <ArgumentPosition arg, class Config_t, class Tangent_t, class JacobianOut_t>
void dIntegrate(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t>& J) const;
const Eigen::MatrixBase<JacobianOut_t>& J) const
{
PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
return dIntegrate(q,v,J,arg);
}
/**
* @brief Computes the Jacobian of a small variation of the configuration vector or the tangent vector into tangent space at identity.
*
* @details This Jacobian corresponds to the Jacobian of \f$ (\bm{q} \oplus \delta \bm{q}) \oplus \bm{v} \f$ with
* \f$ \delta \bm{q} \rightarrow 0 \f$ if arg == ARG0 or \f$ \delta \bm{v} \rightarrow 0 \f$ if arg == ARG1.
*
* @param[in] q configuration vector.
* @param[in] v tangent vector.
* @param[in] arg ARG0 (resp. ARG1) to get the Jacobian with respect to q (resp. v).
*
* @param[out] J the Jacobian of the Integrate operation w.r.t. the argument arg.
*/
template<class Config_t, class Tangent_t, class JacobianOut_t>
void dIntegrate(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J,
const ArgumentPosition arg) const;
/**
* @brief Computes the Jacobian of a small variation of the configuration vector into tangent space at identity.
......
......@@ -50,12 +50,14 @@ namespace pinocchio {
}
template <class Derived>
template <ArgumentPosition arg, class Config_t, class Tangent_t, class JacobianOut_t>
template<class Config_t, class Tangent_t, class JacobianOut_t>
void LieGroupBase<Derived>::dIntegrate(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t>& J) const
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t>& J,
const ArgumentPosition arg) const
{
PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
assert((arg==ARG0||arg==ARG1) && "arg should be either ARG0 or ARG1");
switch (arg) {
case ARG0:
dIntegrate_dq(q,v,J); return;
......
Supports Markdown
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