liegroups.hpp

//
// 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<context::Scalar, Eigen::Dynamic, 1> ConfigVector_t;
 typedef Eigen::Matrix<context::Scalar, Eigen::Dynamic, 1> TangentVector_t;
 typedef Eigen::Matrix<context::Scalar, 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 context::Scalar & 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(
 const LieGroupType & lg,
 const ConfigVector_t & q,
 const TangentVector_t & v,
 const ArgumentPosition arg)
 {
 JacobianMatrix_t J(lg.nv(), lg.nv());
 lg.dIntegrate(q, v, J, arg);
 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 JacobianMatrix_t dIntegrateTransport_proxy(
 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<context::Scalar, Eigen::Dynamic, 1> ConfigVector_t;

 typedef LieGroupWrapperTpl<LieGroupType> LieGroupWrapper;
 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::dIntegrateTransport_proxy)
 .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)
#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
 .def(bp::self == bp::self)
#endif
 ;
 }

 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__