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 
39 namespace py = pybind11;
40 
41 namespace dart {
42 namespace python {
43 
Inertia(py::module & m)44 void Inertia(py::module& m)
45 {
46   ::py::class_<
47       dart::dynamics::Inertia,
48       std::shared_ptr<dart::dynamics::Inertia>>(m, "Inertia")
49       .def(
50           ::py::init<double, const Eigen::Vector3d&, const Eigen::Matrix3d&>(),
51           ::py::arg_v("mass", 1),
52           ::py::arg_v(
53               "com", Eigen::Vector3d::Zero(), "Eigen::Vector3d::Zero()"),
54           ::py::arg_v(
55               "momentOfInertia",
56               Eigen::Matrix3d::Identity(),
57               "Eigen::Matrix3d::Identity()"))
58       .def(
59           ::py::init<const Eigen::Matrix6d&>(),
60           ::py::arg("spatialInertiaTensor"))
61       .def(
62           "setMass",
63           +[](dart::dynamics::Inertia* self, double mass) {
64             self->setMass(mass);
65           },
66           ::py::arg("mass"))
67       .def(
68           "getMass",
69           +[](const dart::dynamics::Inertia* self) -> double {
70             return self->getMass();
71           })
72       .def(
73           "setLocalCOM",
74           +[](dart::dynamics::Inertia* self, const Eigen::Vector3d& com) {
75             self->setLocalCOM(com);
76           },
77           ::py::arg("com"))
78       .def(
79           "getLocalCOM",
80           +[](const dart::dynamics::Inertia* self) -> const Eigen::Vector3d& {
81             return self->getLocalCOM();
82           },
83           ::py::return_value_policy::reference_internal)
84       .def(
85           "setMoment",
86           +[](dart::dynamics::Inertia* self, const Eigen::Matrix3d& moment) {
87             self->setMoment(moment);
88           },
89           ::py::arg("moment"))
90       .def(
91           "getMoment",
92           +[](const dart::dynamics::Inertia* self) -> Eigen::Matrix3d {
93             return self->getMoment();
94           })
95       .def(
96           "setSpatialTensor",
97           +[](dart::dynamics::Inertia* self, const Eigen::Matrix6d& spatial) {
98             self->setSpatialTensor(spatial);
99           },
100           ::py::arg("spatial"))
101       .def(
102           "getSpatialTensor",
103           +[](const dart::dynamics::Inertia* self) -> const Eigen::Matrix6d& {
104             return self->getSpatialTensor();
105           },
106           ::py::return_value_policy::reference_internal)
107       .def(
108           "verify",
109           +[](const dart::dynamics::Inertia* self,
110               bool printWarnings,
111               double tolerance) -> bool {
112             return self->verify(printWarnings, tolerance);
113           },
114           ::py::arg_v("printWarnings", true),
115           ::py::arg_v("tolerance", 1e-8))
116       .def(
117           "__eq__",
118           +[](const dart::dynamics::Inertia* self,
119               const dart::dynamics::Inertia& other) -> bool {
120             return self && *self == other;
121           })
122       .def_static(
123           "verifyMoment",
124           +[](const Eigen::Matrix3d& moment,
125               bool printWarnings,
126               double tolerance) -> bool {
127             return dart::dynamics::Inertia::verifyMoment(
128                 moment, printWarnings, tolerance);
129           },
130           ::py::arg("moment"),
131           ::py::arg_v("printWarnings", true),
132           ::py::arg_v("tolerance", 1e-8))
133       .def_static(
134           "verifySpatialTensor",
135           +[](const Eigen::Matrix6d& spatial,
136               bool printWarnings = true,
137               double tolerance = 1e-8) -> bool {
138             return dart::dynamics::Inertia::verifySpatialTensor(
139                 spatial, printWarnings, tolerance);
140           },
141           ::py::arg("spatial"),
142           ::py::arg_v("printWarnings", true),
143           ::py::arg_v("tolerance", 1e-8));
144 }
145 
146 } // namespace python
147 } // namespace dart
148