1 |
|
|
// |
2 |
|
|
// Copyright (c) 2015-2020 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include "pinocchio/bindings/python/algorithm/algorithms.hpp" |
6 |
|
|
#include "pinocchio/algorithm/joint-configuration.hpp" |
7 |
|
|
|
8 |
|
|
namespace pinocchio |
9 |
|
|
{ |
10 |
|
|
namespace python |
11 |
|
|
{ |
12 |
|
|
|
13 |
✗✗ |
38 |
BOOST_PYTHON_FUNCTION_OVERLOADS(isNormalized_overload,isNormalized,2,3) |
14 |
|
|
|
15 |
|
|
static Eigen::VectorXd normalize_proxy(const Model & model, |
16 |
|
|
const Eigen::VectorXd & config) |
17 |
|
|
{ |
18 |
|
|
Eigen::VectorXd q(config); |
19 |
|
|
normalize(model,q); |
20 |
|
|
return q; |
21 |
|
|
} |
22 |
|
|
|
23 |
|
5 |
static Eigen::VectorXd randomConfiguration_proxy(const Model & model) |
24 |
|
|
{ |
25 |
|
5 |
return randomConfiguration(model); |
26 |
|
|
} |
27 |
|
|
|
28 |
|
|
bp::tuple dIntegrate_proxy(const Model & model, |
29 |
|
|
const Eigen::VectorXd & q, |
30 |
|
|
const Eigen::VectorXd & v) |
31 |
|
|
{ |
32 |
|
|
Eigen::MatrixXd J0(Eigen::MatrixXd::Zero(model.nv,model.nv)); |
33 |
|
|
Eigen::MatrixXd J1(Eigen::MatrixXd::Zero(model.nv,model.nv)); |
34 |
|
|
|
35 |
|
|
dIntegrate(model,q,v,J0,ARG0); |
36 |
|
|
dIntegrate(model,q,v,J1,ARG1); |
37 |
|
|
|
38 |
|
|
return bp::make_tuple(J0,J1); |
39 |
|
|
} |
40 |
|
|
|
41 |
|
|
Eigen::MatrixXd dIntegrate_arg_proxy(const Model & model, |
42 |
|
|
const Eigen::VectorXd & q, |
43 |
|
|
const Eigen::VectorXd & v, |
44 |
|
|
const ArgumentPosition arg) |
45 |
|
|
{ |
46 |
|
|
Eigen::MatrixXd J(Eigen::MatrixXd::Zero(model.nv,model.nv)); |
47 |
|
|
|
48 |
|
|
dIntegrate(model,q,v,J,arg, SETTO); |
49 |
|
|
|
50 |
|
|
return J; |
51 |
|
|
} |
52 |
|
|
|
53 |
|
|
Eigen::MatrixXd dIntegrateTransport_proxy(const Model & model, |
54 |
|
|
const Eigen::VectorXd & q, |
55 |
|
|
const Eigen::VectorXd & v, |
56 |
|
|
const Eigen::MatrixXd & Jin, |
57 |
|
|
const ArgumentPosition arg) |
58 |
|
|
{ |
59 |
|
|
int ncols = Jin.cols(); |
60 |
|
|
Eigen::MatrixXd Jout(Eigen::MatrixXd::Zero(model.nv,ncols)); |
61 |
|
|
dIntegrateTransport(model, q, v, Jin, Jout, arg); |
62 |
|
|
return Jout; |
63 |
|
|
} |
64 |
|
|
|
65 |
|
|
bp::tuple dDifference_proxy(const Model & model, |
66 |
|
|
const Eigen::VectorXd & q1, |
67 |
|
|
const Eigen::VectorXd & q2) |
68 |
|
|
{ |
69 |
|
|
Eigen::MatrixXd J0(Eigen::MatrixXd::Zero(model.nv,model.nv)); |
70 |
|
|
Eigen::MatrixXd J1(Eigen::MatrixXd::Zero(model.nv,model.nv)); |
71 |
|
|
|
72 |
|
|
dDifference(model,q1,q2,J0,ARG0); |
73 |
|
|
dDifference(model,q1,q2,J1,ARG1); |
74 |
|
|
|
75 |
|
|
return bp::make_tuple(J0,J1); |
76 |
|
|
} |
77 |
|
|
|
78 |
|
|
Eigen::MatrixXd dDifference_arg_proxy(const Model & model, |
79 |
|
|
const Eigen::VectorXd & q1, |
80 |
|
|
const Eigen::VectorXd & q2, |
81 |
|
|
const ArgumentPosition arg) |
82 |
|
|
{ |
83 |
|
|
Eigen::MatrixXd J(Eigen::MatrixXd::Zero(model.nv,model.nv)); |
84 |
|
|
|
85 |
|
|
dDifference(model,q1,q2,J,arg); |
86 |
|
|
|
87 |
|
|
return J; |
88 |
|
|
} |
89 |
|
|
|
90 |
|
19 |
void exposeJointsAlgo() |
91 |
|
|
{ |
92 |
|
|
using namespace Eigen; |
93 |
|
|
|
94 |
✓✗ |
19 |
bp::def("integrate", |
95 |
|
|
&integrate<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>, |
96 |
|
38 |
bp::args("model","q","v"), |
97 |
|
|
"Integrate the joint configuration vector q with a tangent vector v during one unit time.\n" |
98 |
|
|
"This is the canonical integrator of a Configuration Space composed of Lie groups, such as most robots.\n\n" |
99 |
|
|
"Parameters:\n" |
100 |
|
|
"\tmodel: model of the kinematic tree\n" |
101 |
|
|
"\tq: the joint configuration vector (size model.nq)\n" |
102 |
|
|
"\tv: the joint velocity vector (size model.nv)\n"); |
103 |
|
|
|
104 |
✓✗ |
19 |
bp::def("dIntegrate", |
105 |
|
|
&dIntegrate_proxy, |
106 |
|
38 |
bp::args("model","q","v"), |
107 |
|
|
"Computes the partial derivatives of the integrate function with respect to the first " |
108 |
|
|
"and the second argument, and returns the two Jacobians as a tuple.\n\n" |
109 |
|
|
"Parameters:\n" |
110 |
|
|
"\tmodel: model of the kinematic tree\n" |
111 |
|
|
"\tq: the joint configuration vector (size model.nq)\n" |
112 |
|
|
"\tv: the joint velocity vector (size model.nv)\n"); |
113 |
|
|
|
114 |
✓✗ |
19 |
bp::def("dIntegrate", |
115 |
|
|
&dIntegrate_arg_proxy, |
116 |
|
38 |
bp::args("model","q","v","argument_position"), |
117 |
|
|
"Computes the partial derivatives of the integrate function with respect to the first (arg == ARG0) " |
118 |
|
|
"or the second argument (arg == ARG1).\n\n" |
119 |
|
|
"Parameters:\n" |
120 |
|
|
"\tmodel: model of the kinematic tree\n" |
121 |
|
|
"\tq: the joint configuration vector (size model.nq)\n" |
122 |
|
|
"\tv: the joint velocity vector (size model.nv)\n" |
123 |
|
|
"\targument_position: either pinocchio.ArgumentPosition.ARG0 or pinocchio.ArgumentPosition.ARG1, depending on the desired Jacobian value.\n"); |
124 |
|
|
|
125 |
✓✗ |
19 |
bp::def("dIntegrateTransport", |
126 |
|
|
&dIntegrateTransport_proxy, |
127 |
|
38 |
bp::args("model","q","v","Jin","argument_position"), |
128 |
|
|
"Takes a matrix expressed at q (+) v and uses parallel transport to express it in the tangent space at q." |
129 |
|
|
"\tThis operation does the product of the matrix by the Jacobian of the integration operation, but more efficiently." |
130 |
|
|
"Parameters:\n" |
131 |
|
|
"\tmodel: model of the kinematic tree\n" |
132 |
|
|
"\tq: the joint configuration vector (size model.nq)\n" |
133 |
|
|
"\tv: the joint velocity vector (size model.nv)\n" |
134 |
|
|
"\tJin: the input matrix (row size model.nv)" |
135 |
|
|
"\targument_position: either pinocchio.ArgumentPosition.ARG0 (q) or pinocchio.ArgumentPosition.ARG1 (v), depending on the desired Jacobian value.\n"); |
136 |
|
|
|
137 |
✓✗ |
19 |
bp::def("interpolate", |
138 |
|
|
&interpolate<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>, |
139 |
|
38 |
bp::args("model","q1","q2","alpha"), |
140 |
|
|
"Interpolate between two given joint configuration vectors q1 and q2.\n\n" |
141 |
|
|
"Parameters:\n" |
142 |
|
|
"\tmodel: model of the kinematic tree\n" |
143 |
|
|
"\tq1: the initial joint configuration vector (size model.nq)\n" |
144 |
|
|
"\tq2: the terminal joint configuration vector (size model.nq)\n" |
145 |
|
|
"\talpha: the interpolation coefficient in [0,1]\n"); |
146 |
|
|
|
147 |
✓✗ |
19 |
bp::def("difference", |
148 |
|
|
&difference<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>, |
149 |
|
38 |
bp::args("model","q1","q2"), |
150 |
|
|
"Difference between two joint configuration vectors, i.e. the tangent vector that must be integrated during one unit time" |
151 |
|
|
"to go from q1 to q2.\n\n" |
152 |
|
|
"Parameters:\n" |
153 |
|
|
"\tmodel: model of the kinematic tree\n" |
154 |
|
|
"\tq1: the initial joint configuration vector (size model.nq)\n" |
155 |
|
|
"\tq2: the terminal joint configuration vector (size model.nq)\n"); |
156 |
|
|
|
157 |
✓✗ |
19 |
bp::def("squaredDistance", |
158 |
|
|
&squaredDistance<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>, |
159 |
|
38 |
bp::args("model","q1","q2"), |
160 |
|
|
"Squared distance vector between two joint configuration vectors.\n\n" |
161 |
|
|
"Parameters:\n" |
162 |
|
|
"\tmodel: model of the kinematic tree\n" |
163 |
|
|
"\tq1: the initial joint configuration vector (size model.nq)\n" |
164 |
|
|
"\tq2: the terminal joint configuration vector (size model.nq)\n"); |
165 |
|
|
|
166 |
✓✗ |
19 |
bp::def("distance", |
167 |
|
|
&distance<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>, |
168 |
|
38 |
bp::args("model","q1","q2"), |
169 |
|
|
"Distance between two joint configuration vectors.\n\n" |
170 |
|
|
"Parameters:\n" |
171 |
|
|
"\tmodel: model of the kinematic tree\n" |
172 |
|
|
"\tq1: the initial joint configuration vector (size model.nq)\n" |
173 |
|
|
"\tq2: the terminal joint configuration vector (size model.nq)\n"); |
174 |
|
|
|
175 |
✓✗ |
19 |
bp::def("dDifference", |
176 |
|
|
&dDifference_proxy, |
177 |
|
38 |
bp::args("model","q1","q2"), |
178 |
|
|
"Computes the partial derivatives of the difference function with respect to the first " |
179 |
|
|
"and the second argument, and returns the two Jacobians as a tuple.\n\n" |
180 |
|
|
"Parameters:\n" |
181 |
|
|
"\tmodel: model of the kinematic tree\n" |
182 |
|
|
"\tq1: the initial joint configuration vector (size model.nq)\n" |
183 |
|
|
"\tq2: the terminal joint configuration vector (size model.nq)\n"); |
184 |
|
|
|
185 |
✓✗ |
19 |
bp::def("dDifference", |
186 |
|
|
&dDifference_arg_proxy, |
187 |
|
38 |
bp::args("model","q1","q2","argument_position"), |
188 |
|
|
"Computes the partial derivatives of the difference function with respect to the first (arg == ARG0) " |
189 |
|
|
"or the second argument (arg == ARG1).\n\n" |
190 |
|
|
"Parameters:\n" |
191 |
|
|
"\tmodel: model of the kinematic tree\n" |
192 |
|
|
"\tq1: the initial joint configuration vector (size model.nq)\n" |
193 |
|
|
"\tq2: the terminal joint configuration vector (size model.nq)\n" |
194 |
|
|
"\targument_position: either pinocchio.ArgumentPosition.ARG0 or pinocchio.ArgumentPosition.ARG1, depending on the desired Jacobian value.\n"); |
195 |
|
|
|
196 |
✓✗ |
19 |
bp::def("randomConfiguration", |
197 |
|
|
&randomConfiguration_proxy, |
198 |
|
38 |
bp::arg("model"), |
199 |
|
|
"Generate a random configuration in the bounds given by the lower and upper limits contained in model.\n\n" |
200 |
|
|
"Parameters:\n" |
201 |
|
|
"\tmodel: model of the kinematic tree\n"); |
202 |
|
|
|
203 |
✓✗ |
19 |
bp::def("randomConfiguration", |
204 |
|
|
&randomConfiguration<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>, |
205 |
|
38 |
bp::args("model","lower_bound","upper_bound"), |
206 |
|
|
"Generate a random configuration in the bounds given by the Joint lower and upper limits arguments.\n\n" |
207 |
|
|
"Parameters:\n" |
208 |
|
|
"\tmodel: model of the kinematic tree\n" |
209 |
|
|
"\tlower_bound: the lower bound on the joint configuration vectors (size model.nq)\n" |
210 |
|
|
"\tupper_bound: the upper bound on the joint configuration vectors (size model.nq)\n"); |
211 |
|
|
|
212 |
✓✗ |
19 |
bp::def("neutral", |
213 |
|
|
&neutral<double,0,JointCollectionDefaultTpl>, |
214 |
|
38 |
bp::arg("model"), |
215 |
|
|
"Returns the neutral configuration vector associated to the model.\n\n" |
216 |
|
|
"Parameters:\n" |
217 |
|
|
"\tmodel: model of the kinematic tree\n"); |
218 |
|
|
|
219 |
✓✗ |
19 |
bp::def("normalize",normalize_proxy, |
220 |
|
38 |
bp::args("model","q"), |
221 |
|
|
"Returns the configuration normalized.\n" |
222 |
|
|
"For instance, when the configuration vectors contains some quaternion values, it must be required to renormalize these components to keep orthonormal rotation values.\n\n" |
223 |
|
|
"Parameters:\n" |
224 |
|
|
"\tmodel: model of the kinematic tree\n" |
225 |
|
|
"\tq: a joint configuration vector to normalize (size model.nq)\n"); |
226 |
|
|
|
227 |
✓✗ |
19 |
bp::def("isSameConfiguration", |
228 |
|
|
&isSameConfiguration<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>, |
229 |
|
38 |
bp::args("model","q1","q2","prec"), |
230 |
|
|
"Return true if two configurations are equivalent within the given precision provided by prec.\n\n" |
231 |
|
|
"Parameters:\n" |
232 |
|
|
"\tmodel: model of the kinematic tree\n" |
233 |
|
|
"\tq1: a joint configuration vector (size model.nq)\n" |
234 |
|
|
"\tq2: a joint configuration vector (size model.nq)\n" |
235 |
|
|
"\tprec: requested accuracy for the comparison\n"); |
236 |
|
|
|
237 |
✓✗ |
19 |
bp::def("isNormalized", |
238 |
|
|
&isNormalized<double,0,JointCollectionDefaultTpl,VectorXd>, |
239 |
✓✗ |
19 |
isNormalized_overload( |
240 |
|
38 |
bp::args("model","q","prec"), |
241 |
|
|
"Check whether a configuration vector is normalized within the given precision provided by prec.\n\n" |
242 |
|
|
"Parameters:\n" |
243 |
|
|
"\tmodel: model of the kinematic tree\n" |
244 |
|
|
"\tq: a joint configuration vector (size model.nq)\n" |
245 |
|
|
"\tprec: requested accuracy for the check\n" |
246 |
|
|
) |
247 |
|
|
); |
248 |
|
19 |
} |
249 |
|
|
|
250 |
|
|
} // namespace python |
251 |
|
|
} // namespace pinocchio |