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 "eigen_geometry_pybind.h"
36 #include "eigen_pybind.h"
37 
38 namespace py = pybind11;
39 
40 namespace dart {
41 namespace python {
42 
JacobianNode(py::module & m)43 void JacobianNode(py::module& m)
44 {
45   ::py::class_<
46       dart::dynamics::JacobianNode,
47       dart::dynamics::Frame,
48       dart::dynamics::Node,
49       std::shared_ptr<dart::dynamics::JacobianNode>>(m, "JacobianNode")
50       .def(
51           "getIK",
52           +[](dart::dynamics::JacobianNode* self)
53               -> std::shared_ptr<dart::dynamics::InverseKinematics> {
54             return self->getIK();
55           })
56       .def(
57           "getIK",
58           +[](dart::dynamics::JacobianNode* self, bool createIfNull)
59               -> std::shared_ptr<dart::dynamics::InverseKinematics> {
60             return self->getIK(createIfNull);
61           },
62           ::py::arg("createIfNull"))
63       .def(
64           "getOrCreateIK",
65           +[](dart::dynamics::JacobianNode* self)
66               -> std::shared_ptr<dart::dynamics::InverseKinematics> {
67             return self->getOrCreateIK();
68           })
69       .def(
70           "clearIK",
71           +[](dart::dynamics::JacobianNode* self) { self->clearIK(); })
72       .def(
73           "dependsOn",
74           +[](const dart::dynamics::JacobianNode* self,
75               std::size_t _genCoordIndex) -> bool {
76             return self->dependsOn(_genCoordIndex);
77           },
78           ::py::arg("genCoordIndex"))
79       .def(
80           "getNumDependentGenCoords",
81           +[](const dart::dynamics::JacobianNode* self) -> std::size_t {
82             return self->getNumDependentGenCoords();
83           })
84       .def(
85           "getDependentGenCoordIndex",
86           +[](const dart::dynamics::JacobianNode* self,
87               std::size_t _arrayIndex) -> std::size_t {
88             return self->getDependentGenCoordIndex(_arrayIndex);
89           },
90           ::py::arg("arrayIndex"))
91       .def(
92           "getNumDependentDofs",
93           +[](const dart::dynamics::JacobianNode* self) -> std::size_t {
94             return self->getNumDependentDofs();
95           })
96       .def(
97           "getChainDofs",
98           +[](const dart::dynamics::JacobianNode* self)
99               -> const std::vector<const dart::dynamics::DegreeOfFreedom*> {
100             return self->getChainDofs();
101           })
102       .def(
103           "getJacobian",
104           +[](const dart::dynamics::JacobianNode* self,
105               const dart::dynamics::Frame* _inCoordinatesOf)
106               -> dart::math::Jacobian {
107             return self->getJacobian(_inCoordinatesOf);
108           },
109           ::py::arg("inCoordinatesOf"))
110       .def(
111           "getJacobian",
112           +[](const dart::dynamics::JacobianNode* self,
113               const Eigen::Vector3d& _offset) -> dart::math::Jacobian {
114             return self->getJacobian(_offset);
115           },
116           ::py::arg("offset"))
117       .def(
118           "getJacobian",
119           +[](const dart::dynamics::JacobianNode* self,
120               const Eigen::Vector3d& _offset,
121               const dart::dynamics::Frame* _inCoordinatesOf)
122               -> dart::math::Jacobian {
123             return self->getJacobian(_offset, _inCoordinatesOf);
124           },
125           ::py::arg("offset"),
126           ::py::arg("inCoordinatesOf"))
127       .def(
128           "getWorldJacobian",
129           +[](const dart::dynamics::JacobianNode* self,
130               const Eigen::Vector3d& _offset) -> dart::math::Jacobian {
131             return self->getWorldJacobian(_offset);
132           },
133           ::py::arg("offset"))
134       .def(
135           "getLinearJacobian",
136           +[](const dart::dynamics::JacobianNode* self)
137               -> dart::math::LinearJacobian {
138             return self->getLinearJacobian();
139           })
140       .def(
141           "getLinearJacobian",
142           +[](const dart::dynamics::JacobianNode* self,
143               const dart::dynamics::Frame* _inCoordinatesOf)
144               -> dart::math::LinearJacobian {
145             return self->getLinearJacobian(_inCoordinatesOf);
146           },
147           ::py::arg("inCoordinatesOf"))
148       .def(
149           "getLinearJacobian",
150           +[](const dart::dynamics::JacobianNode* self,
151               const Eigen::Vector3d& _offset) -> dart::math::LinearJacobian {
152             return self->getLinearJacobian(_offset);
153           },
154           ::py::arg("offset"))
155       .def(
156           "getLinearJacobian",
157           +[](const dart::dynamics::JacobianNode* self,
158               const Eigen::Vector3d& _offset,
159               const dart::dynamics::Frame* _inCoordinatesOf)
160               -> dart::math::LinearJacobian {
161             return self->getLinearJacobian(_offset, _inCoordinatesOf);
162           },
163           ::py::arg("offset"),
164           ::py::arg("inCoordinatesOf"))
165       .def(
166           "getAngularJacobian",
167           +[](const dart::dynamics::JacobianNode* self)
168               -> dart::math::AngularJacobian {
169             return self->getAngularJacobian();
170           })
171       .def(
172           "getAngularJacobian",
173           +[](const dart::dynamics::JacobianNode* self,
174               const dart::dynamics::Frame* _inCoordinatesOf)
175               -> dart::math::AngularJacobian {
176             return self->getAngularJacobian(_inCoordinatesOf);
177           },
178           ::py::arg("inCoordinatesOf"))
179       .def(
180           "getJacobianSpatialDeriv",
181           +[](const dart::dynamics::JacobianNode* self,
182               const dart::dynamics::Frame* _inCoordinatesOf)
183               -> dart::math::Jacobian {
184             return self->getJacobianSpatialDeriv(_inCoordinatesOf);
185           },
186           ::py::arg("inCoordinatesOf"))
187       .def(
188           "getJacobianSpatialDeriv",
189           +[](const dart::dynamics::JacobianNode* self,
190               const Eigen::Vector3d& _offset) -> dart::math::Jacobian {
191             return self->getJacobianSpatialDeriv(_offset);
192           },
193           ::py::arg("offset"))
194       .def(
195           "getJacobianSpatialDeriv",
196           +[](const dart::dynamics::JacobianNode* self,
197               const Eigen::Vector3d& _offset,
198               const dart::dynamics::Frame* _inCoordinatesOf)
199               -> dart::math::Jacobian {
200             return self->getJacobianSpatialDeriv(_offset, _inCoordinatesOf);
201           },
202           ::py::arg("offset"),
203           ::py::arg("inCoordinatesOf"))
204       .def(
205           "getJacobianClassicDeriv",
206           +[](const dart::dynamics::JacobianNode* self,
207               const dart::dynamics::Frame* _inCoordinatesOf)
208               -> dart::math::Jacobian {
209             return self->getJacobianClassicDeriv(_inCoordinatesOf);
210           },
211           ::py::arg("inCoordinatesOf"))
212       .def(
213           "getJacobianClassicDeriv",
214           +[](const dart::dynamics::JacobianNode* self,
215               const Eigen::Vector3d& _offset) -> dart::math::Jacobian {
216             return self->getJacobianClassicDeriv(_offset);
217           },
218           ::py::arg("offset"))
219       .def(
220           "getJacobianClassicDeriv",
221           +[](const dart::dynamics::JacobianNode* self,
222               const Eigen::Vector3d& _offset,
223               const dart::dynamics::Frame* _inCoordinatesOf)
224               -> dart::math::Jacobian {
225             return self->getJacobianClassicDeriv(_offset, _inCoordinatesOf);
226           },
227           ::py::arg("offset"),
228           ::py::arg("inCoordinatesOf"))
229       .def(
230           "getLinearJacobianDeriv",
231           +[](const dart::dynamics::JacobianNode* self)
232               -> dart::math::LinearJacobian {
233             return self->getLinearJacobianDeriv();
234           })
235       .def(
236           "getLinearJacobianDeriv",
237           +[](const dart::dynamics::JacobianNode* self,
238               const dart::dynamics::Frame* _inCoordinatesOf)
239               -> dart::math::LinearJacobian {
240             return self->getLinearJacobianDeriv(_inCoordinatesOf);
241           },
242           ::py::arg("inCoordinatesOf"))
243       .def(
244           "getLinearJacobianDeriv",
245           +[](const dart::dynamics::JacobianNode* self,
246               const Eigen::Vector3d& _offset) -> dart::math::LinearJacobian {
247             return self->getLinearJacobianDeriv(_offset);
248           },
249           ::py::arg("offset"))
250       .def(
251           "getLinearJacobianDeriv",
252           +[](const dart::dynamics::JacobianNode* self,
253               const Eigen::Vector3d& _offset,
254               const dart::dynamics::Frame* _inCoordinatesOf)
255               -> dart::math::LinearJacobian {
256             return self->getLinearJacobianDeriv(_offset, _inCoordinatesOf);
257           },
258           ::py::arg("offset"),
259           ::py::arg("inCoordinatesOf"))
260       .def(
261           "getAngularJacobianDeriv",
262           +[](const dart::dynamics::JacobianNode* self)
263               -> dart::math::AngularJacobian {
264             return self->getAngularJacobianDeriv();
265           })
266       .def(
267           "getAngularJacobianDeriv",
268           +[](const dart::dynamics::JacobianNode* self,
269               const dart::dynamics::Frame* _inCoordinatesOf)
270               -> dart::math::AngularJacobian {
271             return self->getAngularJacobianDeriv(_inCoordinatesOf);
272           },
273           ::py::arg("inCoordinatesOf"))
274       .def(
275           "dirtyJacobian",
276           +[](dart::dynamics::JacobianNode* self) { self->dirtyJacobian(); })
277       .def("dirtyJacobianDeriv", +[](dart::dynamics::JacobianNode* self) {
278         self->dirtyJacobianDeriv();
279       });
280 }
281 
282 } // namespace python
283 } // namespace dart
284