Directory: | ./ |
---|---|
File: | include/tsid/bindings/python/robots/robot-wrapper.hpp |
Date: | 2025-03-29 14:29:37 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 81 | 113 | 71.7% |
Branches: | 96 | 192 | 50.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2018 CNRS | ||
3 | // | ||
4 | // This file is part of tsid | ||
5 | // tsid is free software: you can redistribute it | ||
6 | // and/or modify it under the terms of the GNU Lesser General Public | ||
7 | // License as published by the Free Software Foundation, either version | ||
8 | // 3 of the License, or (at your option) any later version. | ||
9 | // tsid is distributed in the hope that it will be | ||
10 | // useful, but WITHOUT ANY WARRANTY; without even the implied warranty | ||
11 | // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU | ||
12 | // General Lesser Public License for more details. You should have | ||
13 | // received a copy of the GNU Lesser General Public License along with | ||
14 | // tsid If not, see | ||
15 | // <http://www.gnu.org/licenses/>. | ||
16 | // | ||
17 | |||
18 | #ifndef __tsid_python_robot_wrapper_hpp__ | ||
19 | #define __tsid_python_robot_wrapper_hpp__ | ||
20 | |||
21 | #include "tsid/bindings/python/fwd.hpp" | ||
22 | |||
23 | #include "tsid/robots/robot-wrapper.hpp" | ||
24 | |||
25 | namespace tsid { | ||
26 | namespace python { | ||
27 | namespace bp = boost::python; | ||
28 | |||
29 | template <typename Robot> | ||
30 | struct RobotPythonVisitor | ||
31 | : public boost::python::def_visitor<RobotPythonVisitor<Robot> > { | ||
32 | typedef std::vector<std::string> std_vec; | ||
33 | typedef Eigen::Matrix<double, 3, Eigen::Dynamic> Matrix3x; | ||
34 | |||
35 | template <class PyClass> | ||
36 | |||
37 | 8 | void visit(PyClass &cl) const { | |
38 | cl | ||
39 |
4/8✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
|
16 | .def(bp::init<std::string, std_vec, bool>( |
40 |
2/4✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
|
24 | (bp::arg("filename"), bp::arg("package_dir"), bp::arg("verbose")), |
41 | "Default constructor without RootJoint.")) | ||
42 |
5/10✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
|
24 | .def(bp::init<std::string, std_vec, pinocchio::JointModelVariant &, |
43 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | bool>((bp::arg("filename"), bp::arg("package_dir"), |
44 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
24 | bp::arg("roottype"), bp::arg("verbose")), |
45 | "Default constructor with RootJoint.")) | ||
46 |
3/6✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
|
8 | .def(bp::init<pinocchio::Model, bool>( |
47 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | (bp::arg("Pinocchio Model"), bp::arg("verbose")), |
48 | "Default constructor from pinocchio model without RootJoint.")) | ||
49 |
4/8✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
|
16 | .def(bp::init<pinocchio::Model, robots::RobotWrapper::RootJointType, |
50 | bool>( | ||
51 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | (bp::arg("Pinocchio Model"), bp::arg("rootJoint"), |
52 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
16 | bp::arg("verbose")), |
53 | "Default constructor from pinocchio model with RootJoint.")) | ||
54 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | .def("__init__", |
55 | bp::make_constructor(RobotPythonVisitor<Robot>::makeClass)) | ||
56 | 16 | .add_property("nq", &Robot::nq) | |
57 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | .add_property("nv", &Robot::nv) |
58 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | .add_property("na", &Robot::na) |
59 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | .add_property("nq_actuated", &Robot::nq_actuated) |
60 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | .add_property("is_fixed_base", &Robot::is_fixed_base) |
61 | |||
62 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
8 | .def("model", &RobotPythonVisitor::model) |
63 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | .def("data", &RobotPythonVisitor::data) |
64 | |||
65 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | .add_property("rotor_inertias", &RobotPythonVisitor::rotor_inertias) |
66 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | .add_property("gear_ratios", &RobotPythonVisitor::gear_ratios) |
67 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | .def("set_rotor_inertias", &RobotPythonVisitor::set_rotor_inertias, |
68 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
16 | bp::arg("inertia vector")) |
69 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | .def("set_gear_ratios", &RobotPythonVisitor::set_gear_ratios, |
70 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
16 | bp::arg("gear ratio vector")) |
71 | |||
72 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | .def("computeAllTerms", &RobotPythonVisitor::computeAllTerms, |
73 | bp::args("data", "q", "v"), "compute all dynamics") | ||
74 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | .def("com", &RobotPythonVisitor::com, bp::arg("data")) |
75 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | .def("com_vel", &RobotPythonVisitor::com_vel, bp::arg("data")) |
76 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | .def("com_acc", &RobotPythonVisitor::com_acc, bp::arg("data")) |
77 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | .def("Jcom", &RobotPythonVisitor::Jcom, bp::arg("data")) |
78 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | .def("mass", &RobotPythonVisitor::mass, bp::arg("data")) |
79 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | .def("nonLinearEffect", &RobotPythonVisitor::nonLinearEffects, |
80 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
16 | bp::arg("data")) |
81 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | .def("position", &RobotPythonVisitor::position, |
82 | bp::args("data", "index")) | ||
83 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | .def("velocity", &RobotPythonVisitor::velocity, |
84 | bp::args("data", "index")) | ||
85 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | .def("acceleration", &RobotPythonVisitor::acceleration, |
86 | bp::args("data", "index")) | ||
87 | |||
88 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | .def("framePosition", &RobotPythonVisitor::framePosition, |
89 | bp::args("data", "index")) | ||
90 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | .def("frameVelocity", &RobotPythonVisitor::frameVelocity, |
91 | bp::args("data", "index")) | ||
92 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | .def("frameAcceleration", &RobotPythonVisitor::frameAcceleration, |
93 | bp::args("data", "index")) | ||
94 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | .def("frameClassicAcceleration", |
95 | &RobotPythonVisitor::frameClassicAcceleration, | ||
96 | bp::args("data", "index")) | ||
97 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | .def("frameVelocityWorldOriented", |
98 | &RobotPythonVisitor::frameVelocityWorldOriented, | ||
99 | bp::args("data", "index")) | ||
100 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | .def("frameAccelerationWorldOriented", |
101 | &RobotPythonVisitor::frameAccelerationWorldOriented, | ||
102 | bp::args("data", "index")) | ||
103 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | .def("frameClassicAccelerationWorldOriented", |
104 | &RobotPythonVisitor::frameClassicAccelerationWorldOriented, | ||
105 | bp::args("data", "index")) | ||
106 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | .def("angularMomentumTimeVariation", |
107 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
16 | &RobotPythonVisitor::angularMomentumTimeVariation, bp::arg("data")) |
108 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
8 | .def("setGravity", &RobotPythonVisitor::setGravity, bp::arg("gravity")); |
109 | 8 | } | |
110 | |||
111 | 6 | static boost::shared_ptr<Robot> makeClass( | |
112 | const std::string &filename, const std::vector<std::string> &stdvec, | ||
113 | bp::object &bpObject, bool verbose) { | ||
114 | 6 | pinocchio::JointModelFreeFlyer root_joint = | |
115 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
6 | bp::extract<pinocchio::JointModelFreeFlyer>(bpObject)(); |
116 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | boost::shared_ptr<Robot> p( |
117 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | new tsid::robots::RobotWrapper(filename, stdvec, root_joint, verbose)); |
118 | 12 | return p; | |
119 | } | ||
120 | |||
121 | 1013 | static pinocchio::Model model(const Robot &self) { return self.model(); } | |
122 | 1005 | static pinocchio::Data data(const Robot &self) { | |
123 | 1005 | pinocchio::Data data(self.model()); | |
124 | 1005 | return data; | |
125 | } | ||
126 | ✗ | static Eigen::VectorXd rotor_inertias(const Robot &self) { | |
127 | ✗ | return self.rotor_inertias(); | |
128 | } | ||
129 | ✗ | static Eigen::VectorXd gear_ratios(const Robot &self) { | |
130 | ✗ | return self.gear_ratios(); | |
131 | } | ||
132 | 1 | static bool set_rotor_inertias(Robot &self, | |
133 | const Eigen::VectorXd &rotor_inertias) { | ||
134 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | return self.rotor_inertias(rotor_inertias); |
135 | } | ||
136 | 1 | static bool set_gear_ratios(Robot &self, const Eigen::VectorXd &gear_ratios) { | |
137 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | return self.gear_ratios(gear_ratios); |
138 | } | ||
139 | |||
140 | 3 | static Eigen::Vector3d com(const Robot &self, const pinocchio::Data &data) { | |
141 | 3 | return self.com(data); | |
142 | } | ||
143 | ✗ | static Eigen::Vector3d com_vel(const Robot &self, | |
144 | const pinocchio::Data &data) { | ||
145 | ✗ | return self.com_vel(data); | |
146 | } | ||
147 | ✗ | static Eigen::Vector3d com_acc(const Robot &self, | |
148 | const pinocchio::Data &data) { | ||
149 | ✗ | return self.com_acc(data); | |
150 | } | ||
151 | ✗ | static Matrix3x Jcom(const Robot &self, const pinocchio::Data &data) { | |
152 | ✗ | return self.Jcom(data); | |
153 | } | ||
154 | 6002 | static void computeAllTerms(const Robot &self, pinocchio::Data &data, | |
155 | const Eigen::VectorXd &q, | ||
156 | const Eigen::VectorXd &v) { | ||
157 | 6002 | self.computeAllTerms(data, q, v); | |
158 | 6002 | } | |
159 | 2 | static Eigen::MatrixXd mass(Robot &self, pinocchio::Data &data) { | |
160 | 2 | return self.mass(data); | |
161 | } | ||
162 | ✗ | static Eigen::VectorXd nonLinearEffects(const Robot &self, | |
163 | const pinocchio::Data &data) { | ||
164 | ✗ | return self.nonLinearEffects(data); | |
165 | } | ||
166 | 3 | static pinocchio::SE3 position(const Robot &self, const pinocchio::Data &data, | |
167 | const pinocchio::Model::JointIndex &index) { | ||
168 | 3 | return self.position(data, index); | |
169 | } | ||
170 | ✗ | static pinocchio::Motion velocity(const Robot &self, | |
171 | const pinocchio::Data &data, | ||
172 | const pinocchio::Model::JointIndex &index) { | ||
173 | ✗ | return self.velocity(data, index); | |
174 | } | ||
175 | ✗ | static pinocchio::Motion acceleration( | |
176 | const Robot &self, const pinocchio::Data &data, | ||
177 | const pinocchio::Model::JointIndex &index) { | ||
178 | ✗ | return self.acceleration(data, index); | |
179 | } | ||
180 | ✗ | static pinocchio::SE3 framePosition( | |
181 | const Robot &self, const pinocchio::Data &data, | ||
182 | const pinocchio::Model::FrameIndex &index) { | ||
183 | ✗ | return self.framePosition(data, index); | |
184 | } | ||
185 | ✗ | static pinocchio::Motion frameVelocity( | |
186 | const Robot &self, const pinocchio::Data &data, | ||
187 | const pinocchio::Model::FrameIndex &index) { | ||
188 | ✗ | return self.frameVelocity(data, index); | |
189 | } | ||
190 | ✗ | static pinocchio::Motion frameAcceleration( | |
191 | const Robot &self, const pinocchio::Data &data, | ||
192 | const pinocchio::Model::FrameIndex &index) { | ||
193 | ✗ | return self.frameAcceleration(data, index); | |
194 | } | ||
195 | ✗ | static pinocchio::Motion frameClassicAcceleration( | |
196 | const Robot &self, const pinocchio::Data &data, | ||
197 | const pinocchio::Model::FrameIndex &index) { | ||
198 | ✗ | return self.frameClassicAcceleration(data, index); | |
199 | } | ||
200 | ✗ | static pinocchio::Motion frameVelocityWorldOriented( | |
201 | const Robot &self, const pinocchio::Data &data, | ||
202 | const pinocchio::Model::FrameIndex &index) { | ||
203 | ✗ | return self.frameVelocityWorldOriented(data, index); | |
204 | } | ||
205 | ✗ | static pinocchio::Motion frameAccelerationWorldOriented( | |
206 | const Robot &self, const pinocchio::Data &data, | ||
207 | const pinocchio::Model::FrameIndex &index) { | ||
208 | ✗ | return self.frameAccelerationWorldOriented(data, index); | |
209 | } | ||
210 | ✗ | static pinocchio::Motion frameClassicAccelerationWorldOriented( | |
211 | const Robot &self, const pinocchio::Data &data, | ||
212 | const pinocchio::Model::FrameIndex &index) { | ||
213 | ✗ | return self.frameClassicAccelerationWorldOriented(data, index); | |
214 | } | ||
215 | ✗ | static Eigen::Vector3d angularMomentumTimeVariation( | |
216 | const Robot &self, const pinocchio::Data &data) { | ||
217 | ✗ | return self.angularMomentumTimeVariation(data); | |
218 | } | ||
219 | 1 | static void setGravity(Robot &self, const pinocchio::Motion &gravity) { | |
220 | 1 | return self.setGravity(gravity); | |
221 | } | ||
222 | 8 | static void expose(const std::string &class_name) { | |
223 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
8 | std::string doc = "Robot Wrapper info."; |
224 |
1/2✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
|
8 | bp::class_<Robot>(class_name.c_str(), doc.c_str(), bp::no_init) |
225 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | .def(RobotPythonVisitor<Robot>()); |
226 | ; | ||
227 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
16 | bp::enum_<robots::RobotWrapper::RootJointType>("RootJointType") |
228 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | .value("FIXED_BASE_SYSTEM", robots::RobotWrapper::FIXED_BASE_SYSTEM) |
229 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | .value("FLOATING_BASE_SYSTEM", |
230 | robots::RobotWrapper::FLOATING_BASE_SYSTEM) | ||
231 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | .export_values(); |
232 | 8 | } | |
233 | }; | ||
234 | } // namespace python | ||
235 | } // namespace tsid | ||
236 | |||
237 | #endif // ifndef __tsid_python_robot_wrapper_hpp__ | ||
238 |