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
FreeJoint(py::module & m)43 void FreeJoint(py::module& m)
44 {
45 ::py::class_<
46 dart::dynamics::FreeJoint::Properties,
47 dart::dynamics::GenericJoint<math::SE3Space>::Properties>(
48 m, "FreeJointProperties")
49 .def(::py::init<>())
50 .def(
51 ::py::init<const dart::dynamics::GenericJoint<
52 dart::math::SE3Space>::Properties&>(),
53 ::py::arg("properties"));
54
55 ::py::class_<
56 dart::dynamics::FreeJoint,
57 dart::dynamics::GenericJoint<dart::math::SE3Space>,
58 std::shared_ptr<dart::dynamics::FreeJoint>>(m, "FreeJoint")
59 .def(
60 "getFreeJointProperties",
61 +[](const dart::dynamics::FreeJoint* self)
62 -> dart::dynamics::FreeJoint::Properties {
63 return self->getFreeJointProperties();
64 })
65 .def(
66 "getType",
67 +[](const dart::dynamics::FreeJoint* self) -> const std::string& {
68 return self->getType();
69 },
70 ::py::return_value_policy::reference_internal)
71 .def(
72 "isCyclic",
73 +[](const dart::dynamics::FreeJoint* self,
74 std::size_t _index) -> bool { return self->isCyclic(_index); },
75 ::py::arg("index"))
76 .def(
77 "setSpatialMotion",
78 +[](dart::dynamics::FreeJoint* self,
79 const Eigen::Isometry3d* newTransform,
80 const dart::dynamics::Frame* withRespectTo,
81 const Eigen::Vector6d* newSpatialVelocity,
82 const dart::dynamics::Frame* velRelativeTo,
83 const dart::dynamics::Frame* velInCoordinatesOf,
84 const Eigen::Vector6d* newSpatialAcceleration,
85 const dart::dynamics::Frame* accRelativeTo,
86 const dart::dynamics::Frame* accInCoordinatesOf) {
87 self->setSpatialMotion(
88 newTransform,
89 withRespectTo,
90 newSpatialVelocity,
91 velRelativeTo,
92 velInCoordinatesOf,
93 newSpatialAcceleration,
94 accRelativeTo,
95 accInCoordinatesOf);
96 },
97 ::py::arg("newTransform"),
98 ::py::arg("withRespectTo"),
99 ::py::arg("newSpatialVelocity"),
100 ::py::arg("velRelativeTo"),
101 ::py::arg("velInCoordinatesOf"),
102 ::py::arg("newSpatialAcceleration"),
103 ::py::arg("accRelativeTo"),
104 ::py::arg("accInCoordinatesOf"))
105 .def(
106 "setRelativeTransform",
107 +[](dart::dynamics::FreeJoint* self,
108 const Eigen::Isometry3d& newTransform) {
109 self->setRelativeTransform(newTransform);
110 },
111 ::py::arg("newTransform"))
112 .def(
113 "setTransform",
114 +[](dart::dynamics::FreeJoint* self,
115 const Eigen::Isometry3d& newTransform) {
116 self->setTransform(newTransform);
117 },
118 ::py::arg("newTransform"))
119 .def(
120 "setTransform",
121 +[](dart::dynamics::FreeJoint* self,
122 const Eigen::Isometry3d& newTransform,
123 const dart::dynamics::Frame* withRespectTo) {
124 self->setTransform(newTransform, withRespectTo);
125 },
126 ::py::arg("newTransform"),
127 ::py::arg("withRespectTo"))
128 .def(
129 "setRelativeSpatialVelocity",
130 +[](dart::dynamics::FreeJoint* self,
131 const Eigen::Vector6d& newSpatialVelocity) {
132 self->setRelativeSpatialVelocity(newSpatialVelocity);
133 },
134 ::py::arg("newSpatialVelocity"))
135 .def(
136 "setRelativeSpatialVelocity",
137 +[](dart::dynamics::FreeJoint* self,
138 const Eigen::Vector6d& newSpatialVelocity,
139 const dart::dynamics::Frame* inCoordinatesOf) {
140 self->setRelativeSpatialVelocity(
141 newSpatialVelocity, inCoordinatesOf);
142 },
143 ::py::arg("newSpatialVelocity"),
144 ::py::arg("inCoordinatesOf"))
145 .def(
146 "setSpatialVelocity",
147 +[](dart::dynamics::FreeJoint* self,
148 const Eigen::Vector6d& newSpatialVelocity,
149 const dart::dynamics::Frame* relativeTo,
150 const dart::dynamics::Frame* inCoordinatesOf) {
151 self->setSpatialVelocity(
152 newSpatialVelocity, relativeTo, inCoordinatesOf);
153 },
154 ::py::arg("newSpatialVelocity"),
155 ::py::arg("relativeTo"),
156 ::py::arg("inCoordinatesOf"))
157 .def(
158 "setLinearVelocity",
159 +[](dart::dynamics::FreeJoint* self,
160 const Eigen::Vector3d& newLinearVelocity) {
161 self->setLinearVelocity(newLinearVelocity);
162 },
163 ::py::arg("newLinearVelocity"))
164 .def(
165 "setLinearVelocity",
166 +[](dart::dynamics::FreeJoint* self,
167 const Eigen::Vector3d& newLinearVelocity,
168 const dart::dynamics::Frame* relativeTo) {
169 self->setLinearVelocity(newLinearVelocity, relativeTo);
170 },
171 ::py::arg("newLinearVelocity"),
172 ::py::arg("relativeTo"))
173 .def(
174 "setLinearVelocity",
175 +[](dart::dynamics::FreeJoint* self,
176 const Eigen::Vector3d& newLinearVelocity,
177 const dart::dynamics::Frame* relativeTo,
178 const dart::dynamics::Frame* inCoordinatesOf) {
179 self->setLinearVelocity(
180 newLinearVelocity, relativeTo, inCoordinatesOf);
181 },
182 ::py::arg("newLinearVelocity"),
183 ::py::arg("relativeTo"),
184 ::py::arg("inCoordinatesOf"))
185 .def(
186 "setAngularVelocity",
187 +[](dart::dynamics::FreeJoint* self,
188 const Eigen::Vector3d& newAngularVelocity) {
189 self->setAngularVelocity(newAngularVelocity);
190 },
191 ::py::arg("newAngularVelocity"))
192 .def(
193 "setAngularVelocity",
194 +[](dart::dynamics::FreeJoint* self,
195 const Eigen::Vector3d& newAngularVelocity,
196 const dart::dynamics::Frame* relativeTo) {
197 self->setAngularVelocity(newAngularVelocity, relativeTo);
198 },
199 ::py::arg("newAngularVelocity"),
200 ::py::arg("relativeTo"))
201 .def(
202 "setAngularVelocity",
203 +[](dart::dynamics::FreeJoint* self,
204 const Eigen::Vector3d& newAngularVelocity,
205 const dart::dynamics::Frame* relativeTo,
206 const dart::dynamics::Frame* inCoordinatesOf) {
207 self->setAngularVelocity(
208 newAngularVelocity, relativeTo, inCoordinatesOf);
209 },
210 ::py::arg("newAngularVelocity"),
211 ::py::arg("relativeTo"),
212 ::py::arg("inCoordinatesOf"))
213 .def(
214 "setRelativeSpatialAcceleration",
215 +[](dart::dynamics::FreeJoint* self,
216 const Eigen::Vector6d& newSpatialAcceleration) {
217 self->setRelativeSpatialAcceleration(newSpatialAcceleration);
218 },
219 ::py::arg("newSpatialAcceleration"))
220 .def(
221 "setRelativeSpatialAcceleration",
222 +[](dart::dynamics::FreeJoint* self,
223 const Eigen::Vector6d& newSpatialAcceleration,
224 const dart::dynamics::Frame* inCoordinatesOf) {
225 self->setRelativeSpatialAcceleration(
226 newSpatialAcceleration, inCoordinatesOf);
227 },
228 ::py::arg("newSpatialAcceleration"),
229 ::py::arg("inCoordinatesOf"))
230 .def(
231 "setSpatialAcceleration",
232 +[](dart::dynamics::FreeJoint* self,
233 const Eigen::Vector6d& newSpatialAcceleration,
234 const dart::dynamics::Frame* relativeTo,
235 const dart::dynamics::Frame* inCoordinatesOf) {
236 self->setSpatialAcceleration(
237 newSpatialAcceleration, relativeTo, inCoordinatesOf);
238 },
239 ::py::arg("newSpatialAcceleration"),
240 ::py::arg("relativeTo"),
241 ::py::arg("inCoordinatesOf"))
242 .def(
243 "setLinearAcceleration",
244 +[](dart::dynamics::FreeJoint* self,
245 const Eigen::Vector3d& newLinearAcceleration) {
246 self->setLinearAcceleration(newLinearAcceleration);
247 },
248 ::py::arg("newLinearAcceleration"))
249 .def(
250 "setLinearAcceleration",
251 +[](dart::dynamics::FreeJoint* self,
252 const Eigen::Vector3d& newLinearAcceleration,
253 const dart::dynamics::Frame* relativeTo) {
254 self->setLinearAcceleration(newLinearAcceleration, relativeTo);
255 },
256 ::py::arg("newLinearAcceleration"),
257 ::py::arg("relativeTo"))
258 .def(
259 "setLinearAcceleration",
260 +[](dart::dynamics::FreeJoint* self,
261 const Eigen::Vector3d& newLinearAcceleration,
262 const dart::dynamics::Frame* relativeTo,
263 const dart::dynamics::Frame* inCoordinatesOf) {
264 self->setLinearAcceleration(
265 newLinearAcceleration, relativeTo, inCoordinatesOf);
266 },
267 ::py::arg("newLinearAcceleration"),
268 ::py::arg("relativeTo"),
269 ::py::arg("inCoordinatesOf"))
270 .def(
271 "setAngularAcceleration",
272 +[](dart::dynamics::FreeJoint* self,
273 const Eigen::Vector3d& newAngularAcceleration) {
274 self->setAngularAcceleration(newAngularAcceleration);
275 },
276 ::py::arg("newAngularAcceleration"))
277 .def(
278 "setAngularAcceleration",
279 +[](dart::dynamics::FreeJoint* self,
280 const Eigen::Vector3d& newAngularAcceleration,
281 const dart::dynamics::Frame* relativeTo) {
282 self->setAngularAcceleration(newAngularAcceleration, relativeTo);
283 },
284 ::py::arg("newAngularAcceleration"),
285 ::py::arg("relativeTo"))
286 .def(
287 "setAngularAcceleration",
288 +[](dart::dynamics::FreeJoint* self,
289 const Eigen::Vector3d& newAngularAcceleration,
290 const dart::dynamics::Frame* relativeTo,
291 const dart::dynamics::Frame* inCoordinatesOf) {
292 self->setAngularAcceleration(
293 newAngularAcceleration, relativeTo, inCoordinatesOf);
294 },
295 ::py::arg("newAngularAcceleration"),
296 ::py::arg("relativeTo"),
297 ::py::arg("inCoordinatesOf"))
298 .def(
299 "getRelativeJacobianStatic",
300 +[](const dart::dynamics::FreeJoint* self,
301 const Eigen::Vector6d& _positions) -> Eigen::Matrix6d {
302 return self->getRelativeJacobianStatic(_positions);
303 },
304 ::py::arg("positions"))
305 .def(
306 "getPositionDifferencesStatic",
307 +[](const dart::dynamics::FreeJoint* self,
308 const Eigen::Vector6d& _q2,
309 const Eigen::Vector6d& _q1) -> Eigen::Vector6d {
310 return self->getPositionDifferencesStatic(_q2, _q1);
311 },
312 ::py::arg("q2"),
313 ::py::arg("q1"))
314 .def_static(
315 "getStaticType",
316 +[]() -> const std::string& {
317 return dart::dynamics::FreeJoint::getStaticType();
318 },
319 ::py::return_value_policy::reference_internal)
320 .def_static(
321 "convertToPositions",
322 +[](const Eigen::Isometry3d& _tf) -> Eigen::Vector6d {
323 return dart::dynamics::FreeJoint::convertToPositions(_tf);
324 },
325 ::py::arg("tf"))
326 .def_static(
327 "convertToTransform",
328 +[](const Eigen::Vector6d& _positions) -> Eigen::Isometry3d {
329 return dart::dynamics::FreeJoint::convertToTransform(_positions);
330 },
331 ::py::arg("positions"))
332 .def_static(
333 "setTransformOf",
334 +[](dart::dynamics::Joint* joint, const Eigen::Isometry3d& tf) {
335 dart::dynamics::FreeJoint::setTransformOf(joint, tf);
336 },
337 ::py::arg("joint"),
338 ::py::arg("tf"))
339 .def_static(
340 "setTransformOf",
341 +[](dart::dynamics::Joint* joint,
342 const Eigen::Isometry3d& tf,
343 const dart::dynamics::Frame* withRespectTo) {
344 dart::dynamics::FreeJoint::setTransformOf(joint, tf, withRespectTo);
345 },
346 ::py::arg("joint"),
347 ::py::arg("tf"),
348 ::py::arg("withRespectTo"))
349 .def_static(
350 "setTransformOf",
351 +[](dart::dynamics::BodyNode* bodyNode, const Eigen::Isometry3d& tf) {
352 dart::dynamics::FreeJoint::setTransformOf(bodyNode, tf);
353 },
354 ::py::arg("bodyNode"),
355 ::py::arg("tf"))
356 .def_static(
357 "setTransformOf",
358 +[](dart::dynamics::BodyNode* bodyNode,
359 const Eigen::Isometry3d& tf,
360 const dart::dynamics::Frame* withRespectTo) {
361 dart::dynamics::FreeJoint::setTransformOf(
362 bodyNode, tf, withRespectTo);
363 },
364 ::py::arg("bodyNode"),
365 ::py::arg("tf"),
366 ::py::arg("withRespectTo"))
367 .def_static(
368 "setTransformOf",
369 +[](dart::dynamics::Skeleton* skeleton, const Eigen::Isometry3d& tf) {
370 dart::dynamics::FreeJoint::setTransformOf(skeleton, tf);
371 },
372 ::py::arg("skeleton"),
373 ::py::arg("tf"))
374 .def_static(
375 "setTransformOf",
376 +[](dart::dynamics::Skeleton* skeleton,
377 const Eigen::Isometry3d& tf,
378 const dart::dynamics::Frame* withRespectTo) {
379 dart::dynamics::FreeJoint::setTransformOf(
380 skeleton, tf, withRespectTo);
381 },
382 ::py::arg("skeleton"),
383 ::py::arg("tf"),
384 ::py::arg("withRespectTo"))
385 .def_static(
386 "setTransformOf",
387 +[](dart::dynamics::Skeleton* skeleton,
388 const Eigen::Isometry3d& tf,
389 const dart::dynamics::Frame* withRespectTo,
390 bool applyToAllRootBodies) {
391 dart::dynamics::FreeJoint::setTransformOf(
392 skeleton, tf, withRespectTo, applyToAllRootBodies);
393 },
394 ::py::arg("skeleton"),
395 ::py::arg("tf"),
396 ::py::arg("withRespectTo"),
397 ::py::arg("applyToAllRootBodies"));
398 }
399
400 } // namespace python
401 } // namespace dart
402