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