pinocchio 4.1.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#pragma once
6
7#include <eigenpy/memory.hpp>
8
9#include "pinocchio/multibody/liegroup.hpp"
10
11namespace pinocchio
12{
13 namespace python
14 {
15 namespace bp = boost::python;
16
17 template<class LieGroupType>
18 struct LieGroupWrapperTpl
19 {
20 typedef Eigen::Matrix<context::Scalar, Eigen::Dynamic, 1> ConfigVector_t;
21 typedef Eigen::Matrix<context::Scalar, Eigen::Dynamic, 1> TangentVector_t;
22 typedef Eigen::Matrix<context::Scalar, Eigen::Dynamic, Eigen::Dynamic> JacobianMatrix_t;
23
24 static ConfigVector_t
25 integrate(const LieGroupType & lg, const ConfigVector_t & q, const TangentVector_t & v)
26 {
27 return lg.integrate(q, v);
28 }
29
30 static ConfigVector_t interpolate(
31 const LieGroupType & lg,
32 const ConfigVector_t & q0,
33 const ConfigVector_t & q1,
34 const context::Scalar & u)
35 {
36 return lg.interpolate(q0, q1, u);
37 }
38
39 static TangentVector_t
40 difference(const LieGroupType & lg, const ConfigVector_t & q0, const ConfigVector_t & q1)
41 {
42 return lg.difference(q0, q1);
43 }
44
45 static JacobianMatrix_t dDifference1(
46 const LieGroupType & lg,
47 const ConfigVector_t & q0,
48 const ConfigVector_t & q1,
49 const ArgumentPosition arg)
50 {
51 JacobianMatrix_t J(lg.nv(), lg.nv());
52 lg.dDifference(q0, q1, J, arg);
53 return J;
54 }
55
56 static JacobianMatrix_t dDifference2(
57 const LieGroupType & lg,
58 const ConfigVector_t & q0,
59 const ConfigVector_t & q1,
60 const ArgumentPosition arg,
61 const JacobianMatrix_t & Jin,
62 int self)
63 {
64 JacobianMatrix_t J(Jin.rows(), Jin.cols());
65 switch (arg)
66 {
67 case ARG0:
68 lg.template dDifference<ARG0>(q0, q1, Jin, self, J, SETTO);
69 break;
70 case ARG1:
71 lg.template dDifference<ARG1>(q0, q1, Jin, self, J, SETTO);
72 break;
73 default:
74 throw std::invalid_argument("arg must be either ARG0 or ARG1");
75 }
76 return J;
77 }
78
79 static JacobianMatrix_t dDifference3(
80 const LieGroupType & lg,
81 const ConfigVector_t & q0,
82 const ConfigVector_t & q1,
83 const ArgumentPosition arg,
84 int self,
85 const JacobianMatrix_t & Jin)
86 {
87 JacobianMatrix_t J(Jin.rows(), Jin.cols());
88 switch (arg)
89 {
90 case ARG0:
91 lg.template dDifference<ARG0>(q0, q1, self, Jin, J, SETTO);
92 break;
93 case ARG1:
94 lg.template dDifference<ARG1>(q0, q1, self, Jin, J, SETTO);
95 break;
96 default:
97 throw std::invalid_argument("arg must be either ARG0 or ARG1");
98 }
99 return J;
100 }
101
102 static JacobianMatrix_t dIntegrate(
103 const LieGroupType & lg,
104 const ConfigVector_t & q,
105 const TangentVector_t & v,
106 const ArgumentPosition arg)
107 {
108 JacobianMatrix_t J(lg.nv(), lg.nv());
109 lg.dIntegrate(q, v, J, arg);
110 return J;
111 }
112
113 static JacobianMatrix_t
114 dIntegrate_dq1(const LieGroupType & lg, const ConfigVector_t & q, const TangentVector_t & v)
115 {
116 JacobianMatrix_t J(lg.nv(), lg.nv());
117 lg.dIntegrate_dq(q, v, J);
118 return J;
119 }
120
121 static JacobianMatrix_t dIntegrate_dq2(
122 const LieGroupType & lg,
123 const ConfigVector_t & q,
124 const TangentVector_t & v,
125 const JacobianMatrix_t & Jin,
126 int self)
127 {
128 JacobianMatrix_t J(Jin.rows(), lg.nv());
129 lg.dIntegrate_dq(q, v, Jin, self, J, SETTO);
130 return J;
131 }
132
133 static JacobianMatrix_t dIntegrate_dq3(
134 const LieGroupType & lg,
135 const ConfigVector_t & q,
136 const TangentVector_t & v,
137 int self,
138 const JacobianMatrix_t & Jin)
139 {
140 JacobianMatrix_t J(lg.nv(), Jin.cols());
141 lg.dIntegrate_dq(q, v, self, Jin, J, SETTO);
142 return J;
143 }
144
145 static JacobianMatrix_t
146 dIntegrate_dv1(const LieGroupType & lg, const ConfigVector_t & q, const TangentVector_t & v)
147 {
148 JacobianMatrix_t J(lg.nv(), lg.nv());
149 lg.dIntegrate_dv(q, v, J);
150 return J;
151 }
152
153 static JacobianMatrix_t dIntegrate_dv2(
154 const LieGroupType & lg,
155 const ConfigVector_t & q,
156 const TangentVector_t & v,
157 const JacobianMatrix_t & Jin,
158 int self)
159 {
160 JacobianMatrix_t J(Jin.rows(), lg.nv());
161 lg.dIntegrate_dv(q, v, Jin, self, J, SETTO);
162 return J;
163 }
164
165 static JacobianMatrix_t dIntegrate_dv3(
166 const LieGroupType & lg,
167 const ConfigVector_t & q,
168 const TangentVector_t & v,
169 int self,
170 const JacobianMatrix_t & Jin)
171 {
172 JacobianMatrix_t J(lg.nv(), Jin.cols());
173 lg.dIntegrate_dv(q, v, self, Jin, J, SETTO);
174 return J;
175 }
176
177 static JacobianMatrix_t dIntegrateTransport_proxy(
178 const LieGroupType & lg,
179 const ConfigVector_t & q,
180 const TangentVector_t & v,
181 const JacobianMatrix_t & J,
182 const ArgumentPosition arg)
183 {
184 JacobianMatrix_t Jout(lg.nv(), J.cols());
185 lg.dIntegrateTransport(q, v, J, Jout, arg);
186 return Jout;
187 }
188 static JacobianMatrix_t tangentMap(const LieGroupType & lg, const ConfigVector_t & q)
189 {
190 JacobianMatrix_t TM(lg.nq(), lg.nv());
191 lg.tangentMap(q, TM, SETTO);
192 return TM;
193 }
194
195 static JacobianMatrix_t tangentMapProduct(
196 const LieGroupType & lg, const ConfigVector_t & q, const JacobianMatrix_t & Min)
197 {
198 JacobianMatrix_t Mout(lg.nq(), Min.cols());
199 lg.tangentMapProduct(q, Min, Mout, SETTO);
200 return Mout;
201 }
202
203 static JacobianMatrix_t tangentMapTransposeProduct(
204 const LieGroupType & lg, const ConfigVector_t & q, const JacobianMatrix_t & Min)
205 {
206 JacobianMatrix_t Mout(lg.nv(), Min.cols());
207 lg.tangentMapTransposeProduct(q, Min, Mout, SETTO);
208 return Mout;
209 }
210 };
211
212 template<class LieGroupType>
213 struct LieGroupPythonVisitor
214 : public boost::python::def_visitor<LieGroupPythonVisitor<LieGroupType>>
215 {
216 public:
217 /* --- Exposing C++ API to python through the handler ----------------- */
218 template<class PyClass>
219 void visit(PyClass & cl) const
220 {
221 typedef Eigen::Matrix<context::Scalar, Eigen::Dynamic, 1> ConfigVector_t;
222
223 typedef LieGroupWrapperTpl<LieGroupType> LieGroupWrapper;
224 cl.def(bp::init<>("Default constructor"))
225 .def("integrate", LieGroupWrapper::integrate)
226 .def("dIntegrate", LieGroupWrapper::dIntegrate)
227 .def("dIntegrate_dq", LieGroupWrapper::dIntegrate_dq1)
228 .def("dIntegrate_dq", LieGroupWrapper::dIntegrate_dq2)
229 .def("dIntegrate_dq", LieGroupWrapper::dIntegrate_dq3)
230 .def("dIntegrate_dv", LieGroupWrapper::dIntegrate_dv1)
231 .def("dIntegrate_dv", LieGroupWrapper::dIntegrate_dv2)
232 .def("dIntegrate_dv", LieGroupWrapper::dIntegrate_dv3)
233 .def("dIntegrateTransport", LieGroupWrapper::dIntegrateTransport_proxy)
234 .def("difference", LieGroupWrapper::difference)
235 .def("dDifference", LieGroupWrapper::dDifference1)
236 .def("dDifference", LieGroupWrapper::dDifference2)
237 .def("dDifference", LieGroupWrapper::dDifference3)
238 .def("tangentMap", LieGroupWrapper::tangentMap)
239 .def("tangentMapProduct", LieGroupWrapper::tangentMapProduct)
240 .def("tangentMapTransposeProduct", LieGroupWrapper::tangentMapTransposeProduct)
241
242 .def("interpolate", LieGroupWrapper::interpolate)
243
244 .def(
245 "random", static_cast<typename LieGroupType::ConfigVector_t (LieGroupType::*)() const>(
246 &LieGroupType::random))
247 .def(
248 "randomConfiguration",
249 &LieGroupType::template randomConfiguration<ConfigVector_t, ConfigVector_t>)
250 .def("distance", &LieGroupType::template distance<ConfigVector_t, ConfigVector_t>)
251 .def(
252 "squaredDistance",
253 &LieGroupType::template squaredDistance<ConfigVector_t, ConfigVector_t>)
254 .def("normalize", &LieGroupType::template normalize<ConfigVector_t>)
255
256 .add_property("name", &LieGroupType::name)
257 .add_property("neutral", &LieGroupType::neutral)
258 .add_property("nq", &LieGroupType::nq)
259 .add_property("nv", &LieGroupType::nv)
260
261 .def(bp::self * bp::self)
262 .def(bp::self *= bp::self)
263#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
264 .def(bp::self == bp::self)
265#endif
266 ;
267 }
268
269 static void expose(const char * name)
270 {
271 bp::class_<LieGroupType>(name, bp::no_init).def(LieGroupPythonVisitor<LieGroupType>());
272 }
273 };
274 } // namespace python
275} // namespace pinocchio
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
pinocchio::distance
Scalar distance(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1)
Distance between two configuration vectors, namely .
pinocchio::randomConfiguration
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
pinocchio::squaredDistance
void squaredDistance(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1, const Eigen::MatrixBase< ReturnType > &out)
Squared distance between two configuration vectors.
pinocchio::dDifference
void dDifference(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVector1 > &q0, const Eigen::MatrixBase< ConfigVector2 > &q1, const Eigen::MatrixBase< JacobianMatrix > &J, const ArgumentPosition arg)
Computes the Jacobian of a small variation of the configuration vector into the tangent space at iden...