GCC Code Coverage Report

Directory: ./
File: include/tsid/bindings/python/robots/robot-wrapper.hpp
Date: 2025-05-10 01:12:46
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