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 #include "eigen_geometry_pybind.h"
37 #include "eigen_pybind.h"
38 #include "pointers.hpp"
39
40 namespace py = pybind11;
41
42 #define DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(joint_type) \
43 .def( \
44 "create" #joint_type "AndBodyNodePair", \
45 +[](dart::dynamics::BodyNode* self) \
46 -> std:: \
47 pair<dart::dynamics::joint_type*, dart::dynamics::BodyNode*> { \
48 return self->createChildJointAndBodyNodePair< \
49 dart::dynamics::joint_type, \
50 dart::dynamics::BodyNode>(); \
51 }, \
52 ::py::return_value_policy::reference_internal) \
53 .def( \
54 "create" #joint_type "AndBodyNodePair", \
55 +[](dart::dynamics::BodyNode* self, \
56 const dart::dynamics::joint_type::Properties& jointProperties) \
57 -> std::pair< \
58 dart::dynamics::joint_type*, \
59 dart::dynamics::BodyNode*> { \
60 return self->createChildJointAndBodyNodePair< \
61 dart::dynamics::joint_type, \
62 dart::dynamics::BodyNode>(jointProperties); \
63 }, \
64 ::py::return_value_policy::reference_internal, \
65 ::py::arg("jointProperties")) \
66 .def( \
67 "create" #joint_type "AndBodyNodePair", \
68 +[](dart::dynamics::BodyNode* self, \
69 const dart::dynamics::joint_type::Properties& jointProperties, \
70 const dart::dynamics::BodyNode::Properties& bodyProperties) \
71 -> std::pair< \
72 dart::dynamics::joint_type*, \
73 dart::dynamics::BodyNode*> { \
74 return self->createChildJointAndBodyNodePair< \
75 dart::dynamics::joint_type, \
76 dart::dynamics::BodyNode>(jointProperties, bodyProperties); \
77 }, \
78 ::py::return_value_policy::reference_internal, \
79 ::py::arg("jointProperties"), \
80 ::py::arg("bodyProperties"))
81
82 namespace dart {
83 namespace python {
84
BodyNode(py::module & m)85 void BodyNode(py::module& m)
86 {
87 ::py::class_<dart::dynamics::detail::BodyNodeAspectProperties>(
88 m, "BodyNodeAspectProperties")
89 .def(::py::init<>())
90 .def(::py::init<const std::string&>(), ::py::arg("name"))
91 .def(
92 ::py::init<const std::string&, const dart::dynamics::Inertia&>(),
93 ::py::arg("name"),
94 ::py::arg("inertia"))
95 .def(
96 ::py::
97 init<const std::string&, const dart::dynamics::Inertia&, bool>(),
98 ::py::arg("name"),
99 ::py::arg("inertia"),
100 ::py::arg("isCollidable"))
101 .def(
102 ::py::init<
103 const std::string&,
104 const dart::dynamics::Inertia&,
105 bool,
106 bool>(),
107 ::py::arg("name"),
108 ::py::arg("inertia"),
109 ::py::arg("isCollidable"),
110 ::py::arg("gravityMode"))
111 .def_readwrite(
112 "mName", &dart::dynamics::detail::BodyNodeAspectProperties::mName)
113 .def_readwrite(
114 "mInertia",
115 &dart::dynamics::detail::BodyNodeAspectProperties::mInertia)
116 .def_readwrite(
117 "mIsCollidable",
118 &dart::dynamics::detail::BodyNodeAspectProperties::mIsCollidable)
119 .def_readwrite(
120 "mGravityMode",
121 &dart::dynamics::detail::BodyNodeAspectProperties::mGravityMode);
122
123 ::py::class_<dart::dynamics::BodyNode::Properties>(m, "BodyNodeProperties")
124 .def(::py::init<>())
125 .def(
126 ::py::init<const dart::dynamics::detail::BodyNodeAspectProperties&>(),
127 ::py::arg("aspectProperties"));
128
129 ::py::class_<
130 dart::dynamics::TemplatedJacobianNode<dart::dynamics::BodyNode>,
131 dart::dynamics::JacobianNode,
132 std::shared_ptr<
133 dart::dynamics::TemplatedJacobianNode<dart::dynamics::BodyNode>>>(
134 m, "TemplatedJacobianBodyNode")
135 .def(
136 "getJacobian",
137 +[](const dart::dynamics::TemplatedJacobianNode<
138 dart::dynamics::BodyNode>* self,
139 const dart::dynamics::Frame* _inCoordinatesOf)
140 -> dart::math::Jacobian {
141 return self->getJacobian(_inCoordinatesOf);
142 },
143 ::py::arg("inCoordinatesOf"))
144 .def(
145 "getJacobian",
146 +[](const dart::dynamics::TemplatedJacobianNode<
147 dart::dynamics::BodyNode>* self,
148 const Eigen::Vector3d& _offset) -> dart::math::Jacobian {
149 return self->getJacobian(_offset);
150 },
151 ::py::arg("offset"))
152 .def(
153 "getJacobian",
154 +[](const dart::dynamics::TemplatedJacobianNode<
155 dart::dynamics::BodyNode>* self,
156 const Eigen::Vector3d& _offset,
157 const dart::dynamics::Frame* _inCoordinatesOf)
158 -> dart::math::Jacobian {
159 return self->getJacobian(_offset, _inCoordinatesOf);
160 },
161 ::py::arg("offset"),
162 ::py::arg("inCoordinatesOf"))
163 .def(
164 "getWorldJacobian",
165 +[](const dart::dynamics::TemplatedJacobianNode<
166 dart::dynamics::BodyNode>* self,
167 const Eigen::Vector3d& _offset) -> dart::math::Jacobian {
168 return self->getWorldJacobian(_offset);
169 },
170 ::py::arg("offset"))
171 .def(
172 "getLinearJacobian",
173 +[](const dart::dynamics::TemplatedJacobianNode<
174 dart::dynamics::BodyNode>* self) -> dart::math::LinearJacobian {
175 return self->getLinearJacobian();
176 })
177 .def(
178 "getLinearJacobian",
179 +[](const dart::dynamics::TemplatedJacobianNode<
180 dart::dynamics::BodyNode>* self,
181 const dart::dynamics::Frame* _inCoordinatesOf)
182 -> dart::math::LinearJacobian {
183 return self->getLinearJacobian(_inCoordinatesOf);
184 },
185 ::py::arg("inCoordinatesOf"))
186 .def(
187 "getLinearJacobian",
188 +[](const dart::dynamics::TemplatedJacobianNode<
189 dart::dynamics::BodyNode>* self,
190 const Eigen::Vector3d& _offset) -> dart::math::LinearJacobian {
191 return self->getLinearJacobian(_offset);
192 },
193 ::py::arg("offset"))
194 .def(
195 "getLinearJacobian",
196 +[](const dart::dynamics::TemplatedJacobianNode<
197 dart::dynamics::BodyNode>* self,
198 const Eigen::Vector3d& _offset,
199 const dart::dynamics::Frame* _inCoordinatesOf)
200 -> dart::math::LinearJacobian {
201 return self->getLinearJacobian(_offset, _inCoordinatesOf);
202 },
203 ::py::arg("offset"),
204 ::py::arg("inCoordinatesOf"))
205 .def(
206 "getAngularJacobian",
207 +[](const dart::dynamics::TemplatedJacobianNode<
208 dart::dynamics::BodyNode>* self) -> dart::math::AngularJacobian {
209 return self->getAngularJacobian();
210 })
211 .def(
212 "getAngularJacobian",
213 +[](const dart::dynamics::TemplatedJacobianNode<
214 dart::dynamics::BodyNode>* self,
215 const dart::dynamics::Frame* _inCoordinatesOf)
216 -> dart::math::AngularJacobian {
217 return self->getAngularJacobian(_inCoordinatesOf);
218 },
219 ::py::arg("inCoordinatesOf"))
220 .def(
221 "getJacobianSpatialDeriv",
222 +[](const dart::dynamics::TemplatedJacobianNode<
223 dart::dynamics::BodyNode>* self,
224 const dart::dynamics::Frame* _inCoordinatesOf)
225 -> dart::math::Jacobian {
226 return self->getJacobianSpatialDeriv(_inCoordinatesOf);
227 },
228 ::py::arg("inCoordinatesOf"))
229 .def(
230 "getJacobianSpatialDeriv",
231 +[](const dart::dynamics::TemplatedJacobianNode<
232 dart::dynamics::BodyNode>* self,
233 const Eigen::Vector3d& _offset) -> dart::math::Jacobian {
234 return self->getJacobianSpatialDeriv(_offset);
235 },
236 ::py::arg("offset"))
237 .def(
238 "getJacobianSpatialDeriv",
239 +[](const dart::dynamics::TemplatedJacobianNode<
240 dart::dynamics::BodyNode>* self,
241 const Eigen::Vector3d& _offset,
242 const dart::dynamics::Frame* _inCoordinatesOf)
243 -> dart::math::Jacobian {
244 return self->getJacobianSpatialDeriv(_offset, _inCoordinatesOf);
245 },
246 ::py::arg("offset"),
247 ::py::arg("inCoordinatesOf"))
248 .def(
249 "getJacobianClassicDeriv",
250 +[](const dart::dynamics::TemplatedJacobianNode<
251 dart::dynamics::BodyNode>* self,
252 const dart::dynamics::Frame* _inCoordinatesOf)
253 -> dart::math::Jacobian {
254 return self->getJacobianClassicDeriv(_inCoordinatesOf);
255 },
256 ::py::arg("inCoordinatesOf"))
257 .def(
258 "getJacobianClassicDeriv",
259 +[](const dart::dynamics::TemplatedJacobianNode<
260 dart::dynamics::BodyNode>* self,
261 const Eigen::Vector3d& _offset) -> dart::math::Jacobian {
262 return self->getJacobianClassicDeriv(_offset);
263 },
264 ::py::arg("offset"))
265 .def(
266 "getJacobianClassicDeriv",
267 +[](const dart::dynamics::TemplatedJacobianNode<
268 dart::dynamics::BodyNode>* self,
269 const Eigen::Vector3d& _offset,
270 const dart::dynamics::Frame* _inCoordinatesOf)
271 -> dart::math::Jacobian {
272 return self->getJacobianClassicDeriv(_offset, _inCoordinatesOf);
273 },
274 ::py::arg("offset"),
275 ::py::arg("inCoordinatesOf"))
276 .def(
277 "getLinearJacobianDeriv",
278 +[](const dart::dynamics::TemplatedJacobianNode<
279 dart::dynamics::BodyNode>* self) -> dart::math::LinearJacobian {
280 return self->getLinearJacobianDeriv();
281 })
282 .def(
283 "getLinearJacobianDeriv",
284 +[](const dart::dynamics::TemplatedJacobianNode<
285 dart::dynamics::BodyNode>* self,
286 const dart::dynamics::Frame* _inCoordinatesOf)
287 -> dart::math::LinearJacobian {
288 return self->getLinearJacobianDeriv(_inCoordinatesOf);
289 },
290 ::py::arg("inCoordinatesOf"))
291 .def(
292 "getLinearJacobianDeriv",
293 +[](const dart::dynamics::TemplatedJacobianNode<
294 dart::dynamics::BodyNode>* self,
295 const Eigen::Vector3d& _offset) -> dart::math::LinearJacobian {
296 return self->getLinearJacobianDeriv(_offset);
297 },
298 ::py::arg("offset"))
299 .def(
300 "getLinearJacobianDeriv",
301 +[](const dart::dynamics::TemplatedJacobianNode<
302 dart::dynamics::BodyNode>* self,
303 const Eigen::Vector3d& _offset,
304 const dart::dynamics::Frame* _inCoordinatesOf)
305 -> dart::math::LinearJacobian {
306 return self->getLinearJacobianDeriv(_offset, _inCoordinatesOf);
307 },
308 ::py::arg("offset"),
309 ::py::arg("inCoordinatesOf"))
310 .def(
311 "getAngularJacobianDeriv",
312 +[](const dart::dynamics::TemplatedJacobianNode<
313 dart::dynamics::BodyNode>* self) -> dart::math::AngularJacobian {
314 return self->getAngularJacobianDeriv();
315 })
316 .def(
317 "getAngularJacobianDeriv",
318 +[](const dart::dynamics::TemplatedJacobianNode<
319 dart::dynamics::BodyNode>* self,
320 const dart::dynamics::Frame* _inCoordinatesOf)
321 -> dart::math::AngularJacobian {
322 return self->getAngularJacobianDeriv(_inCoordinatesOf);
323 },
324 ::py::arg("inCoordinatesOf"));
325
326 ::py::class_<
327 dart::dynamics::BodyNode,
328 dart::dynamics::TemplatedJacobianNode<dart::dynamics::BodyNode>,
329 dart::dynamics::Frame,
330 dart::dynamics::BodyNodePtr>(m, "BodyNode")
331 .def(
332 "setAllNodeStates",
333 +[](dart::dynamics::BodyNode* self,
334 const dart::dynamics::BodyNode::AllNodeStates& states) {
335 self->setAllNodeStates(states);
336 },
337 ::py::arg("states"))
338 .def(
339 "getAllNodeStates",
340 +[](const dart::dynamics::BodyNode* self)
341 -> dart::dynamics::BodyNode::AllNodeStates {
342 return self->getAllNodeStates();
343 })
344 .def(
345 "setAllNodeProperties",
346 +[](dart::dynamics::BodyNode* self,
347 const dart::dynamics::BodyNode::AllNodeProperties& properties) {
348 self->setAllNodeProperties(properties);
349 },
350 ::py::arg("properties"))
351 .def(
352 "getAllNodeProperties",
353 +[](const dart::dynamics::BodyNode* self)
354 -> dart::dynamics::BodyNode::AllNodeProperties {
355 return self->getAllNodeProperties();
356 })
357 .def(
358 "setProperties",
359 +[](dart::dynamics::BodyNode* self,
360 const dart::dynamics::BodyNode::CompositeProperties&
361 _properties) { self->setProperties(_properties); },
362 ::py::arg("properties"))
363 .def(
364 "setProperties",
365 +[](dart::dynamics::BodyNode* self,
366 const dart::dynamics::BodyNode::AspectProperties& _properties) {
367 self->setProperties(_properties);
368 },
369 ::py::arg("properties"))
370 .def(
371 "setAspectState",
372 +[](dart::dynamics::BodyNode* self,
373 const dart::common::EmbedStateAndPropertiesOnTopOf<
374 dart::dynamics::BodyNode,
375 dart::dynamics::detail::BodyNodeState,
376 dart::dynamics::detail::BodyNodeAspectProperties,
377 dart::common::RequiresAspect<
378 dart::common::ProxyStateAndPropertiesAspect<
379 dart::dynamics::BodyNode,
380 dart::common::ProxyCloneable<
381 dart::common::Aspect::State,
382 dart::dynamics::BodyNode,
383 dart::common::CloneableMap<std::map<
384 std::type_index,
385 std::unique_ptr<
386 dart::common::CloneableVector<
387 std::unique_ptr<
388 dart::dynamics::Node::State,
389 std::default_delete<
390 dart::dynamics::Node::
391 State>>>,
392 std::default_delete<
393 dart::common::CloneableVector<
394 std::unique_ptr<
395 dart::dynamics::Node::State,
396 std::default_delete<
397 dart::dynamics::Node::
398 State>>>>>,
399 std::less<std::type_index>,
400 std::allocator<std::pair<
401 const std::type_index,
402 std::unique_ptr<
403 dart::common::CloneableVector<
404 std::unique_ptr<
405 dart::dynamics::Node::State,
406 std::default_delete<
407 dart::dynamics::Node::
408 State>>>,
409 std::default_delete<
410 dart::common::CloneableVector<
411 std::unique_ptr<
412 dart::dynamics::Node::
413 State,
414 std::default_delete<
415 dart::dynamics::Node::
416 State>>>>>>>>>,
417 &dart::dynamics::detail::setAllNodeStates,
418 &dart::dynamics::detail::getAllNodeStates>,
419 dart::common::ProxyCloneable<
420 dart::common::Aspect::Properties,
421 dart::dynamics::BodyNode,
422 dart::common::CloneableMap<std::map<
423 std::type_index,
424 std::unique_ptr<
425 dart::common::CloneableVector<
426 std::unique_ptr<
427 dart::dynamics::Node::Properties,
428 std::default_delete<
429 dart::dynamics::Node::
430 Properties>>>,
431 std::default_delete<
432 dart::common::CloneableVector<
433 std::unique_ptr<
434 dart::dynamics::Node::
435 Properties,
436 std::default_delete<
437 dart::dynamics::Node::
438 Properties>>>>>,
439 std::less<std::type_index>,
440 std::allocator<std::pair<
441 const std::type_index,
442 std::unique_ptr<
443 dart::common::CloneableVector<
444 std::unique_ptr<
445 dart::dynamics::Node::
446 Properties,
447 std::default_delete<
448 dart::dynamics::Node::
449 Properties>>>,
450 std::default_delete<
451 dart::common::CloneableVector<
452 std::unique_ptr<
453 dart::dynamics::Node::
454 Properties,
455 std::default_delete<
456 dart::dynamics::Node::
457 Properties>>>>>>>>>,
458 &dart::dynamics::detail::setAllNodeProperties,
459 &dart::dynamics::detail::
460 getAllNodeProperties>>>>::AspectState&
461 state) { self->setAspectState(state); },
462 ::py::arg("state"))
463 .def(
464 "setAspectProperties",
465 +[](dart::dynamics::BodyNode* self,
466 const dart::dynamics::BodyNode::AspectProperties& properties) {
467 self->setAspectProperties(properties);
468 },
469 ::py::arg("properties"))
470 .def(
471 "getBodyNodeProperties",
472 +[](const dart::dynamics::BodyNode* self)
473 -> dart::dynamics::BodyNode::Properties {
474 return self->getBodyNodeProperties();
475 })
476 .def(
477 "copy",
478 +[](dart::dynamics::BodyNode* self,
479 const dart::dynamics::BodyNode& otherBodyNode) {
480 self->copy(otherBodyNode);
481 },
482 ::py::arg("otherBodyNode"))
483 .def(
484 "copy",
485 +[](dart::dynamics::BodyNode* self,
486 const dart::dynamics::BodyNode* otherBodyNode) {
487 self->copy(otherBodyNode);
488 },
489 ::py::arg("otherBodyNode"))
490 .def(
491 "duplicateNodes",
492 +[](dart::dynamics::BodyNode* self,
493 const dart::dynamics::BodyNode* otherBodyNode) {
494 self->duplicateNodes(otherBodyNode);
495 },
496 ::py::arg("otherBodyNode"))
497 .def(
498 "matchNodes",
499 +[](dart::dynamics::BodyNode* self,
500 const dart::dynamics::BodyNode* otherBodyNode) {
501 self->matchNodes(otherBodyNode);
502 },
503 ::py::arg("otherBodyNode"))
504 .def(
505 "setName",
506 +[](dart::dynamics::BodyNode* self, const std::string& _name)
507 -> const std::string& { return self->setName(_name); },
508 ::py::return_value_policy::reference_internal,
509 ::py::arg("name"))
510 .def(
511 "getName",
512 +[](const dart::dynamics::BodyNode* self) -> const std::string& {
513 return self->getName();
514 },
515 ::py::return_value_policy::reference_internal)
516 .def(
517 "setGravityMode",
518 +[](dart::dynamics::BodyNode* self, bool _gravityMode) {
519 self->setGravityMode(_gravityMode);
520 },
521 ::py::arg("gravityMode"))
522 .def(
523 "getGravityMode",
524 +[](const dart::dynamics::BodyNode* self)
525 -> bool { return self->getGravityMode(); })
526 .def(
527 "isCollidable",
528 +[](const dart::dynamics::BodyNode* self)
529 -> bool { return self->isCollidable(); })
530 .def(
531 "setCollidable",
532 +[](dart::dynamics::BodyNode* self, bool _isCollidable) {
533 self->setCollidable(_isCollidable);
534 },
535 ::py::arg("isCollidable"))
536 .def(
537 "setMass",
538 +[](dart::dynamics::BodyNode* self,
539 double mass) { self->setMass(mass); },
540 ::py::arg("mass"))
541 .def(
542 "getMass",
543 +[](const dart::dynamics::BodyNode* self)
544 -> double { return self->getMass(); })
545 .def(
546 "setMomentOfInertia",
547 +[](dart::dynamics::BodyNode* self,
548 double _Ixx,
549 double _Iyy,
550 double _Izz) { self->setMomentOfInertia(_Ixx, _Iyy, _Izz); },
551 ::py::arg("Ixx"),
552 ::py::arg("Iyy"),
553 ::py::arg("Izz"))
554 .def(
555 "setMomentOfInertia",
556 +[](dart::dynamics::BodyNode* self,
557 double _Ixx,
558 double _Iyy,
559 double _Izz,
560 double _Ixy) {
561 self->setMomentOfInertia(_Ixx, _Iyy, _Izz, _Ixy);
562 },
563 ::py::arg("Ixx"),
564 ::py::arg("Iyy"),
565 ::py::arg("Izz"),
566 ::py::arg("Ixy"))
567 .def(
568 "setMomentOfInertia",
569 +[](dart::dynamics::BodyNode* self,
570 double _Ixx,
571 double _Iyy,
572 double _Izz,
573 double _Ixy,
574 double _Ixz) {
575 self->setMomentOfInertia(_Ixx, _Iyy, _Izz, _Ixy, _Ixz);
576 },
577 ::py::arg("Ixx"),
578 ::py::arg("Iyy"),
579 ::py::arg("Izz"),
580 ::py::arg("Ixy"),
581 ::py::arg("Ixz"))
582 .def(
583 "setMomentOfInertia",
584 +[](dart::dynamics::BodyNode* self,
585 double _Ixx,
586 double _Iyy,
587 double _Izz,
588 double _Ixy,
589 double _Ixz,
590 double _Iyz) {
591 self->setMomentOfInertia(_Ixx, _Iyy, _Izz, _Ixy, _Ixz, _Iyz);
592 },
593 ::py::arg("Ixx"),
594 ::py::arg("Iyy"),
595 ::py::arg("Izz"),
596 ::py::arg("Ixy"),
597 ::py::arg("Ixz"),
598 ::py::arg("Iyz"))
599 .def(
600 "getMomentOfInertia",
601 +[](const dart::dynamics::BodyNode* self,
602 double& _Ixx,
603 double& _Iyy,
604 double& _Izz,
605 double& _Ixy,
606 double& _Ixz,
607 double& _Iyz) {
608 self->getMomentOfInertia(_Ixx, _Iyy, _Izz, _Ixy, _Ixz, _Iyz);
609 },
610 ::py::arg("Ixx"),
611 ::py::arg("Iyy"),
612 ::py::arg("Izz"),
613 ::py::arg("Ixy"),
614 ::py::arg("Ixz"),
615 ::py::arg("Iyz"))
616 .def(
617 "setInertia",
618 +[](dart::dynamics::BodyNode* self,
619 const dart::dynamics::Inertia& inertia) {
620 self->setInertia(inertia);
621 },
622 ::py::arg("inertia"))
623 .def(
624 "getInertia",
625 +[](const dart::dynamics::BodyNode* self)
626 -> const dart::dynamics::Inertia& { return self->getInertia(); },
627 ::py::return_value_policy::reference_internal)
628 .def(
629 "setLocalCOM",
630 +[](dart::dynamics::BodyNode* self, const Eigen::Vector3d& _com) {
631 self->setLocalCOM(_com);
632 },
633 ::py::arg("com"))
634 .def(
635 "getLocalCOM",
636 +[](const dart::dynamics::BodyNode* self) -> const Eigen::Vector3d& {
637 return self->getLocalCOM();
638 },
639 ::py::return_value_policy::reference_internal)
640 .def(
641 "getCOM",
642 +[](const dart::dynamics::BodyNode* self) -> Eigen::Vector3d {
643 return self->getCOM();
644 })
645 .def(
646 "getCOM",
647 +[](const dart::dynamics::BodyNode* self,
648 const dart::dynamics::Frame* _withRespectTo) -> Eigen::Vector3d {
649 return self->getCOM(_withRespectTo);
650 },
651 ::py::arg("withRespectTo"))
652 .def(
653 "getCOMLinearVelocity",
654 +[](const dart::dynamics::BodyNode* self) -> Eigen::Vector3d {
655 return self->getCOMLinearVelocity();
656 })
657 .def(
658 "getCOMLinearVelocity",
659 +[](const dart::dynamics::BodyNode* self,
660 const dart::dynamics::Frame* _relativeTo) -> Eigen::Vector3d {
661 return self->getCOMLinearVelocity(_relativeTo);
662 },
663 ::py::arg("relativeTo"))
664 .def(
665 "getCOMLinearVelocity",
666 +[](const dart::dynamics::BodyNode* self,
667 const dart::dynamics::Frame* _relativeTo,
668 const dart::dynamics::Frame* _inCoordinatesOf)
669 -> Eigen::Vector3d {
670 return self->getCOMLinearVelocity(_relativeTo, _inCoordinatesOf);
671 },
672 ::py::arg("relativeTo"),
673 ::py::arg("inCoordinatesOf"))
674 .def(
675 "getCOMSpatialVelocity",
676 +[](const dart::dynamics::BodyNode* self) -> Eigen::Vector6d {
677 return self->getCOMSpatialVelocity();
678 })
679 .def(
680 "getCOMSpatialVelocity",
681 +[](const dart::dynamics::BodyNode* self,
682 const dart::dynamics::Frame* _relativeTo,
683 const dart::dynamics::Frame* _inCoordinatesOf)
684 -> Eigen::Vector6d {
685 return self->getCOMSpatialVelocity(_relativeTo, _inCoordinatesOf);
686 },
687 ::py::arg("relativeTo"),
688 ::py::arg("inCoordinatesOf"))
689 .def(
690 "getCOMLinearAcceleration",
691 +[](const dart::dynamics::BodyNode* self) -> Eigen::Vector3d {
692 return self->getCOMLinearAcceleration();
693 })
694 .def(
695 "getCOMLinearAcceleration",
696 +[](const dart::dynamics::BodyNode* self,
697 const dart::dynamics::Frame* _relativeTo) -> Eigen::Vector3d {
698 return self->getCOMLinearAcceleration(_relativeTo);
699 },
700 ::py::arg("relativeTo"))
701 .def(
702 "getCOMLinearAcceleration",
703 +[](const dart::dynamics::BodyNode* self,
704 const dart::dynamics::Frame* _relativeTo,
705 const dart::dynamics::Frame* _inCoordinatesOf)
706 -> Eigen::Vector3d {
707 return self->getCOMLinearAcceleration(
708 _relativeTo, _inCoordinatesOf);
709 },
710 ::py::arg("relativeTo"),
711 ::py::arg("inCoordinatesOf"))
712 .def(
713 "getCOMSpatialAcceleration",
714 +[](const dart::dynamics::BodyNode* self) -> Eigen::Vector6d {
715 return self->getCOMSpatialAcceleration();
716 })
717 .def(
718 "getCOMSpatialAcceleration",
719 +[](const dart::dynamics::BodyNode* self,
720 const dart::dynamics::Frame* _relativeTo,
721 const dart::dynamics::Frame* _inCoordinatesOf)
722 -> Eigen::Vector6d {
723 return self->getCOMSpatialAcceleration(
724 _relativeTo, _inCoordinatesOf);
725 },
726 ::py::arg("relativeTo"),
727 ::py::arg("inCoordinatesOf"))
728 .def(
729 "getIndexInSkeleton",
730 +[](const dart::dynamics::BodyNode* self) -> std::size_t {
731 return self->getIndexInSkeleton();
732 })
733 .def(
734 "getIndexInTree",
735 +[](const dart::dynamics::BodyNode* self) -> std::size_t {
736 return self->getIndexInTree();
737 })
738 .def(
739 "getTreeIndex",
740 +[](const dart::dynamics::BodyNode* self) -> std::size_t {
741 return self->getTreeIndex();
742 })
743 .def(
744 "remove",
745 +[](dart::dynamics::BodyNode* self) -> dart::dynamics::SkeletonPtr {
746 return self->remove();
747 })
748 .def(
749 "remove",
750 +[](dart::dynamics::BodyNode* self, const std::string& _name)
751 -> dart::dynamics::SkeletonPtr { return self->remove(_name); },
752 ::py::arg("name"))
753 .def(
754 "moveTo",
755 +[](dart::dynamics::BodyNode* self,
756 dart::dynamics::BodyNode* _newParent) -> bool {
757 return self->moveTo(_newParent);
758 },
759 ::py::arg("newParent"))
760 .def(
761 "moveTo",
762 +[](dart::dynamics::BodyNode* self,
763 const dart::dynamics::SkeletonPtr& _newSkeleton,
764 dart::dynamics::BodyNode* _newParent) -> bool {
765 return self->moveTo(_newSkeleton, _newParent);
766 },
767 ::py::arg("newSkeleton"),
768 ::py::arg("newParent"))
769 .def(
770 "split",
771 +[](dart::dynamics::BodyNode* self,
772 const std::string& _skeletonName) -> dart::dynamics::SkeletonPtr {
773 return self->split(_skeletonName);
774 },
775 ::py::arg("skeletonName"))
776 .def(
777 "copyTo",
778 +[](dart::dynamics::BodyNode* self,
779 dart::dynamics::BodyNode* _newParent)
780 -> std::pair<dart::dynamics::Joint*, dart::dynamics::BodyNode*> {
781 return self->copyTo(_newParent);
782 },
783 ::py::arg("newParent"))
784 .def(
785 "copyTo",
786 +[](dart::dynamics::BodyNode* self,
787 dart::dynamics::BodyNode* _newParent,
788 bool _recursive)
789 -> std::pair<dart::dynamics::Joint*, dart::dynamics::BodyNode*> {
790 return self->copyTo(_newParent, _recursive);
791 },
792 ::py::arg("newParent"),
793 ::py::arg("recursive"))
794 .def(
795 "copyTo",
796 +[](const dart::dynamics::BodyNode* self,
797 const dart::dynamics::SkeletonPtr& _newSkeleton,
798 dart::dynamics::BodyNode* _newParent)
799 -> std::pair<dart::dynamics::Joint*, dart::dynamics::BodyNode*> {
800 return self->copyTo(_newSkeleton, _newParent);
801 },
802 ::py::arg("newSkeleton"),
803 ::py::arg("newParent"))
804 .def(
805 "copyTo",
806 +[](const dart::dynamics::BodyNode* self,
807 const dart::dynamics::SkeletonPtr& _newSkeleton,
808 dart::dynamics::BodyNode* _newParent,
809 bool _recursive)
810 -> std::pair<dart::dynamics::Joint*, dart::dynamics::BodyNode*> {
811 return self->copyTo(_newSkeleton, _newParent, _recursive);
812 },
813 ::py::arg("newSkeleton"),
814 ::py::arg("newParent"),
815 ::py::arg("recursive"))
816 .def(
817 "copyAs",
818 +[](const dart::dynamics::BodyNode* self,
819 const std::string& _skeletonName) -> dart::dynamics::SkeletonPtr {
820 return self->copyAs(_skeletonName);
821 },
822 ::py::arg("skeletonName"))
823 .def(
824 "copyAs",
825 +[](const dart::dynamics::BodyNode* self,
826 const std::string& _skeletonName,
827 bool _recursive) -> dart::dynamics::SkeletonPtr {
828 return self->copyAs(_skeletonName, _recursive);
829 },
830 ::py::arg("skeletonName"),
831 ::py::arg("recursive"))
832 .def(
833 "getSkeleton",
834 +[](dart::dynamics::BodyNode* self) -> dart::dynamics::SkeletonPtr {
835 return self->getSkeleton();
836 })
837 .def(
838 "getSkeleton",
839 +[](const dart::dynamics::BodyNode* self)
840 -> dart::dynamics::ConstSkeletonPtr {
841 return self->getSkeleton();
842 })
843 .def(
844 "getParentJoint",
845 +[](dart::dynamics::BodyNode* self) -> dart::dynamics::Joint* {
846 return self->getParentJoint();
847 },
848 ::py::return_value_policy::reference_internal)
849 .def(
850 "getParentBodyNode",
851 +[](dart::dynamics::BodyNode* self) -> dart::dynamics::BodyNode* {
852 return self->getParentBodyNode();
853 },
854 ::py::return_value_policy::reference_internal)
855 // clang-format off
856 DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(WeldJoint)
857 DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(RevoluteJoint)
858 DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(PrismaticJoint)
859 DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(ScrewJoint)
860 DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(UniversalJoint)
861 DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(TranslationalJoint2D)
862 DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(PlanarJoint)
863 DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(EulerJoint)
864 DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(BallJoint)
865 DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(TranslationalJoint)
866 DARTPY_DEFINE_CREATE_CHILD_JOINT_AND_BODY_NODE_PAIR(FreeJoint)
867 // clang-format on
868 .def(
869 "getNumChildBodyNodes",
870 +[](const dart::dynamics::BodyNode* self) -> std::size_t {
871 return self->getNumChildBodyNodes();
872 })
873 .def(
874 "getChildBodyNode",
875 +[](dart::dynamics::BodyNode* self,
876 std::size_t index) -> dart::dynamics::BodyNode* {
877 return self->getChildBodyNode(index);
878 },
879 ::py::arg("index"),
880 ::py::return_value_policy::reference_internal)
881 .def(
882 "getNumChildJoints",
883 +[](const dart::dynamics::BodyNode* self) -> std::size_t {
884 return self->getNumChildJoints();
885 })
886 .def(
887 "getChildJoint",
888 +[](dart::dynamics::BodyNode* self, std::size_t index)
889 -> dart::dynamics::Joint* { return self->getChildJoint(index); },
890 ::py::arg("index"),
891 ::py::return_value_policy::reference_internal)
892 .def(
893 "getNumShapeNodes",
894 +[](const dart::dynamics::BodyNode* self) -> std::size_t {
895 return self->getNumShapeNodes();
896 })
897 .def(
898 "getShapeNode",
899 +[](dart::dynamics::BodyNode* self,
900 std::size_t index) -> dart::dynamics::ShapeNode* {
901 return self->getShapeNode(index);
902 },
903 ::py::return_value_policy::reference_internal,
904 ::py::arg("index"))
905 .def(
906 "createShapeNode",
907 +[](dart::dynamics::BodyNode* self,
908 dart::dynamics::ShapePtr shape) -> dart::dynamics::ShapeNode* {
909 return self->createShapeNode(shape);
910 },
911 ::py::return_value_policy::reference_internal,
912 ::py::arg("shape"))
913 .def(
914 "createShapeNode",
915 +[](dart::dynamics::BodyNode* self,
916 dart::dynamics::ShapePtr shape,
917 const std::string& name) -> dart::dynamics::ShapeNode* {
918 return self->createShapeNode(shape, name);
919 },
920 ::py::return_value_policy::reference_internal,
921 ::py::arg("shape"),
922 ::py::arg("name"))
923 .def(
924 "getShapeNodes",
925 +[](dart::dynamics::BodyNode* self)
926 -> const std::vector<dart::dynamics::ShapeNode*> {
927 return self->getShapeNodes();
928 },
929 ::py::return_value_policy::reference_internal)
930 .def(
931 "removeAllShapeNodes",
932 +[](dart::dynamics::BodyNode* self) { self->removeAllShapeNodes(); })
933 .def(
934 "getNumEndEffectors",
935 +[](const dart::dynamics::BodyNode* self) -> std::size_t {
936 return self->getNumEndEffectors();
937 })
938 .def(
939 "getNumMarkers",
940 +[](const dart::dynamics::BodyNode* self) -> std::size_t {
941 return self->getNumMarkers();
942 })
943 .def(
944 "dependsOn",
945 +[](const dart::dynamics::BodyNode* self, std::size_t _genCoordIndex)
946 -> bool { return self->dependsOn(_genCoordIndex); },
947 ::py::arg("genCoordIndex"))
948 .def(
949 "getNumDependentGenCoords",
950 +[](const dart::dynamics::BodyNode* self) -> std::size_t {
951 return self->getNumDependentGenCoords();
952 })
953 .def(
954 "getDependentGenCoordIndex",
955 +[](const dart::dynamics::BodyNode* self,
956 std::size_t _arrayIndex) -> std::size_t {
957 return self->getDependentGenCoordIndex(_arrayIndex);
958 },
959 ::py::arg("arrayIndex"))
960 .def(
961 "getNumDependentDofs",
962 +[](const dart::dynamics::BodyNode* self) -> std::size_t {
963 return self->getNumDependentDofs();
964 })
965 .def(
966 "getChainDofs",
967 +[](const dart::dynamics::BodyNode* self)
968 -> const std::vector<const dart::dynamics::DegreeOfFreedom*> {
969 return self->getChainDofs();
970 })
971 .def(
972 "addExtForce",
973 +[](dart::dynamics::BodyNode* self, const Eigen::Vector3d& _force) {
974 self->addExtForce(_force);
975 },
976 ::py::arg("force"))
977 .def(
978 "addExtForce",
979 +[](dart::dynamics::BodyNode* self,
980 const Eigen::Vector3d& _force,
981 const Eigen::Vector3d& _offset) {
982 self->addExtForce(_force, _offset);
983 },
984 ::py::arg("force"),
985 ::py::arg("offset"))
986 .def(
987 "addExtForce",
988 +[](dart::dynamics::BodyNode* self,
989 const Eigen::Vector3d& _force,
990 const Eigen::Vector3d& _offset,
991 bool _isForceLocal) {
992 self->addExtForce(_force, _offset, _isForceLocal);
993 },
994 ::py::arg("force"),
995 ::py::arg("offset"),
996 ::py::arg("isForceLocal"))
997 .def(
998 "addExtForce",
999 +[](dart::dynamics::BodyNode* self,
1000 const Eigen::Vector3d& _force,
1001 const Eigen::Vector3d& _offset,
1002 bool _isForceLocal,
1003 bool _isOffsetLocal) {
1004 self->addExtForce(_force, _offset, _isForceLocal, _isOffsetLocal);
1005 },
1006 ::py::arg("force"),
1007 ::py::arg("offset"),
1008 ::py::arg("isForceLocal"),
1009 ::py::arg("isOffsetLocal"))
1010 .def(
1011 "setExtForce",
1012 +[](dart::dynamics::BodyNode* self, const Eigen::Vector3d& _force) {
1013 self->setExtForce(_force);
1014 },
1015 ::py::arg("force"))
1016 .def(
1017 "setExtForce",
1018 +[](dart::dynamics::BodyNode* self,
1019 const Eigen::Vector3d& _force,
1020 const Eigen::Vector3d& _offset) {
1021 self->setExtForce(_force, _offset);
1022 },
1023 ::py::arg("force"),
1024 ::py::arg("offset"))
1025 .def(
1026 "setExtForce",
1027 +[](dart::dynamics::BodyNode* self,
1028 const Eigen::Vector3d& _force,
1029 const Eigen::Vector3d& _offset,
1030 bool _isForceLocal) {
1031 self->setExtForce(_force, _offset, _isForceLocal);
1032 },
1033 ::py::arg("force"),
1034 ::py::arg("offset"),
1035 ::py::arg("isForceLocal"))
1036 .def(
1037 "setExtForce",
1038 +[](dart::dynamics::BodyNode* self,
1039 const Eigen::Vector3d& _force,
1040 const Eigen::Vector3d& _offset,
1041 bool _isForceLocal,
1042 bool _isOffsetLocal) {
1043 self->setExtForce(_force, _offset, _isForceLocal, _isOffsetLocal);
1044 },
1045 ::py::arg("force"),
1046 ::py::arg("offset"),
1047 ::py::arg("isForceLocal"),
1048 ::py::arg("isOffsetLocal"))
1049 .def(
1050 "addExtTorque",
1051 +[](dart::dynamics::BodyNode* self, const Eigen::Vector3d& _torque) {
1052 self->addExtTorque(_torque);
1053 },
1054 ::py::arg("torque"))
1055 .def(
1056 "addExtTorque",
1057 +[](dart::dynamics::BodyNode* self,
1058 const Eigen::Vector3d& _torque,
1059 bool _isLocal) { self->addExtTorque(_torque, _isLocal); },
1060 ::py::arg("torque"),
1061 ::py::arg("isLocal"))
1062 .def(
1063 "setExtTorque",
1064 +[](dart::dynamics::BodyNode* self, const Eigen::Vector3d& _torque) {
1065 self->setExtTorque(_torque);
1066 },
1067 ::py::arg("torque"))
1068 .def(
1069 "setExtTorque",
1070 +[](dart::dynamics::BodyNode* self,
1071 const Eigen::Vector3d& _torque,
1072 bool _isLocal) { self->setExtTorque(_torque, _isLocal); },
1073 ::py::arg("torque"),
1074 ::py::arg("isLocal"))
1075 .def(
1076 "clearExternalForces",
1077 +[](dart::dynamics::BodyNode* self) { self->clearExternalForces(); })
1078 .def(
1079 "clearInternalForces",
1080 +[](dart::dynamics::BodyNode* self) { self->clearInternalForces(); })
1081 .def(
1082 "getExternalForceGlobal",
1083 +[](const dart::dynamics::BodyNode* self) -> Eigen::Vector6d {
1084 return self->getExternalForceGlobal();
1085 })
1086 .def(
1087 "getBodyForce",
1088 &dart::dynamics::BodyNode::getBodyForce,
1089 "Get spatial body force transmitted from the parent joint. \n\nThe "
1090 "spatial body force is transmitted to this BodyNode from the parent "
1091 "body through the connecting joint. It is expressed in this "
1092 "BodyNode's frame.")
1093 .def(
1094 "isReactive",
1095 +[](const dart::dynamics::BodyNode* self)
1096 -> bool { return self->isReactive(); })
1097 .def(
1098 "setConstraintImpulse",
1099 +[](dart::dynamics::BodyNode* self,
1100 const Eigen::Vector6d& _constImp) {
1101 self->setConstraintImpulse(_constImp);
1102 },
1103 ::py::arg("constImp"))
1104 .def(
1105 "addConstraintImpulse",
1106 +[](dart::dynamics::BodyNode* self,
1107 const Eigen::Vector6d& _constImp) {
1108 self->addConstraintImpulse(_constImp);
1109 },
1110 ::py::arg("constImp"))
1111 .def(
1112 "addConstraintImpulse",
1113 +[](dart::dynamics::BodyNode* self,
1114 const Eigen::Vector3d& _constImp,
1115 const Eigen::Vector3d& _offset) {
1116 self->addConstraintImpulse(_constImp, _offset);
1117 },
1118 ::py::arg("constImp"),
1119 ::py::arg("offset"))
1120 .def(
1121 "addConstraintImpulse",
1122 +[](dart::dynamics::BodyNode* self,
1123 const Eigen::Vector3d& _constImp,
1124 const Eigen::Vector3d& _offset,
1125 bool _isImpulseLocal) {
1126 self->addConstraintImpulse(_constImp, _offset, _isImpulseLocal);
1127 },
1128 ::py::arg("constImp"),
1129 ::py::arg("offset"),
1130 ::py::arg("isImpulseLocal"))
1131 .def(
1132 "addConstraintImpulse",
1133 +[](dart::dynamics::BodyNode* self,
1134 const Eigen::Vector3d& _constImp,
1135 const Eigen::Vector3d& _offset,
1136 bool _isImpulseLocal,
1137 bool _isOffsetLocal) {
1138 self->addConstraintImpulse(
1139 _constImp, _offset, _isImpulseLocal, _isOffsetLocal);
1140 },
1141 ::py::arg("constImp"),
1142 ::py::arg("offset"),
1143 ::py::arg("isImpulseLocal"),
1144 ::py::arg("isOffsetLocal"))
1145 .def(
1146 "clearConstraintImpulse",
1147 +[](dart::dynamics::BodyNode*
1148 self) { self->clearConstraintImpulse(); })
1149 .def(
1150 "computeLagrangian",
1151 +[](const dart::dynamics::BodyNode* self,
1152 const Eigen::Vector3d& gravity) -> double {
1153 return self->computeLagrangian(gravity);
1154 },
1155 ::py::arg("gravity"))
1156 .def(
1157 "computeKineticEnergy",
1158 +[](const dart::dynamics::BodyNode* self) -> double {
1159 return self->computeKineticEnergy();
1160 })
1161 .def(
1162 "computePotentialEnergy",
1163 +[](const dart::dynamics::BodyNode* self,
1164 const Eigen::Vector3d& gravity) -> double {
1165 return self->computePotentialEnergy(gravity);
1166 },
1167 ::py::arg("gravity"))
1168 .def(
1169 "getLinearMomentum",
1170 +[](const dart::dynamics::BodyNode* self) -> Eigen::Vector3d {
1171 return self->getLinearMomentum();
1172 })
1173 .def(
1174 "getAngularMomentum",
1175 +[](dart::dynamics::BodyNode* self) -> Eigen::Vector3d {
1176 return self->getAngularMomentum();
1177 })
1178 .def(
1179 "getAngularMomentum",
1180 +[](dart::dynamics::BodyNode* self, const Eigen::Vector3d& _pivot)
1181 -> Eigen::Vector3d { return self->getAngularMomentum(_pivot); },
1182 ::py::arg("pivot"))
1183 .def(
1184 "dirtyTransform",
1185 +[](dart::dynamics::BodyNode* self) { self->dirtyTransform(); })
1186 .def(
1187 "dirtyVelocity",
1188 +[](dart::dynamics::BodyNode* self) { self->dirtyVelocity(); })
1189 .def(
1190 "dirtyAcceleration",
1191 +[](dart::dynamics::BodyNode* self) { self->dirtyAcceleration(); })
1192 .def(
1193 "dirtyArticulatedInertia",
1194 +[](dart::dynamics::BodyNode*
1195 self) { self->dirtyArticulatedInertia(); })
1196 .def(
1197 "dirtyExternalForces",
1198 +[](dart::dynamics::BodyNode* self) { self->dirtyExternalForces(); })
1199 .def("dirtyCoriolisForces", +[](dart::dynamics::BodyNode* self) {
1200 self->dirtyCoriolisForces();
1201 });
1202 }
1203
1204 } // namespace python
1205 } // namespace dart
1206