Commit 0c64f31e authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[LieGroup] Provide base architecture for Ja = dIntegrate() * Jb (and reverse)

parent d5b2ce89
......@@ -55,12 +55,55 @@ struct LieGroupWrapperTpl
return J;
}
static JacobianMatrix_t dIntegrate(const LieGroupType& lg,
static JacobianMatrix_t dIntegrate_dq1(const LieGroupType& lg,
const ConfigVector_t& q, const TangentVector_t& v)
{
JacobianMatrix_t J (lg.nv(), lg.nv());
lg.dIntegrate_dq(q, v, J);
return J;
}
static JacobianMatrix_t dIntegrate_dq2(const LieGroupType& lg,
const ConfigVector_t& q, const TangentVector_t& v,
const ArgumentPosition arg)
const JacobianMatrix_t& Jin, int self)
{
JacobianMatrix_t J (Jin.rows(), lg.nv());
lg.dIntegrate_dq(q, v, Jin, self, J, SETTO);
return J;
}
static JacobianMatrix_t dIntegrate_dq3(const LieGroupType& lg,
const ConfigVector_t& q, const TangentVector_t& v, int self,
const JacobianMatrix_t& Jin)
{
JacobianMatrix_t J (lg.nv(), Jin.cols());
lg.dIntegrate_dq(q, v, self, Jin, J, SETTO);
return J;
}
static JacobianMatrix_t dIntegrate_dv1(const LieGroupType& lg,
const ConfigVector_t& q, const TangentVector_t& v)
{
JacobianMatrix_t J (lg.nv(), lg.nv());
lg.dIntegrate(q, v, J, arg);
lg.dIntegrate_dv(q, v, J);
return J;
}
static JacobianMatrix_t dIntegrate_dv2(const LieGroupType& lg,
const ConfigVector_t& q, const TangentVector_t& v,
const JacobianMatrix_t& Jin, int self)
{
JacobianMatrix_t J (Jin.rows(), lg.nv());
lg.dIntegrate_dv(q, v, Jin, self, J, SETTO);
return J;
}
static JacobianMatrix_t dIntegrate_dv3(const LieGroupType& lg,
const ConfigVector_t& q, const TangentVector_t& v, int self,
const JacobianMatrix_t& Jin)
{
JacobianMatrix_t J (lg.nv(), Jin.cols());
lg.dIntegrate_dv(q, v, self, Jin, J, SETTO);
return J;
}
......@@ -99,7 +142,12 @@ public:
cl
.def(bp::init<>("Default constructor"))
.def("integrate", LieGroupWrapper::integrate)
.def("dIntegrate", LieGroupWrapper::dIntegrate)
.def("dIntegrate_dq", LieGroupWrapper::dIntegrate_dq1)
.def("dIntegrate_dq", LieGroupWrapper::dIntegrate_dq2)
.def("dIntegrate_dq", LieGroupWrapper::dIntegrate_dq3)
.def("dIntegrate_dv", LieGroupWrapper::dIntegrate_dv1)
.def("dIntegrate_dv", LieGroupWrapper::dIntegrate_dv2)
.def("dIntegrate_dv", LieGroupWrapper::dIntegrate_dv3)
.def("dIntegrateTransport", LieGroupWrapper::dIntegrateTransport1)
.def("dIntegrateTransport", LieGroupWrapper::dIntegrateTransport2)
......
......@@ -11,6 +11,11 @@
namespace pinocchio
{
#if __cplusplus >= 201103L
constexpr int SELF = 0;
#else
enum { SELF = 0 };
#endif
#define PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,TYPENAME) \
typedef LieGroupBase<Derived> Base; \
......@@ -141,6 +146,22 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
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_dq(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
int self,
const Eigen::MatrixBase<JacobianOut_t> & Jout,
const AssignmentOperatorType op = SETTO) const;
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrate_dq(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
int self,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout,
const AssignmentOperatorType op = SETTO) const;
/**
* @brief Computes the Jacobian of a small variation of the tangent vector into tangent space at identity.
*
......@@ -159,6 +180,22 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
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_dv(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
int self,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout,
const AssignmentOperatorType op = SETTO) const;
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrate_dv(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
int self,
const Eigen::MatrixBase<JacobianOut_t> & Jout,
const AssignmentOperatorType op = SETTO) const;
/**
*
* @brief Transport a matrix from the terminal to the originate tangent space of the integrate operation, with respect to the configuration or the velocity arguments.
......@@ -487,6 +524,15 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
/// \name Default implementations
/// \{
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 ConfigL_t, class ConfigR_t, class ConfigOut_t>
void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
......
......@@ -75,6 +75,50 @@ namespace pinocchio {
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),op);
}
template <class Derived>
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void LieGroupBase<Derived>::dIntegrate_dq(
const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
int self,
const Eigen::MatrixBase<JacobianOut_t> & Jout,
const AssignmentOperatorType op) const
{
PINOCCHIO_UNUSED_VARIABLE(self);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
assert(Jin.cols() == nv());
assert(Jout.cols() == nv());
assert(Jout.rows() == Jin.rows());
derived().dIntegrate_product_impl(
q.derived(), v.derived(),
Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout),
false, ARG0, op);
}
template <class Derived>
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void LieGroupBase<Derived>::dIntegrate_dq(
const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
int self,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout,
const AssignmentOperatorType op) const
{
PINOCCHIO_UNUSED_VARIABLE(self);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
assert(Jin.cols() == nv());
assert(Jout.cols() == nv());
assert(Jout.rows() == Jin.rows());
derived().dIntegrate_product_impl(
q.derived(), v.derived(),
Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout),
true, ARG0, op);
}
template <class Derived>
template <class Config_t, class Tangent_t, class JacobianOut_t>
void LieGroupBase<Derived>::dIntegrate_dv(
......@@ -92,6 +136,50 @@ namespace pinocchio {
op);
}
template <class Derived>
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void LieGroupBase<Derived>::dIntegrate_dv(
const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
int self,
const Eigen::MatrixBase<JacobianOut_t> & Jout,
const AssignmentOperatorType op) const
{
PINOCCHIO_UNUSED_VARIABLE(self);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
assert(Jin.cols() == nv());
assert(Jout.cols() == nv());
assert(Jout.rows() == Jin.rows());
derived().dIntegrate_product_impl(
q.derived(), v.derived(),
Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout),
false, ARG1, op);
}
template <class Derived>
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void LieGroupBase<Derived>::dIntegrate_dv(
const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
int self,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout,
const AssignmentOperatorType op) const
{
PINOCCHIO_UNUSED_VARIABLE(self);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
assert(Jin.cols() == nv());
assert(Jout.cols() == nv());
assert(Jout.rows() == Jin.rows());
derived().dIntegrate_product_impl(
q.derived(), v.derived(),
Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout),
true, ARG1, op);
}
template <class Derived>
template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void LieGroupBase<Derived>::dIntegrateTransport(const Eigen::MatrixBase<Config_t > & q,
......@@ -406,6 +494,36 @@ namespace pinocchio {
}
// ----------------- Default implementations ------------------------------ //
template <class Derived>
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void LieGroupBase<Derived>::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 nv_ (nv());
JacobianMatrix_t J (nv_, nv_);
dIntegrate(q, v, J, arg);
switch (op) {
case SETTO:
if(dIntegrateOnTheLeft) Jout = J * Jin;
else Jout = Jin * J;
return;
case ADDTO:
if(dIntegrateOnTheLeft) Jout += J * Jin;
else Jout += Jin * J;
return;
case RMTO:
if(dIntegrateOnTheLeft) Jout -= J * Jin;
else Jout -= Jin * J;
return;
}
}
template <class Derived>
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
void LieGroupBase<Derived>::interpolate_impl(
......
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