1 /*
2 * Copyright (c) 2011-2021, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include <dart/dart.hpp>
34 #include <pybind11/pybind11.h>
35 #include <pybind11/stl.h>
36
37 #include "eigen_geometry_pybind.h"
38 #include "eigen_pybind.h"
39
40 namespace py = pybind11;
41
42 namespace dart {
43 namespace python {
44
InverseKinematics(py::module & m)45 void InverseKinematics(py::module& m)
46 {
47 ::py::class_<dart::dynamics::InverseKinematics::ErrorMethod::Properties>(
48 m, "InverseKinematicsErrorMethodProperties")
49 .def(
50 ::py::init<
51 const dart::dynamics::InverseKinematics::ErrorMethod::Bounds&,
52 double,
53 const Eigen::Vector6d&>(),
54 ::py::arg("bounds")
55 = dart::dynamics::InverseKinematics::ErrorMethod::Bounds(
56 Eigen::Vector6d::Constant(-dart::dynamics::DefaultIKTolerance),
57 Eigen::Vector6d::Constant(dart::dynamics::DefaultIKTolerance)),
58 ::py::arg("errorClamp") = dart::dynamics::DefaultIKErrorClamp,
59 ::py::arg("errorWeights") = Eigen::compose(
60 Eigen::Vector3d::Constant(dart::dynamics::DefaultIKAngularWeight),
61 Eigen::Vector3d::Constant(dart::dynamics::DefaultIKLinearWeight)))
62 .def_readwrite(
63 "mBounds",
64 &dart::dynamics::InverseKinematics::ErrorMethod::Properties::mBounds)
65 .def_readwrite(
66 "mErrorLengthClamp",
67 &dart::dynamics::InverseKinematics::ErrorMethod::Properties::
68 mErrorLengthClamp)
69 .def_readwrite(
70 "mErrorWeights",
71 &dart::dynamics::InverseKinematics::ErrorMethod::Properties::
72 mErrorWeights);
73
74 ::py::class_<
75 dart::dynamics::InverseKinematics::ErrorMethod,
76 dart::common::Subject,
77 std::shared_ptr<dart::dynamics::InverseKinematics::ErrorMethod>>(
78 m, "InverseKinematicsErrorMethod")
79 .def(
80 "clone",
81 +[](const dart::dynamics::InverseKinematics::ErrorMethod* self,
82 dart::dynamics::InverseKinematics* _newIK)
83 -> std::unique_ptr<
84 dart::dynamics::InverseKinematics::ErrorMethod> {
85 return self->clone(_newIK);
86 },
87 ::py::arg("newIK"))
88 .def(
89 "computeError",
90 +[](dart::dynamics::InverseKinematics::ErrorMethod* self)
91 -> Eigen::Vector6d { return self->computeError(); })
92 .def(
93 "computeDesiredTransform",
94 +[](dart::dynamics::InverseKinematics::ErrorMethod* self,
95 const Eigen::Isometry3d& _currentTf,
96 const Eigen::Vector6d& _error) -> Eigen::Isometry3d {
97 return self->computeDesiredTransform(_currentTf, _error);
98 },
99 ::py::arg("currentTf"),
100 ::py::arg("error"))
101 .def(
102 "getMethodName",
103 +[](const dart::dynamics::InverseKinematics::ErrorMethod* self)
104 -> const std::string& { return self->getMethodName(); },
105 ::py::return_value_policy::reference_internal)
106 .def(
107 "setBounds",
108 +[](dart::dynamics::InverseKinematics::ErrorMethod* self) {
109 self->setBounds();
110 })
111 .def(
112 "setBounds",
113 +[](dart::dynamics::InverseKinematics::ErrorMethod* self,
114 const Eigen::Vector6d& _lower) { self->setBounds(_lower); },
115 ::py::arg("lower"))
116 .def(
117 "setBounds",
118 +[](dart::dynamics::InverseKinematics::ErrorMethod* self,
119 const Eigen::Vector6d& _lower,
120 const Eigen::Vector6d& _upper) {
121 self->setBounds(_lower, _upper);
122 },
123 ::py::arg("lower"),
124 ::py::arg("upper"))
125 .def(
126 "setBounds",
127 +[](dart::dynamics::InverseKinematics::ErrorMethod* self,
128 const std::pair<Eigen::Vector6d, Eigen::Vector6d>& _bounds) {
129 self->setBounds(_bounds);
130 },
131 ::py::arg("bounds"))
132 .def(
133 "getBounds",
134 +[](const dart::dynamics::InverseKinematics::ErrorMethod* self)
135 -> const std::pair<Eigen::Vector6d, Eigen::Vector6d>& {
136 return self->getBounds();
137 })
138 .def(
139 "setAngularBounds",
140 +[](dart::dynamics::InverseKinematics::ErrorMethod* self) {
141 self->setAngularBounds();
142 })
143 .def(
144 "setAngularBounds",
145 +[](dart::dynamics::InverseKinematics::ErrorMethod* self,
146 const Eigen::Vector3d& _lower) {
147 self->setAngularBounds(_lower);
148 },
149 ::py::arg("lower"))
150 .def(
151 "setAngularBounds",
152 +[](dart::dynamics::InverseKinematics::ErrorMethod* self,
153 const Eigen::Vector3d& _lower,
154 const Eigen::Vector3d& _upper) {
155 self->setAngularBounds(_lower, _upper);
156 },
157 ::py::arg("lower"),
158 ::py::arg("upper"))
159 .def(
160 "setAngularBounds",
161 +[](dart::dynamics::InverseKinematics::ErrorMethod* self,
162 const std::pair<Eigen::Vector3d, Eigen::Vector3d>& _bounds) {
163 self->setAngularBounds(_bounds);
164 },
165 ::py::arg("bounds"))
166 .def(
167 "getAngularBounds",
168 +[](const dart::dynamics::InverseKinematics::ErrorMethod* self)
169 -> std::pair<Eigen::Vector3d, Eigen::Vector3d> {
170 return self->getAngularBounds();
171 })
172 .def(
173 "setLinearBounds",
174 +[](dart::dynamics::InverseKinematics::ErrorMethod* self) {
175 self->setLinearBounds();
176 })
177 .def(
178 "setLinearBounds",
179 +[](dart::dynamics::InverseKinematics::ErrorMethod* self,
180 const Eigen::Vector3d& _lower) { self->setLinearBounds(_lower); },
181 ::py::arg("lower"))
182 .def(
183 "setLinearBounds",
184 +[](dart::dynamics::InverseKinematics::ErrorMethod* self,
185 const Eigen::Vector3d& _lower,
186 const Eigen::Vector3d& _upper) {
187 self->setLinearBounds(_lower, _upper);
188 },
189 ::py::arg("lower"),
190 ::py::arg("upper"))
191 .def(
192 "setLinearBounds",
193 +[](dart::dynamics::InverseKinematics::ErrorMethod* self,
194 const std::pair<Eigen::Vector3d, Eigen::Vector3d>& _bounds) {
195 self->setLinearBounds(_bounds);
196 },
197 ::py::arg("bounds"))
198 .def(
199 "getLinearBounds",
200 +[](const dart::dynamics::InverseKinematics::ErrorMethod* self)
201 -> std::pair<Eigen::Vector3d, Eigen::Vector3d> {
202 return self->getLinearBounds();
203 })
204 .def(
205 "setErrorLengthClamp",
206 +[](dart::dynamics::InverseKinematics::ErrorMethod* self) {
207 self->setErrorLengthClamp();
208 })
209 .def(
210 "setErrorLengthClamp",
211 +[](dart::dynamics::InverseKinematics::ErrorMethod* self,
212 double _clampSize) { self->setErrorLengthClamp(_clampSize); },
213 ::py::arg("clampSize"))
214 .def(
215 "getErrorLengthClamp",
216 +[](const dart::dynamics::InverseKinematics::ErrorMethod* self)
217 -> double { return self->getErrorLengthClamp(); })
218 .def(
219 "setErrorWeights",
220 +[](dart::dynamics::InverseKinematics::ErrorMethod* self,
221 const Eigen::Vector6d& _weights) {
222 self->setErrorWeights(_weights);
223 },
224 ::py::arg("weights"))
225 .def(
226 "setAngularErrorWeights",
227 +[](dart::dynamics::InverseKinematics::ErrorMethod* self) {
228 self->setAngularErrorWeights();
229 })
230 .def(
231 "setAngularErrorWeights",
232 +[](dart::dynamics::InverseKinematics::ErrorMethod* self,
233 const Eigen::Vector3d& _weights) {
234 self->setAngularErrorWeights(_weights);
235 },
236 ::py::arg("weights"))
237 .def(
238 "getAngularErrorWeights",
239 +[](const dart::dynamics::InverseKinematics::ErrorMethod* self)
240 -> Eigen::Vector3d { return self->getAngularErrorWeights(); })
241 .def(
242 "setLinearErrorWeights",
243 +[](dart::dynamics::InverseKinematics::ErrorMethod* self) {
244 self->setLinearErrorWeights();
245 })
246 .def(
247 "setLinearErrorWeights",
248 +[](dart::dynamics::InverseKinematics::ErrorMethod* self,
249 const Eigen::Vector3d& _weights) {
250 self->setLinearErrorWeights(_weights);
251 },
252 ::py::arg("weights"))
253 .def(
254 "getLinearErrorWeights",
255 +[](const dart::dynamics::InverseKinematics::ErrorMethod* self)
256 -> Eigen::Vector3d { return self->getLinearErrorWeights(); })
257 .def(
258 "getErrorMethodProperties",
259 +[](const dart::dynamics::InverseKinematics::ErrorMethod* self)
260 -> dart::dynamics::InverseKinematics::ErrorMethod::Properties {
261 return self->getErrorMethodProperties();
262 })
263 .def(
264 "clearCache",
265 +[](dart::dynamics::InverseKinematics::ErrorMethod* self) {
266 self->clearCache();
267 });
268
269 ::py::class_<
270 dart::dynamics::InverseKinematics::TaskSpaceRegion::UniqueProperties>(
271 m, "InverseKinematicsTaskSpaceRegionUniqueProperties")
272 .def(
273 ::py::init<bool, dart::dynamics::SimpleFramePtr>(),
274 ::py::arg("computeErrorFromCenter") = true,
275 ::py::arg("referenceFrame") = nullptr)
276 .def_readwrite(
277 "mComputeErrorFromCenter",
278 &dart::dynamics::InverseKinematics::TaskSpaceRegion::
279 UniqueProperties::mComputeErrorFromCenter)
280 .def_readwrite(
281 "mReferenceFrame",
282 &dart::dynamics::InverseKinematics::TaskSpaceRegion::
283 UniqueProperties::mReferenceFrame);
284
285 ::py::class_<
286 dart::dynamics::InverseKinematics::TaskSpaceRegion::Properties,
287 dart::dynamics::InverseKinematics::ErrorMethod::Properties,
288 dart::dynamics::InverseKinematics::TaskSpaceRegion::UniqueProperties>(
289 m, "InverseKinematicsTaskSpaceRegionProperties")
290 .def(
291 ::py::init<
292 const dart::dynamics::InverseKinematics::ErrorMethod::Properties&,
293 const dart::dynamics::InverseKinematics::TaskSpaceRegion::
294 UniqueProperties&>(),
295 ::py::arg("errorProperties")
296 = dart::dynamics::InverseKinematics::ErrorMethod::Properties(),
297 ::py::arg("taskSpaceProperties") = dart::dynamics::InverseKinematics::
298 TaskSpaceRegion::UniqueProperties());
299
300 ::py::class_<
301 dart::dynamics::InverseKinematics::TaskSpaceRegion,
302 dart::dynamics::InverseKinematics::ErrorMethod,
303 std::shared_ptr<dart::dynamics::InverseKinematics::TaskSpaceRegion>>(
304 m, "InverseKinematicsTaskSpaceRegion")
305 .def(
306 ::py::init<
307 dart::dynamics::InverseKinematics*,
308 dart::dynamics::InverseKinematics::TaskSpaceRegion::Properties>(),
309 ::py::arg("ik"),
310 ::py::arg("properties")
311 = dart::dynamics::InverseKinematics::TaskSpaceRegion::Properties())
312 .def(
313 "setComputeFromCenter",
314 &dart::dynamics::InverseKinematics::TaskSpaceRegion::
315 setComputeFromCenter,
316 ::py::arg("computeFromCenter"),
317 "Set whether this TaskSpaceRegion should compute its error vector "
318 "from the center of the region.")
319 .def(
320 "isComputingFromCenter",
321 &dart::dynamics::InverseKinematics::TaskSpaceRegion::
322 isComputingFromCenter,
323 "Get whether this TaskSpaceRegion is set to compute its error vector "
324 "from the center of the region.")
325 .def(
326 "setReferenceFrame",
327 &dart::dynamics::InverseKinematics::TaskSpaceRegion::
328 setReferenceFrame,
329 ::py::arg("referenceFrame"),
330 "Set the reference frame that the task space region is expressed. "
331 "Pass None to use the parent frame of the target frame instead.")
332 .def(
333 "getReferenceFrame",
334 &dart::dynamics::InverseKinematics::TaskSpaceRegion::
335 getReferenceFrame,
336 "Get the reference frame that the task space region is expressed.")
337 .def(
338 "getTaskSpaceRegionProperties",
339 &dart::dynamics::InverseKinematics::TaskSpaceRegion::
340 getTaskSpaceRegionProperties,
341 "Get the Properties of this TaskSpaceRegion.");
342
343 ::py::class_<
344 dart::dynamics::InverseKinematics,
345 dart::common::Subject,
346 std::shared_ptr<dart::dynamics::InverseKinematics>>(
347 m, "InverseKinematics")
348 .def(
349 ::py::init(
350 +[](dart::dynamics::JacobianNode* node)
351 -> dart::dynamics::InverseKinematicsPtr {
352 return dart::dynamics::InverseKinematics::create(node);
353 }),
354 ::py::arg("node"))
355 .def(
356 "findSolution",
357 +[](dart::dynamics::InverseKinematics* self,
358 Eigen::VectorXd& positions) -> bool {
359 return self->findSolution(positions);
360 },
361 py::arg("positions"))
362 .def(
363 "solveAndApply",
364 +[](dart::dynamics::InverseKinematics* self) -> bool {
365 return self->solveAndApply();
366 })
367 .def(
368 "solveAndApply",
369 +[](dart::dynamics::InverseKinematics* self,
370 bool allowIncompleteResult) -> bool {
371 return self->solveAndApply(allowIncompleteResult);
372 },
373 py::arg("allowIncompleteResult"))
374 .def(
375 "solveAndApply",
376 +[](dart::dynamics::InverseKinematics* self,
377 Eigen::VectorXd& positions,
378 bool allowIncompleteResult) -> bool {
379 return self->solveAndApply(positions, allowIncompleteResult);
380 },
381 py::arg("positions"),
382 py::arg("allowIncompleteResult"))
383 .def(
384 "clone",
385 +[](const dart::dynamics::InverseKinematics* self,
386 dart::dynamics::JacobianNode* _newNode)
387 -> dart::dynamics::InverseKinematicsPtr {
388 return self->clone(_newNode);
389 },
390 ::py::arg("newNode"))
391 .def(
392 "setActive",
393 +[](dart::dynamics::InverseKinematics* self) { self->setActive(); })
394 .def(
395 "setActive",
396 +[](dart::dynamics::InverseKinematics* self, bool _active) {
397 self->setActive(_active);
398 },
399 ::py::arg("active"))
400 .def(
401 "setInactive",
402 +[](dart::dynamics::InverseKinematics* self) { self->setInactive(); })
403 .def(
404 "isActive",
405 +[](const dart::dynamics::InverseKinematics* self) -> bool {
406 return self->isActive();
407 })
408 .def(
409 "setHierarchyLevel",
410 +[](dart::dynamics::InverseKinematics* self, std::size_t _level) {
411 self->setHierarchyLevel(_level);
412 },
413 ::py::arg("level"))
414 .def(
415 "getHierarchyLevel",
416 +[](const dart::dynamics::InverseKinematics* self) -> std::size_t {
417 return self->getHierarchyLevel();
418 })
419 .def(
420 "useChain",
421 +[](dart::dynamics::InverseKinematics* self) { self->useChain(); })
422 .def(
423 "useWholeBody",
424 +[](dart::dynamics::InverseKinematics* self) {
425 self->useWholeBody();
426 })
427 .def(
428 "setDofs",
429 +[](dart::dynamics::InverseKinematics* self,
430 const std::vector<std::size_t>& _dofs) { self->setDofs(_dofs); },
431 ::py::arg("dofs"))
432 .def(
433 "setObjective",
434 +[](dart::dynamics::InverseKinematics* self,
435 const std::shared_ptr<dart::optimizer::Function>& _objective) {
436 self->setObjective(_objective);
437 },
438 ::py::arg("objective"))
439 .def(
440 "getObjective",
441 +[](const dart::dynamics::InverseKinematics* self)
442 -> std::shared_ptr<const dart::optimizer::Function> {
443 return self->getObjective();
444 })
445 .def(
446 "setNullSpaceObjective",
447 +[](dart::dynamics::InverseKinematics* self,
448 const std::shared_ptr<dart::optimizer::Function>& _nsObjective) {
449 self->setNullSpaceObjective(_nsObjective);
450 },
451 ::py::arg("nsObjective"))
452 .def(
453 "getNullSpaceObjective",
454 +[](const dart::dynamics::InverseKinematics* self)
455 -> std::shared_ptr<const dart::optimizer::Function> {
456 return self->getNullSpaceObjective();
457 })
458 .def(
459 "hasNullSpaceObjective",
460 +[](const dart::dynamics::InverseKinematics* self) -> bool {
461 return self->hasNullSpaceObjective();
462 })
463 .def(
464 "getErrorMethod",
465 +[](dart::dynamics::InverseKinematics* self)
466 -> dart::dynamics::InverseKinematics::ErrorMethod& {
467 return self->getErrorMethod();
468 },
469 ::py::return_value_policy::reference_internal)
470 .def(
471 "getProblem",
472 +[](const dart::dynamics::InverseKinematics* self)
473 -> std::shared_ptr<const dart::optimizer::Problem> {
474 return self->getProblem();
475 })
476 .def(
477 "resetProblem",
478 +[](dart::dynamics::InverseKinematics* self) {
479 self->resetProblem();
480 })
481 .def(
482 "resetProblem",
483 +[](dart::dynamics::InverseKinematics* self, bool _clearSeeds) {
484 self->resetProblem(_clearSeeds);
485 },
486 ::py::arg("clearSeeds"))
487 .def(
488 "setSolver",
489 +[](dart::dynamics::InverseKinematics* self,
490 const std::shared_ptr<dart::optimizer::Solver>& _newSolver) {
491 self->setSolver(_newSolver);
492 },
493 ::py::arg("newSolver"))
494 .def(
495 "getSolver",
496 +[](dart::dynamics::InverseKinematics* self)
497 -> std::shared_ptr<dart::optimizer::Solver> {
498 return self->getSolver();
499 })
500 .def(
501 "setOffset",
502 +[](dart::dynamics::InverseKinematics* self) { self->setOffset(); })
503 .def(
504 "setOffset",
505 +[](dart::dynamics::InverseKinematics* self,
506 const Eigen::Vector3d& _offset) { self->setOffset(_offset); },
507 ::py::arg("offset"))
508 .def(
509 "getOffset",
510 +[](const dart::dynamics::InverseKinematics* self)
511 -> const Eigen::Vector3d& { return self->getOffset(); },
512 ::py::return_value_policy::reference_internal)
513 .def(
514 "hasOffset",
515 +[](const dart::dynamics::InverseKinematics* self) -> bool {
516 return self->hasOffset();
517 })
518 .def(
519 "setTarget",
520 +[](dart::dynamics::InverseKinematics* self,
521 std::shared_ptr<dart::dynamics::SimpleFrame> _newTarget) {
522 self->setTarget(_newTarget);
523 },
524 ::py::arg("newTarget"))
525 .def(
526 "getTarget",
527 +[](dart::dynamics::InverseKinematics* self)
528 -> std::shared_ptr<dart::dynamics::SimpleFrame> {
529 return self->getTarget();
530 })
531 .def(
532 "getTarget",
533 +[](const dart::dynamics::InverseKinematics* self)
534 -> std::shared_ptr<const dart::dynamics::SimpleFrame> {
535 return self->getTarget();
536 })
537 .def(
538 "getPositions",
539 +[](const dart::dynamics::InverseKinematics* self)
540 -> Eigen::VectorXd { return self->getPositions(); })
541 .def(
542 "setPositions",
543 +[](dart::dynamics::InverseKinematics* self,
544 const Eigen::VectorXd& _q) { self->setPositions(_q); },
545 ::py::arg("q"))
546 .def("clearCaches", +[](dart::dynamics::InverseKinematics* self) {
547 self->clearCaches();
548 });
549 }
550
551 } // namespace python
552 } // namespace dart
553