pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
liegroups.hpp
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
15namespace pinocchio
16{
17 namespace python
18 {
19 namespace bp = boost::python;
20
21 template<class LieGroupType>
22 struct LieGroupWrapperTpl
23 {
24 typedef Eigen::Matrix<context::Scalar, Eigen::Dynamic, 1> ConfigVector_t;
25 typedef Eigen::Matrix<context::Scalar, Eigen::Dynamic, 1> TangentVector_t;
26 typedef Eigen::Matrix<context::Scalar, Eigen::Dynamic, Eigen::Dynamic> JacobianMatrix_t;
27
28 static ConfigVector_t
29 integrate(const LieGroupType & lg, const ConfigVector_t & q, const TangentVector_t & v)
30 {
31 return lg.integrate(q, v);
32 }
33
34 static ConfigVector_t interpolate(
35 const LieGroupType & lg,
36 const ConfigVector_t & q0,
37 const ConfigVector_t & q1,
38 const context::Scalar & u)
39 {
40 return lg.interpolate(q0, q1, u);
41 }
42
43 static TangentVector_t
44 difference(const LieGroupType & lg, const ConfigVector_t & q0, const ConfigVector_t & q1)
45 {
46 return lg.difference(q0, q1);
47 }
48
49 static JacobianMatrix_t dDifference1(
50 const LieGroupType & lg,
51 const ConfigVector_t & q0,
52 const ConfigVector_t & q1,
53 const ArgumentPosition arg)
54 {
55 JacobianMatrix_t J(lg.nv(), lg.nv());
56 lg.dDifference(q0, q1, J, arg);
57 return J;
58 }
59
60 static JacobianMatrix_t dDifference2(
61 const LieGroupType & lg,
62 const ConfigVector_t & q0,
63 const ConfigVector_t & q1,
64 const ArgumentPosition arg,
65 const JacobianMatrix_t & Jin,
66 int self)
67 {
68 JacobianMatrix_t J(Jin.rows(), Jin.cols());
69 switch (arg)
70 {
71 case ARG0:
72 lg.template dDifference<ARG0>(q0, q1, Jin, self, J, SETTO);
73 break;
74 case ARG1:
75 lg.template dDifference<ARG1>(q0, q1, Jin, self, J, SETTO);
76 break;
77 default:
78 throw std::invalid_argument("arg must be either ARG0 or ARG1");
79 }
80 return J;
81 }
82
83 static JacobianMatrix_t dDifference3(
84 const LieGroupType & lg,
85 const ConfigVector_t & q0,
86 const ConfigVector_t & q1,
87 const ArgumentPosition arg,
88 int self,
89 const JacobianMatrix_t & Jin)
90 {
91 JacobianMatrix_t J(Jin.rows(), Jin.cols());
92 switch (arg)
93 {
94 case ARG0:
95 lg.template dDifference<ARG0>(q0, q1, self, Jin, J, SETTO);
96 break;
97 case ARG1:
98 lg.template dDifference<ARG1>(q0, q1, self, Jin, J, SETTO);
99 break;
100 default:
101 throw std::invalid_argument("arg must be either ARG0 or ARG1");
102 }
103 return J;
104 }
105
106 static JacobianMatrix_t dIntegrate(
107 const LieGroupType & lg,
108 const ConfigVector_t & q,
109 const TangentVector_t & v,
110 const ArgumentPosition arg)
111 {
112 JacobianMatrix_t J(lg.nv(), lg.nv());
113 lg.dIntegrate(q, v, J, arg);
114 return J;
115 }
116
117 static JacobianMatrix_t
118 dIntegrate_dq1(const LieGroupType & lg, const ConfigVector_t & q, const TangentVector_t & v)
119 {
120 JacobianMatrix_t J(lg.nv(), lg.nv());
121 lg.dIntegrate_dq(q, v, J);
122 return J;
123 }
124
125 static JacobianMatrix_t dIntegrate_dq2(
126 const LieGroupType & lg,
127 const ConfigVector_t & q,
128 const TangentVector_t & v,
129 const JacobianMatrix_t & Jin,
130 int self)
131 {
132 JacobianMatrix_t J(Jin.rows(), lg.nv());
133 lg.dIntegrate_dq(q, v, Jin, self, J, SETTO);
134 return J;
135 }
136
137 static JacobianMatrix_t dIntegrate_dq3(
138 const LieGroupType & lg,
139 const ConfigVector_t & q,
140 const TangentVector_t & v,
141 int self,
142 const JacobianMatrix_t & Jin)
143 {
144 JacobianMatrix_t J(lg.nv(), Jin.cols());
145 lg.dIntegrate_dq(q, v, self, Jin, J, SETTO);
146 return J;
147 }
148
149 static JacobianMatrix_t
150 dIntegrate_dv1(const LieGroupType & lg, const ConfigVector_t & q, const TangentVector_t & v)
151 {
152 JacobianMatrix_t J(lg.nv(), lg.nv());
153 lg.dIntegrate_dv(q, v, J);
154 return J;
155 }
156
157 static JacobianMatrix_t dIntegrate_dv2(
158 const LieGroupType & lg,
159 const ConfigVector_t & q,
160 const TangentVector_t & v,
161 const JacobianMatrix_t & Jin,
162 int self)
163 {
164 JacobianMatrix_t J(Jin.rows(), lg.nv());
165 lg.dIntegrate_dv(q, v, Jin, self, J, SETTO);
166 return J;
167 }
168
169 static JacobianMatrix_t dIntegrate_dv3(
170 const LieGroupType & lg,
171 const ConfigVector_t & q,
172 const TangentVector_t & v,
173 int self,
174 const JacobianMatrix_t & Jin)
175 {
176 JacobianMatrix_t J(lg.nv(), Jin.cols());
177 lg.dIntegrate_dv(q, v, self, Jin, J, SETTO);
178 return J;
179 }
180
181 static JacobianMatrix_t dIntegrateTransport_proxy(
182 const LieGroupType & lg,
183 const ConfigVector_t & q,
184 const TangentVector_t & v,
185 const JacobianMatrix_t & J,
186 const ArgumentPosition arg)
187 {
188 JacobianMatrix_t Jout(lg.nv(), J.cols());
189 lg.dIntegrateTransport(q, v, J, Jout, arg);
190 return Jout;
191 }
192 };
193
194 template<class LieGroupType>
195 struct LieGroupPythonVisitor
196 : public boost::python::def_visitor<LieGroupPythonVisitor<LieGroupType>>
197 {
198 public:
199 /* --- Exposing C++ API to python through the handler ----------------- */
200 template<class PyClass>
201 void visit(PyClass & cl) const
202 {
203 typedef Eigen::Matrix<context::Scalar, Eigen::Dynamic, 1> ConfigVector_t;
204
205 typedef LieGroupWrapperTpl<LieGroupType> LieGroupWrapper;
206 cl.def(bp::init<>("Default constructor"))
207 .def("integrate", LieGroupWrapper::integrate)
208 .def("dIntegrate", LieGroupWrapper::dIntegrate)
209 .def("dIntegrate_dq", LieGroupWrapper::dIntegrate_dq1)
210 .def("dIntegrate_dq", LieGroupWrapper::dIntegrate_dq2)
211 .def("dIntegrate_dq", LieGroupWrapper::dIntegrate_dq3)
212 .def("dIntegrate_dv", LieGroupWrapper::dIntegrate_dv1)
213 .def("dIntegrate_dv", LieGroupWrapper::dIntegrate_dv2)
214 .def("dIntegrate_dv", LieGroupWrapper::dIntegrate_dv3)
215 .def("dIntegrateTransport", LieGroupWrapper::dIntegrateTransport_proxy)
216 .def("difference", LieGroupWrapper::difference)
217 .def("dDifference", LieGroupWrapper::dDifference1)
218 .def("dDifference", LieGroupWrapper::dDifference2)
219 .def("dDifference", LieGroupWrapper::dDifference3)
220
221 .def("interpolate", LieGroupWrapper::interpolate)
222
223 .def(
224 "random", static_cast<typename LieGroupType::ConfigVector_t (LieGroupType::*)() const>(
225 &LieGroupType::random))
226 .def(
227 "randomConfiguration",
228 &LieGroupType::template randomConfiguration<ConfigVector_t, ConfigVector_t>)
229 .def("distance", &LieGroupType::template distance<ConfigVector_t, ConfigVector_t>)
230 .def(
231 "squaredDistance",
232 &LieGroupType::template squaredDistance<ConfigVector_t, ConfigVector_t>)
233 .def("normalize", &LieGroupType::template normalize<ConfigVector_t>)
234
235 .add_property("name", &LieGroupType::name)
236 .add_property("neutral", &LieGroupType::neutral)
237 .add_property("nq", &LieGroupType::nq)
238 .add_property("nv", &LieGroupType::nv)
239
240 .def(bp::self * bp::self)
241 .def(bp::self *= bp::self)
242#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
243 .def(bp::self == bp::self)
244#endif
245 ;
246 }
247
248 static void expose(const char * name)
249 {
250 bp::class_<LieGroupType>(name, bp::no_init).def(LieGroupPythonVisitor<LieGroupType>());
251 }
252 };
253 } // namespace python
254} // namespace pinocchio
255
256#endif // ifndef __pinocchio_python_geometry_model_hpp__
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11
pinocchio::ArgumentPosition
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition fwd.hpp:122
pinocchio::name
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.