Skip to content
GitLab
Unverified Commit 2509a303 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1226 from jmirabel/liegroups 

LieGroup Python bindings.
parents cdcef625 7a1c5682
Expand all Hide whitespace changes
Inline Side-by-side
bindings/python/fwd.hpp
View file @ 2509a303
......@@ -20,6 +20,7 @@ namespace pinocchio
void exposeInertia();
void exposeExplog();
void exposeSkew();
void exposeLieGroups();
// Expose math module
void exposeRpy();
......
bindings/python/module.cpp
View file @ 2509a303
......@@ -59,6 +59,7 @@ BOOST_PYTHON_MODULE(pinocchio_pywrap)
exposeExplog();
exposeRpy();
exposeSkew();
exposeLieGroups();
bp::enum_< ::pinocchio::ReferenceFrame >("ReferenceFrame")
.value("WORLD",::pinocchio::WORLD)
......
bindings/python/multibody/expose-liegroups.cpp 0 → 100644
View file @ 2509a303
//
// Copyright (c) 2015-2020 CNRS INRIA
//
#include "pinocchio/bindings/python/fwd.hpp"
#include "pinocchio/bindings/python/multibody/liegroups.hpp"
#include "pinocchio/bindings/python/utils/namespace.hpp"
#include <eigenpy/memory.hpp>
namespace pinocchio
{
namespace python
{
namespace bp = boost::python;
template<typename LgType>
CartesianProductOperationVariantTpl<double,0,LieGroupCollectionDefaultTpl>
makeLieGroup()
{
return CartesianProductOperationVariantTpl<double,0,LieGroupCollectionDefaultTpl> (LgType());
}
CartesianProductOperationVariantTpl<double,0,LieGroupCollectionDefaultTpl>
makeRn(int n)
{
return CartesianProductOperationVariantTpl<double,0,LieGroupCollectionDefaultTpl> (
VectorSpaceOperationTpl<Eigen::Dynamic,double,0>(n));
}
/* --- Expose --------------------------------------------------------- */
void exposeLieGroups()
{
LieGroupPythonVisitor<
CartesianProductOperationVariantTpl<double,0,LieGroupCollectionDefaultTpl>
>::expose("LieGroup");
{
// Switch the scope to the submodule, add methods and classes.
bp::scope submoduleScope = getOrCreatePythonNamespace("liegroups");
bp::def("R1", makeLieGroup<VectorSpaceOperationTpl<1,double,0> >);
bp::def("R2", makeLieGroup<VectorSpaceOperationTpl<2,double,0> >);
bp::def("R3", makeLieGroup<VectorSpaceOperationTpl<3,double,0> >);
bp::def("Rn", makeRn);
bp::def("SO2", makeLieGroup<SpecialOrthogonalOperationTpl<2,double,0> >);
bp::def("SO3", makeLieGroup<SpecialOrthogonalOperationTpl<3,double,0> >);
bp::def("SE2", makeLieGroup<SpecialEuclideanOperationTpl<2,double,0> >);
bp::def("SE3", makeLieGroup<SpecialEuclideanOperationTpl<3,double,0> >);
}
}
} // namespace python
} // namespace pinocchio
bindings/python/multibody/liegroups.hpp 0 → 100644
View file @ 2509a303
//
// Copyright (c) 2015-2020 CNRS INRIA
//
#ifndef __pinocchio_python_lie_group_hpp__
#define __pinocchio_python_lie_group_hpp__
#include <eigenpy/memory.hpp>
#include "pinocchio/multibody/liegroup/liegroup.hpp"
#include "pinocchio/multibody/liegroup/cartesian-product-variant.hpp"
#include "pinocchio/multibody/liegroup/liegroup-generic.hpp"
#include "pinocchio/multibody/liegroup/liegroup-collection.hpp"
namespace pinocchio
{
namespace python
{
namespace bp = boost::python;
template<class LieGroupType>
struct LieGroupWrapperTpl
{
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> ConfigVector_t;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> TangentVector_t;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> JacobianMatrix_t;
static ConfigVector_t integrate(const LieGroupType& lg,
const ConfigVector_t& q, const TangentVector_t& v)
{
return lg.integrate(q, v);
}
static ConfigVector_t interpolate(const LieGroupType& lg,
const ConfigVector_t& q0,
const ConfigVector_t& q1,
const double& u)
{
return lg.interpolate(q0,q1,u);
}
static TangentVector_t difference(const LieGroupType& lg,
const ConfigVector_t & q0,
const ConfigVector_t & q1)
{
return lg.difference(q0,q1);
}
static JacobianMatrix_t dDifference1(const LieGroupType& lg,
const ConfigVector_t& q0, const ConfigVector_t& q1,
const ArgumentPosition arg)
{
JacobianMatrix_t J (lg.nv(), lg.nv());
lg.dDifference(q0, q1, J, arg);
return J;
}
static JacobianMatrix_t dDifference2(const LieGroupType& lg,
const ConfigVector_t& q0, const ConfigVector_t& q1,
const ArgumentPosition arg,
const JacobianMatrix_t& Jin, int self)
{
JacobianMatrix_t J (Jin.rows(), Jin.cols());
switch (arg) {
case ARG0:
lg.template dDifference<ARG0>(q0, q1, Jin, self, J, SETTO);
break;
case ARG1:
lg.template dDifference<ARG1>(q0, q1, Jin, self, J, SETTO);
break;
default:
throw std::invalid_argument("arg must be either ARG0 or ARG1");
}
return J;
}
static JacobianMatrix_t dDifference3(const LieGroupType& lg,
const ConfigVector_t& q0, const ConfigVector_t& q1,
const ArgumentPosition arg,
int self, const JacobianMatrix_t& Jin)
{
JacobianMatrix_t J (Jin.rows(), Jin.cols());
switch (arg) {
case ARG0:
lg.template dDifference<ARG0>(q0, q1, self, Jin, J, SETTO);
break;
case ARG1:
lg.template dDifference<ARG1>(q0, q1, self, Jin, J, SETTO);
break;
default:
throw std::invalid_argument("arg must be either ARG0 or ARG1");
}
return J;
}
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 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_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;
}
static void dIntegrateTransport1(const LieGroupType& lg,
const ConfigVector_t& q, const TangentVector_t& v,
JacobianMatrix_t& J,
const ArgumentPosition arg)
{
lg.dIntegrateTransport(q, v, J, arg);
}
static JacobianMatrix_t dIntegrateTransport2(const LieGroupType& lg,
const ConfigVector_t& q, const TangentVector_t& v,
const JacobianMatrix_t& J,
const ArgumentPosition arg)
{
JacobianMatrix_t Jout (lg.nv(), J.cols());
lg.dIntegrateTransport(q, v, J, Jout, arg);
return Jout;
}
};
template<class LieGroupType>
struct LieGroupPythonVisitor
: public boost::python::def_visitor< LieGroupPythonVisitor<LieGroupType> >
{
public:
/* --- Exposing C++ API to python through the handler ----------------- */
template<class PyClass>
void visit(PyClass& cl) const
{
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> ConfigVector_t;
typedef LieGroupWrapperTpl<LieGroupType> LieGroupWrapper;
cl
.def(bp::init<>("Default constructor"))
.def("integrate", LieGroupWrapper::integrate)
.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)
.def("difference", LieGroupWrapper::difference)
.def("dDifference", LieGroupWrapper::dDifference1)
.def("dDifference", LieGroupWrapper::dDifference2)
.def("dDifference", LieGroupWrapper::dDifference3)
.def("interpolate", LieGroupWrapper::interpolate)
.def("random", static_cast<typename LieGroupType::ConfigVector_t (LieGroupType::*)() const>(&LieGroupType::random))
.def("randomConfiguration", &LieGroupType::template randomConfiguration<ConfigVector_t, ConfigVector_t>)
.def("distance", &LieGroupType::template distance<ConfigVector_t, ConfigVector_t>)
.def("squaredDistance", &LieGroupType::template squaredDistance<ConfigVector_t, ConfigVector_t>)
.def("normalize", &LieGroupType::template normalize<ConfigVector_t>)
.add_property("name", &LieGroupType::name)
.add_property("neutral", &LieGroupType::neutral)
.add_property("nq", &LieGroupType::nq)
.add_property("nv", &LieGroupType::nv)
.def(bp::self * bp::self)
.def(bp::self *= bp::self)
.def(bp::self == bp::self)
;
}
static void expose(const char* name)
{
bp::class_<LieGroupType>(name, bp::no_init)
.def(LieGroupPythonVisitor<LieGroupType>());
}
};
} // namespace python
} // namespace pinocchio
#endif // ifndef __pinocchio_python_geometry_model_hpp__
src/multibody/liegroup/cartesian-product-variant.hpp
View file @ 2509a303
......@@ -5,22 +5,25 @@
#ifndef __pinocchio_cartesian_product_variant_hpp__
#define __pinocchio_cartesian_product_variant_hpp__
#include "pinocchio/multibody/liegroup/operation-base.hpp"
#include "pinocchio/multibody/liegroup/liegroup-variant.hpp"
#include "pinocchio/multibody/liegroup/liegroup-variant-visitors.hpp"
#include "pinocchio/multibody/liegroup/liegroup-base.hpp"
#include "pinocchio/multibody/liegroup/liegroup-collection.hpp"
#include <vector>
namespace pinocchio
{
struct CartesianProductOperationVariant;
template<typename Scalar, int Options = 0,
template<typename,int> class LieGroupCollectionTpl = LieGroupCollectionDefaultTpl>
struct CartesianProductOperationVariantTpl;
typedef CartesianProductOperationVariantTpl<double, 0, LieGroupCollectionDefaultTpl> CartesianProductOperationVariant;
template<>
struct traits<CartesianProductOperationVariant>
template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
struct traits<CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl> >
{
typedef double Scalar;
typedef _Scalar Scalar;
enum {
Options = _Options,
NQ = Eigen::Dynamic,
NV = Eigen::Dynamic
};
......@@ -29,12 +32,17 @@ namespace pinocchio
///
/// \brief Dynamic Cartesian product composed of elementary Lie groups defined in LieGroupVariant
///
struct CartesianProductOperationVariant : public LieGroupOperationBase<CartesianProductOperationVariant>
template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
struct CartesianProductOperationVariantTpl : public LieGroupBase<CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl> >
{
PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE(CartesianProductOperationVariant);
PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(CartesianProductOperationVariantTpl);
typedef LieGroupCollectionTpl<Scalar, Options> LieGroupCollection;
typedef typename LieGroupCollection::LieGroupVariant LieGroupVariant;
typedef LieGroupGenericTpl<LieGroupCollection> LieGroupGeneric;
/// \brief Default constructor
CartesianProductOperationVariant()
CartesianProductOperationVariantTpl()
: m_nq(0), m_nv(0)
, lg_nqs(0), lg_nvs(0)
, m_neutral(0)
......@@ -45,7 +53,10 @@ namespace pinocchio
///
/// \param[in] lg Lie group variant to insert inside the Cartesian product
///
CartesianProductOperationVariant(const LieGroupVariant & lg)
explicit CartesianProductOperationVariantTpl(const LieGroupGeneric & lg)
: m_nq(0), m_nv(0)
, lg_nqs(0), lg_nvs(0)
, m_neutral(0)
{
append(lg);
};
......@@ -56,8 +67,11 @@ namespace pinocchio
/// \param[in] lg1 Lie group variant to insert inside the Cartesian product
/// \param[in] lg2 Lie group variant to insert inside the Cartesian product
///
CartesianProductOperationVariant(const LieGroupVariant & lg1,
const LieGroupVariant & lg2)
CartesianProductOperationVariantTpl(const LieGroupGeneric & lg1,
const LieGroupGeneric & lg2)
: m_nq(0), m_nv(0)
, lg_nqs(0), lg_nvs(0)
, m_neutral(0)
{
append(lg1); append(lg2);
};
......@@ -67,19 +81,16 @@ namespace pinocchio
///
/// \param[in] lg Lie group variant to insert inside the Cartesian product
///
void append(const LieGroupVariant & lg)
void append(const LieGroupGeneric & lg);
CartesianProductOperationVariantTpl operator* (const CartesianProductOperationVariantTpl& other) const;
CartesianProductOperationVariantTpl& operator*= (const CartesianProductOperationVariantTpl& other);
inline CartesianProductOperationVariantTpl& operator*= (const LieGroupGeneric& lg)
{
liegroups.push_back(lg);
const Index lg_nq = ::pinocchio::nq(lg); lg_nqs.push_back(lg_nq); m_nq += lg_nq;
const Index lg_nv = ::pinocchio::nv(lg); lg_nvs.push_back(lg_nv); m_nv += lg_nv;
if(liegroups.size() > 1)
m_name += " x ";
m_name += ::pinocchio::name(lg);
m_neutral.conservativeResize(m_nq);
m_neutral.tail(lg_nq) = ::pinocchio::neutral(lg);
append(lg);
return *this;
}
int nq() const { return m_nq; }
......@@ -89,49 +100,104 @@ namespace pinocchio
ConfigVector_t neutral() const { return m_neutral; }
template <class ConfigL_t, class ConfigR_t, class Tangent_t>
void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<Tangent_t> & d) const;
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
void dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianOut_t> & J) const;
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
void dDifference_product_impl(const ConfigL_t & q0,
const ConfigR_t & q1,
const JacobianIn_t & Jin,
JacobianOut_t & Jout,
bool dDifferenceOnTheLeft,
const AssignmentOperatorType op) const;
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Velocity_t> & v,
const Eigen::MatrixBase<ConfigOut_t> & qout) const
{
assert(q.size() == m_nq);
assert(v.size() == m_nv);
assert(qout.size() == m_nq);
ConfigOut_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
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];
::pinocchio::integrate(liegroups[k],
q.segment(id_q,lg_nqs[k]),
v.segment(id_v,lg_nvs[k]),
qout_.segment(id_q,lg_nqs[k]));
id_q += nq; id_v += nv;
}
const Eigen::MatrixBase<ConfigOut_t> & qout) const;
template <class Config_t, class Jacobian_t>
void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
const Eigen::MatrixBase<Jacobian_t> & J) const;
template <class Config_t, class Tangent_t, class JacobianOut_t>
void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J,
const AssignmentOperatorType op=SETTO) const;
template <class Config_t, class Tangent_t, class JacobianOut_t>
void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
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,
const Eigen::MatrixBase<JacobianIn_t> & J_in,
const Eigen::MatrixBase<JacobianOut_t> & J_out) const;
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & J_in,
const Eigen::MatrixBase<JacobianOut_t> & J_out) const;
template <class Config_t, class Tangent_t, class JacobianOut_t>
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J) const;
template <class Config_t, class Tangent_t, class JacobianOut_t>
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J) const;
template <class ConfigL_t, class ConfigR_t>
Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1) const;
template <class Config_t>
void normalize_impl (const Eigen::MatrixBase<Config_t>& qout) const;
template <class Config_t>
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const;
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
const Eigen::MatrixBase<ConfigR_t> & upper,
const Eigen::MatrixBase<ConfigOut_t> & qout) const;
template <class ConfigL_t, class ConfigR_t>
bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec) const;
bool isEqual_impl (const CartesianProductOperationVariantTpl& other) const;
template <typename LieGroup1, typename LieGroup2>
bool isEqual(const CartesianProductOperation<LieGroup1, LieGroup2> & other) const;
}
// template <class Config_t>
// void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
// {
// R3crossSO3_t().random(qout);
// }
//
// template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
// void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
// const Eigen::MatrixBase<ConfigR_t> & upper,
// const Eigen::MatrixBase<ConfigOut_t> & qout)
// const
// {
// R3crossSO3_t ().randomConfiguration(lower, upper, qout);
// }
protected:
std::vector<LieGroupVariant> liegroups;
std::vector<LieGroupGeneric> liegroups;
Index m_nq, m_nv;
std::vector<Index> lg_nqs, lg_nvs;
std::string m_name;
......@@ -142,4 +208,6 @@ namespace pinocchio
} // namespace pinocchio
#include <pinocchio/multibody/liegroup/cartesian-product-variant.hxx>
#endif // ifndef __pinocchio_cartesian_product_variant_hpp__
src/multibody/liegroup/cartesian-product-variant.hxx 0 → 100644
View file @ 2509a303
//
// Copyright (c) 2020 CNRS
//
#ifndef __pinocchio_cartesian_product_variant_hxx__
#define __pinocchio_cartesian_product_variant_hxx__
#include "pinocchio/multibody/liegroup/liegroup-variant-visitors.hpp"
namespace pinocchio
{
template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
append(const LieGroupGeneric & lg)
{
liegroups.push_back(lg);
const Index lg_nq = ::pinocchio::nq(lg); lg_nqs.push_back(lg_nq); m_nq += lg_nq;
const Index lg_nv = ::pinocchio::nv(lg); lg_nvs.push_back(lg_nv); m_nv += lg_nv;
if(liegroups.size() > 1)
m_name += " x ";
m_name += ::pinocchio::name(lg);
m_neutral.conservativeResize(m_nq);
m_neutral.tail(lg_nq) = ::pinocchio::neutral(lg);