liegroups.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_python_lie_group_hpp__
6 #define __pinocchio_python_lie_group_hpp__
7 
8 #include <eigenpy/memory.hpp>
9 
10 #include "pinocchio/multibody/liegroup/liegroup.hpp"
11 #include "pinocchio/multibody/liegroup/cartesian-product-variant.hpp"
12 #include "pinocchio/multibody/liegroup/liegroup-generic.hpp"
13 #include "pinocchio/multibody/liegroup/liegroup-collection.hpp"
14 
15 namespace pinocchio
16 {
17 namespace python
18 {
19 namespace bp = boost::python;
20 
21 template<class LieGroupType>
23 {
24  typedef Eigen::Matrix<double, Eigen::Dynamic, 1> ConfigVector_t;
25  typedef Eigen::Matrix<double, Eigen::Dynamic, 1> TangentVector_t;
26  typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> JacobianMatrix_t;
27 
28  static ConfigVector_t integrate(const LieGroupType& lg,
29  const ConfigVector_t& q, const TangentVector_t& v)
30  {
31  return lg.integrate(q, v);
32  }
33 
34  static ConfigVector_t interpolate(const LieGroupType& lg,
35  const ConfigVector_t& q0,
36  const ConfigVector_t& q1,
37  const double& u)
38  {
39  return lg.interpolate(q0,q1,u);
40  }
41 
42  static TangentVector_t difference(const LieGroupType& lg,
43  const ConfigVector_t & q0,
44  const ConfigVector_t & q1)
45  {
46  return lg.difference(q0,q1);
47  }
48 
49  static JacobianMatrix_t dDifference1(const LieGroupType& lg,
50  const ConfigVector_t& q0, const ConfigVector_t& q1,
51  const ArgumentPosition arg)
52  {
53  JacobianMatrix_t J(lg.nv(), lg.nv());
54  lg.dDifference(q0, q1, J, arg);
55  return J;
56  }
57 
58  static JacobianMatrix_t dDifference2(const LieGroupType& lg,
59  const ConfigVector_t& q0, const ConfigVector_t& q1,
60  const ArgumentPosition arg,
61  const JacobianMatrix_t& Jin, int self)
62  {
63  JacobianMatrix_t J(Jin.rows(), Jin.cols());
64  switch (arg) {
65  case ARG0:
66  lg.template dDifference<ARG0>(q0, q1, Jin, self, J, SETTO);
67  break;
68  case ARG1:
69  lg.template dDifference<ARG1>(q0, q1, Jin, self, J, SETTO);
70  break;
71  default:
72  throw std::invalid_argument("arg must be either ARG0 or ARG1");
73  }
74  return J;
75  }
76 
77  static JacobianMatrix_t dDifference3(const LieGroupType& lg,
78  const ConfigVector_t& q0, const ConfigVector_t& q1,
79  const ArgumentPosition arg,
80  int self, const JacobianMatrix_t& Jin)
81  {
82  JacobianMatrix_t J(Jin.rows(), Jin.cols());
83  switch (arg) {
84  case ARG0:
85  lg.template dDifference<ARG0>(q0, q1, self, Jin, J, SETTO);
86  break;
87  case ARG1:
88  lg.template dDifference<ARG1>(q0, q1, self, Jin, J, SETTO);
89  break;
90  default:
91  throw std::invalid_argument("arg must be either ARG0 or ARG1");
92  }
93  return J;
94  }
95 
96  static JacobianMatrix_t dIntegrate(const LieGroupType& lg,
97  const ConfigVector_t& q,
98  const TangentVector_t& v,
99  const ArgumentPosition arg)
100  {
101  JacobianMatrix_t J(lg.nv(), lg.nv());
102  lg.dIntegrate(q, v, J, arg);
103  return J;
104  }
105 
106  static JacobianMatrix_t dIntegrate_dq1(const LieGroupType& lg,
107  const ConfigVector_t& q, const TangentVector_t& v)
108  {
109  JacobianMatrix_t J(lg.nv(), lg.nv());
110  lg.dIntegrate_dq(q, v, J);
111  return J;
112  }
113 
114  static JacobianMatrix_t dIntegrate_dq2(const LieGroupType& lg,
115  const ConfigVector_t& q, const TangentVector_t& v,
116  const JacobianMatrix_t& Jin, int self)
117  {
118  JacobianMatrix_t J(Jin.rows(), lg.nv());
119  lg.dIntegrate_dq(q, v, Jin, self, J, SETTO);
120  return J;
121  }
122 
123  static JacobianMatrix_t dIntegrate_dq3(const LieGroupType& lg,
124  const ConfigVector_t& q, const TangentVector_t& v, int self,
125  const JacobianMatrix_t& Jin)
126  {
127  JacobianMatrix_t J(lg.nv(), Jin.cols());
128  lg.dIntegrate_dq(q, v, self, Jin, J, SETTO);
129  return J;
130  }
131 
132  static JacobianMatrix_t dIntegrate_dv1(const LieGroupType& lg,
133  const ConfigVector_t& q, const TangentVector_t& v)
134  {
135  JacobianMatrix_t J(lg.nv(), lg.nv());
136  lg.dIntegrate_dv(q, v, J);
137  return J;
138  }
139 
140  static JacobianMatrix_t dIntegrate_dv2(const LieGroupType& lg,
141  const ConfigVector_t& q, const TangentVector_t& v,
142  const JacobianMatrix_t& Jin, int self)
143  {
144  JacobianMatrix_t J(Jin.rows(), lg.nv());
145  lg.dIntegrate_dv(q, v, Jin, self, J, SETTO);
146  return J;
147  }
148 
149  static JacobianMatrix_t dIntegrate_dv3(const LieGroupType& lg,
150  const ConfigVector_t& q, const TangentVector_t& v, int self,
151  const JacobianMatrix_t& Jin)
152  {
153  JacobianMatrix_t J(lg.nv(), Jin.cols());
154  lg.dIntegrate_dv(q, v, self, Jin, J, SETTO);
155  return J;
156  }
157 
158  static JacobianMatrix_t
159  dIntegrateTransport_proxy(const LieGroupType & lg,
160  const ConfigVector_t & q,
161  const TangentVector_t & v,
162  const JacobianMatrix_t & J,
163  const ArgumentPosition arg)
164  {
165  JacobianMatrix_t Jout (lg.nv(), J.cols());
166  lg.dIntegrateTransport(q, v, J, Jout, arg);
167  return Jout;
168  }
169 };
170 
171 template<class LieGroupType>
173 : public boost::python::def_visitor< LieGroupPythonVisitor<LieGroupType> >
174 {
175 public:
176 
177  /* --- Exposing C++ API to python through the handler ----------------- */
178  template<class PyClass>
179  void visit(PyClass& cl) const
180  {
181  typedef Eigen::Matrix<double, Eigen::Dynamic, 1> ConfigVector_t;
182 
183  typedef LieGroupWrapperTpl<LieGroupType> LieGroupWrapper;
184  cl
185  .def(bp::init<>("Default constructor"))
186  .def("integrate", LieGroupWrapper::integrate)
187  .def("dIntegrate", LieGroupWrapper::dIntegrate)
188  .def("dIntegrate_dq", LieGroupWrapper::dIntegrate_dq1)
189  .def("dIntegrate_dq", LieGroupWrapper::dIntegrate_dq2)
190  .def("dIntegrate_dq", LieGroupWrapper::dIntegrate_dq3)
191  .def("dIntegrate_dv", LieGroupWrapper::dIntegrate_dv1)
192  .def("dIntegrate_dv", LieGroupWrapper::dIntegrate_dv2)
193  .def("dIntegrate_dv", LieGroupWrapper::dIntegrate_dv3)
194  .def("dIntegrateTransport", LieGroupWrapper::dIntegrateTransport_proxy)
195 
196  .def("difference", LieGroupWrapper::difference)
197  .def("dDifference", LieGroupWrapper::dDifference1)
198  .def("dDifference", LieGroupWrapper::dDifference2)
199  .def("dDifference", LieGroupWrapper::dDifference3)
200 
201  .def("interpolate", LieGroupWrapper::interpolate)
202 
203  .def("random", static_cast<typename LieGroupType::ConfigVector_t (LieGroupType::*)() const>(&LieGroupType::random))
204  .def("randomConfiguration", &LieGroupType::template randomConfiguration<ConfigVector_t, ConfigVector_t>)
205  .def("distance", &LieGroupType::template distance<ConfigVector_t, ConfigVector_t>)
206  .def("squaredDistance", &LieGroupType::template squaredDistance<ConfigVector_t, ConfigVector_t>)
207  .def("normalize", &LieGroupType::template normalize<ConfigVector_t>)
208 
209  .add_property("name", &LieGroupType::name)
210  .add_property("neutral", &LieGroupType::neutral)
211  .add_property("nq", &LieGroupType::nq)
212  .add_property("nv", &LieGroupType::nv)
213 
214  .def(bp::self * bp::self)
215  .def(bp::self *= bp::self)
216  .def(bp::self == bp::self)
217  ;
218  }
219 
220  static void expose(const char* name)
221  {
222  bp::class_<LieGroupType>(name, bp::no_init)
224  }
225 };
226 } // namespace python
227 } // namespace pinocchio
228 
229 #endif // ifndef __pinocchio_python_geometry_model_hpp__
static ConfigVector_t integrate(const LieGroupType &lg, const ConfigVector_t &q, const TangentVector_t &v)
Definition: liegroups.hpp:28
static JacobianMatrix_t dIntegrate_dv3(const LieGroupType &lg, const ConfigVector_t &q, const TangentVector_t &v, int self, const JacobianMatrix_t &Jin)
Definition: liegroups.hpp:149
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
static JacobianMatrix_t dIntegrate(const LieGroupType &lg, const ConfigVector_t &q, const TangentVector_t &v, const ArgumentPosition arg)
Definition: liegroups.hpp:96
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: src/fwd.hpp:59
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space...
void integrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< ReturnType > &qout)
Integrate a configuration vector for the specified model for a tangent vector during one unit time...
void difference(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1, const Eigen::MatrixBase< ReturnType > &dvout)
Compute the tangent vector that must be integrated during one unit time to go from q0 to q1...
static JacobianMatrix_t dIntegrate_dq3(const LieGroupType &lg, const ConfigVector_t &q, const TangentVector_t &v, int self, const JacobianMatrix_t &Jin)
Definition: liegroups.hpp:123
static void expose(const char *name)
Definition: liegroups.hpp:220
Eigen::Matrix< double, Eigen::Dynamic, 1 > ConfigVector_t
Definition: liegroups.hpp:24
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
static TangentVector_t difference(const LieGroupType &lg, const ConfigVector_t &q0, const ConfigVector_t &q1)
Definition: liegroups.hpp:42
void def(const char *name, Func func)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
static JacobianMatrix_t dDifference1(const LieGroupType &lg, const ConfigVector_t &q0, const ConfigVector_t &q1, const ArgumentPosition arg)
Definition: liegroups.hpp:49
Eigen::Matrix< double, Eigen::Dynamic, 1 > TangentVector_t
Definition: liegroups.hpp:25
void dIntegrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< JacobianMatrixType > &J, const ArgumentPosition arg, const AssignmentOperatorType op=SETTO)
Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the...
static JacobianMatrix_t dIntegrate_dv2(const LieGroupType &lg, const ConfigVector_t &q, const TangentVector_t &v, const JacobianMatrix_t &Jin, int self)
Definition: liegroups.hpp:140
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > const Scalar & u
static JacobianMatrix_t dDifference3(const LieGroupType &lg, const ConfigVector_t &q0, const ConfigVector_t &q1, const ArgumentPosition arg, int self, const JacobianMatrix_t &Jin)
Definition: liegroups.hpp:77
static JacobianMatrix_t dIntegrate_dq2(const LieGroupType &lg, const ConfigVector_t &q, const TangentVector_t &v, const JacobianMatrix_t &Jin, int self)
Definition: liegroups.hpp:114
static JacobianMatrix_t dIntegrate_dv1(const LieGroupType &lg, const ConfigVector_t &q, const TangentVector_t &v)
Definition: liegroups.hpp:132
static JacobianMatrix_t dIntegrateTransport_proxy(const LieGroupType &lg, const ConfigVector_t &q, const TangentVector_t &v, const JacobianMatrix_t &J, const ArgumentPosition arg)
Definition: liegroups.hpp:159
static JacobianMatrix_t dIntegrate_dq1(const LieGroupType &lg, const ConfigVector_t &q, const TangentVector_t &v)
Definition: liegroups.hpp:106
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & q1
Eigen::MatrixXd dIntegrateTransport_proxy(const Model &model, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Eigen::MatrixXd &Jin, const ArgumentPosition arg)
Main pinocchio namespace.
Definition: timings.cpp:28
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > & q0
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
void random(const LieGroupGenericTpl< LieGroupCollection > &lg, const Eigen::MatrixBase< Config_t > &qout)
void interpolate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1, const Scalar &u, const Eigen::MatrixBase< ReturnType > &qout)
Interpolate two configurations for a given model.
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > JacobianMatrix_t
Definition: liegroups.hpp:26
static JacobianMatrix_t dDifference2(const LieGroupType &lg, const ConfigVector_t &q0, const ConfigVector_t &q1, const ArgumentPosition arg, const JacobianMatrix_t &Jin, int self)
Definition: liegroups.hpp:58
static ConfigVector_t interpolate(const LieGroupType &lg, const ConfigVector_t &q0, const ConfigVector_t &q1, const double &u)
Definition: liegroups.hpp:34


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:31