Directory: | ./ |
---|---|
File: | include/tsid/bindings/python/robots/robot-wrapper.hpp |
Date: | 2024-08-26 20:29:39 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 75 | 113 | 66.4% |
Branches: | 94 | 192 | 49.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 | 7 | void visit(PyClass &cl) const { | |
38 | cl | ||
39 |
4/8✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
|
14 | .def(bp::init<std::string, std_vec, bool>( |
40 |
2/4✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
|
21 | (bp::arg("filename"), bp::arg("package_dir"), bp::arg("verbose")), |
41 | "Default constructor without RootJoint.")) | ||
42 |
5/10✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
|
21 | .def(bp::init<std::string, std_vec, pinocchio::JointModelVariant &, |
43 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | bool>((bp::arg("filename"), bp::arg("package_dir"), |
44 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
21 | bp::arg("roottype"), bp::arg("verbose")), |
45 | "Default constructor with RootJoint.")) | ||
46 |
3/6✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
|
7 | .def(bp::init<pinocchio::Model, bool>( |
47 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | (bp::arg("Pinocchio Model"), bp::arg("verbose")), |
48 | "Default constructor from pinocchio model without RootJoint.")) | ||
49 |
4/8✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
|
14 | .def(bp::init<pinocchio::Model, robots::RobotWrapper::RootJointType, |
50 | bool>( | ||
51 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | (bp::arg("Pinocchio Model"), bp::arg("rootJoint"), |
52 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
14 | bp::arg("verbose")), |
53 | "Default constructor from pinocchio model with RootJoint.")) | ||
54 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("__init__", |
55 | bp::make_constructor(RobotPythonVisitor<Robot>::makeClass)) | ||
56 | 14 | .add_property("nq", &Robot::nq) | |
57 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | .add_property("nv", &Robot::nv) |
58 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | .add_property("na", &Robot::na) |
59 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | .add_property("nq_actuated", &Robot::nq_actuated) |
60 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | .add_property("is_fixed_base", &Robot::is_fixed_base) |
61 | |||
62 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
7 | .def("model", &RobotPythonVisitor::model) |
63 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | .def("data", &RobotPythonVisitor::data) |
64 | |||
65 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | .add_property("rotor_inertias", &RobotPythonVisitor::rotor_inertias) |
66 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | .add_property("gear_ratios", &RobotPythonVisitor::gear_ratios) |
67 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | .def("set_rotor_inertias", &RobotPythonVisitor::set_rotor_inertias, |
68 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
14 | bp::arg("inertia vector")) |
69 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | .def("set_gear_ratios", &RobotPythonVisitor::set_gear_ratios, |
70 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
14 | bp::arg("gear ratio vector")) |
71 | |||
72 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("computeAllTerms", &RobotPythonVisitor::computeAllTerms, |
73 | bp::args("data", "q", "v"), "compute all dynamics") | ||
74 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("com", &RobotPythonVisitor::com, bp::arg("data")) |
75 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("com_vel", &RobotPythonVisitor::com_vel, bp::arg("data")) |
76 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("com_acc", &RobotPythonVisitor::com_acc, bp::arg("data")) |
77 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("Jcom", &RobotPythonVisitor::Jcom, bp::arg("data")) |
78 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("mass", &RobotPythonVisitor::mass, bp::arg("data")) |
79 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | .def("nonLinearEffect", &RobotPythonVisitor::nonLinearEffects, |
80 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
14 | bp::arg("data")) |
81 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("position", &RobotPythonVisitor::position, |
82 | bp::args("data", "index")) | ||
83 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("velocity", &RobotPythonVisitor::velocity, |
84 | bp::args("data", "index")) | ||
85 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("acceleration", &RobotPythonVisitor::acceleration, |
86 | bp::args("data", "index")) | ||
87 | |||
88 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("framePosition", &RobotPythonVisitor::framePosition, |
89 | bp::args("data", "index")) | ||
90 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("frameVelocity", &RobotPythonVisitor::frameVelocity, |
91 | bp::args("data", "index")) | ||
92 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("frameAcceleration", &RobotPythonVisitor::frameAcceleration, |
93 | bp::args("data", "index")) | ||
94 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("frameClassicAcceleration", |
95 | &RobotPythonVisitor::frameClassicAcceleration, | ||
96 | bp::args("data", "index")) | ||
97 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("frameVelocityWorldOriented", |
98 | &RobotPythonVisitor::frameVelocityWorldOriented, | ||
99 | bp::args("data", "index")) | ||
100 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("frameAccelerationWorldOriented", |
101 | &RobotPythonVisitor::frameAccelerationWorldOriented, | ||
102 | bp::args("data", "index")) | ||
103 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
14 | .def("frameClassicAccelerationWorldOriented", |
104 | &RobotPythonVisitor::frameClassicAccelerationWorldOriented, | ||
105 | bp::args("data", "index")) | ||
106 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | .def("angularMomentumTimeVariation", |
107 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
14 | &RobotPythonVisitor::angularMomentumTimeVariation, bp::arg("data")) |
108 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
7 | .def("setGravity", &RobotPythonVisitor::setGravity, bp::arg("gravity")); |
109 | 7 | } | |
110 | |||
111 | 5 | static boost::shared_ptr<Robot> makeClass( | |
112 | const std::string &filename, const std::vector<std::string> &stdvec, | ||
113 | bp::object &bpObject, bool verbose) { | ||
114 | 5 | pinocchio::JointModelFreeFlyer root_joint = | |
115 |
3/6✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
|
5 | bp::extract<pinocchio::JointModelFreeFlyer>(bpObject)(); |
116 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
10 | boost::shared_ptr<Robot> p( |
117 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
5 | new tsid::robots::RobotWrapper(filename, stdvec, root_joint, verbose)); |
118 | 10 | return p; | |
119 | } | ||
120 | |||
121 | 1012 | static pinocchio::Model model(const Robot &self) { return self.model(); } | |
122 | 1004 | static pinocchio::Data data(const Robot &self) { | |
123 | 1004 | pinocchio::Data data(self.model()); | |
124 | 1004 | 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 | ✗ | static bool set_rotor_inertias(Robot &self, Eigen::VectorXd &rotor_inertias) { | |
133 | ✗ | return self.rotor_inertias(rotor_inertias); | |
134 | } | ||
135 | ✗ | static bool set_gear_ratios(Robot &self, Eigen::VectorXd &gear_ratios) { | |
136 | ✗ | return self.gear_ratios(gear_ratios); | |
137 | } | ||
138 | |||
139 | 3 | static Eigen::Vector3d com(const Robot &self, const pinocchio::Data &data) { | |
140 | 3 | return self.com(data); | |
141 | } | ||
142 | ✗ | static Eigen::Vector3d com_vel(const Robot &self, | |
143 | const pinocchio::Data &data) { | ||
144 | ✗ | return self.com_vel(data); | |
145 | } | ||
146 | ✗ | static Eigen::Vector3d com_acc(const Robot &self, | |
147 | const pinocchio::Data &data) { | ||
148 | ✗ | return self.com_acc(data); | |
149 | } | ||
150 | ✗ | static Matrix3x Jcom(const Robot &self, const pinocchio::Data &data) { | |
151 | ✗ | return self.Jcom(data); | |
152 | } | ||
153 | 6001 | static void computeAllTerms(const Robot &self, pinocchio::Data &data, | |
154 | const Eigen::VectorXd &q, | ||
155 | const Eigen::VectorXd &v) { | ||
156 | 6001 | self.computeAllTerms(data, q, v); | |
157 | 6001 | } | |
158 | ✗ | static Eigen::MatrixXd mass(Robot &self, pinocchio::Data &data) { | |
159 | ✗ | return self.mass(data); | |
160 | } | ||
161 | ✗ | static Eigen::VectorXd nonLinearEffects(const Robot &self, | |
162 | const pinocchio::Data &data) { | ||
163 | ✗ | return self.nonLinearEffects(data); | |
164 | } | ||
165 | 3 | static pinocchio::SE3 position(const Robot &self, const pinocchio::Data &data, | |
166 | const pinocchio::Model::JointIndex &index) { | ||
167 | 3 | return self.position(data, index); | |
168 | } | ||
169 | ✗ | static pinocchio::Motion velocity(const Robot &self, | |
170 | const pinocchio::Data &data, | ||
171 | const pinocchio::Model::JointIndex &index) { | ||
172 | ✗ | return self.velocity(data, index); | |
173 | } | ||
174 | ✗ | static pinocchio::Motion acceleration( | |
175 | const Robot &self, const pinocchio::Data &data, | ||
176 | const pinocchio::Model::JointIndex &index) { | ||
177 | ✗ | return self.acceleration(data, index); | |
178 | } | ||
179 | ✗ | static pinocchio::SE3 framePosition( | |
180 | const Robot &self, const pinocchio::Data &data, | ||
181 | const pinocchio::Model::FrameIndex &index) { | ||
182 | ✗ | return self.framePosition(data, index); | |
183 | } | ||
184 | ✗ | static pinocchio::Motion frameVelocity( | |
185 | const Robot &self, const pinocchio::Data &data, | ||
186 | const pinocchio::Model::FrameIndex &index) { | ||
187 | ✗ | return self.frameVelocity(data, index); | |
188 | } | ||
189 | ✗ | static pinocchio::Motion frameAcceleration( | |
190 | const Robot &self, const pinocchio::Data &data, | ||
191 | const pinocchio::Model::FrameIndex &index) { | ||
192 | ✗ | return self.frameAcceleration(data, index); | |
193 | } | ||
194 | ✗ | static pinocchio::Motion frameClassicAcceleration( | |
195 | const Robot &self, const pinocchio::Data &data, | ||
196 | const pinocchio::Model::FrameIndex &index) { | ||
197 | ✗ | return self.frameClassicAcceleration(data, index); | |
198 | } | ||
199 | ✗ | static pinocchio::Motion frameVelocityWorldOriented( | |
200 | const Robot &self, const pinocchio::Data &data, | ||
201 | const pinocchio::Model::FrameIndex &index) { | ||
202 | ✗ | return self.frameVelocityWorldOriented(data, index); | |
203 | } | ||
204 | ✗ | static pinocchio::Motion frameAccelerationWorldOriented( | |
205 | const Robot &self, const pinocchio::Data &data, | ||
206 | const pinocchio::Model::FrameIndex &index) { | ||
207 | ✗ | return self.frameAccelerationWorldOriented(data, index); | |
208 | } | ||
209 | ✗ | static pinocchio::Motion frameClassicAccelerationWorldOriented( | |
210 | const Robot &self, const pinocchio::Data &data, | ||
211 | const pinocchio::Model::FrameIndex &index) { | ||
212 | ✗ | return self.frameClassicAccelerationWorldOriented(data, index); | |
213 | } | ||
214 | ✗ | static Eigen::Vector3d angularMomentumTimeVariation( | |
215 | const Robot &self, const pinocchio::Data &data) { | ||
216 | ✗ | return self.angularMomentumTimeVariation(data); | |
217 | } | ||
218 | 1 | static void setGravity(Robot &self, const pinocchio::Motion &gravity) { | |
219 | 1 | return self.setGravity(gravity); | |
220 | } | ||
221 | 7 | static void expose(const std::string &class_name) { | |
222 |
1/2✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
|
7 | std::string doc = "Robot Wrapper info."; |
223 |
1/2✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
|
7 | bp::class_<Robot>(class_name.c_str(), doc.c_str(), bp::no_init) |
224 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | .def(RobotPythonVisitor<Robot>()); |
225 | ; | ||
226 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
14 | bp::enum_<robots::RobotWrapper::RootJointType>("RootJointType") |
227 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | .value("FIXED_BASE_SYSTEM", robots::RobotWrapper::FIXED_BASE_SYSTEM) |
228 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | .value("FLOATING_BASE_SYSTEM", |
229 | robots::RobotWrapper::FLOATING_BASE_SYSTEM) | ||
230 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | .export_values(); |
231 | 7 | } | |
232 | }; | ||
233 | } // namespace python | ||
234 | } // namespace tsid | ||
235 | |||
236 | #endif // ifndef __tsid_python_robot_wrapper_hpp__ | ||
237 |