GCC Code Coverage Report

Directory: ./
File: include/tsid/bindings/python/contacts/contact-6d.hpp
Date: 2025-05-10 01:12:46
Exec Total Coverage
Lines: 56 96 58.3%
Branches: 81 224 36.2%
Line Branch Exec Source
1 //
2 // Copyright (c) 2018 CNRS, NYU, MPI Tübingen
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_contact_6d_hpp__
19 #define __tsid_python_contact_6d_hpp__
20
21 #include "tsid/bindings/python/fwd.hpp"
22
23 #include "tsid/contacts/contact-6d.hpp"
24 #include "tsid/robots/robot-wrapper.hpp"
25 #include "tsid/math/constraint-inequality.hpp"
26 #include "tsid/math/constraint-equality.hpp"
27 #include "tsid/math/constraint-base.hpp"
28 #include "tsid/tasks/task-se3-equality.hpp"
29
30 namespace tsid {
31 namespace python {
32 namespace bp = boost::python;
33
34 template <typename Contact6d>
35 struct Contact6DPythonVisitor
36 : public boost::python::def_visitor<Contact6DPythonVisitor<Contact6d> > {
37 template <class PyClass>
38
39 8 void visit(PyClass &cl) const {
40
9/18
✓ 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.
✓ Branch 16 taken 8 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 8 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 8 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 8 times.
✗ Branch 26 not taken.
 56 cl.def(bp::init<std::string, robots::RobotWrapper &, std::string,
41 Eigen::MatrixXd, Eigen::VectorXd, double, double, double>(
42
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 24 (bp::arg("name"), bp::arg("robot"), bp::arg("framename"),
43
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 24 bp::arg("contactPoint"), bp::arg("contactNormal"),
44
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 24 bp::arg("frictionCoeff"), bp::arg("minForce"),
45
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 bp::arg("maxForce")),
46 "Default Constructor"))
47
10/20
✓ 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.
✓ Branch 16 taken 8 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 8 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 8 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 8 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 8 times.
✗ Branch 29 not taken.
 64 .def(bp::init<std::string, robots::RobotWrapper &, std::string,
48 Eigen::MatrixXd, Eigen::VectorXd, double, double, double,
49 double>(
50
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.
 24 (bp::arg("name"), bp::arg("robot"), bp::arg("framename"),
51
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 24 bp::arg("contactPoint"), bp::arg("contactNormal"),
52
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.
 32 bp::arg("frictionCoeff"), bp::arg("minForce"), bp::arg("maxForce"),
53
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 bp::arg("wForceReg")),
54 "Deprecated Constructor"))
55 16 .add_property("n_motion", &Contact6d::n_motion,
56 "return number of motion")
57
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 .add_property("n_force", &Contact6d::n_force, "return number of force")
58
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 .add_property("name", &Contact6DPythonVisitor::name, "return name")
59
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("computeMotionTask", &Contact6DPythonVisitor::computeMotionTask,
60 bp::args("t", "q", "v", "data"))
61
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("computeForceTask", &Contact6DPythonVisitor::computeForceTask,
62 bp::args("t", "q", "v", "data"))
63
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("computeForceRegularizationTask",
64 &Contact6DPythonVisitor::computeForceRegularizationTask,
65 bp::args("t", "q", "v", "data"))
66
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 .def("getMotionTask", &Contact6DPythonVisitor::getMotionTask)
67
68
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 .add_property("getForceGeneratorMatrix",
69 bp::make_function(
70 &Contact6DPythonVisitor::getForceGeneratorMatrix,
71 8 bp::return_value_policy<bp::copy_const_reference>()))
72
73
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 .def("getNormalForce", &Contact6DPythonVisitor::getNormalForce,
74
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 bp::arg("vec"))
75 16 .add_property("getMinNormalForce", &Contact6d::getMinNormalForce)
76
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 .add_property("getMaxNormalForce", &Contact6d::getMaxNormalForce)
77
78
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 .add_property("Kp",
79 bp::make_function(
80 &Contact6DPythonVisitor::Kp,
81 8 bp::return_value_policy<bp::copy_const_reference>()))
82
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 .add_property("Kd",
83 bp::make_function(
84 &Contact6DPythonVisitor::Kd,
85 8 bp::return_value_policy<bp::copy_const_reference>()))
86
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("setKp", &Contact6DPythonVisitor::setKp, bp::arg("Kp"))
87
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("setKd", &Contact6DPythonVisitor::setKd, bp::arg("Kd"))
88
89
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("setContactPoints", &Contact6DPythonVisitor::setContactPoints,
90 bp::args("vec"))
91
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("setContactNormal", &Contact6DPythonVisitor::setContactNormal,
92 bp::args("vec"))
93
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("setFrictionCoefficient",
94 &Contact6DPythonVisitor::setFrictionCoefficient,
95 bp::args("friction_coeff"))
96
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("setMinNormalForce", &Contact6DPythonVisitor::setMinNormalForce,
97 bp::args("min_force"))
98
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("setMaxNormalForce", &Contact6DPythonVisitor::setMaxNormalForce,
99 bp::args("max_force"))
100
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("setReference", &Contact6DPythonVisitor::setReference,
101 bp::args("SE3"))
102
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 .def("setForceReference", &Contact6DPythonVisitor::setForceReference,
103 bp::args("f_vec"))
104
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 .def("setRegularizationTaskWeightVector",
105 &Contact6DPythonVisitor::setRegularizationTaskWeightVector,
106 bp::args("w_vec"));
107 8 }
108 39 static std::string name(Contact6d &self) {
109 39 std::string name = self.name();
110 39 return name;
111 }
112
113 ✗ static math::ConstraintEquality computeMotionTask(Contact6d &self,
114 const double t,
115 const Eigen::VectorXd &q,
116 const Eigen::VectorXd &v,
117 pinocchio::Data &data) {
118 ✗ self.computeMotionTask(t, q, v, data);
119 ✗ math::ConstraintEquality cons(self.getMotionConstraint().name(),
120 ✗ self.getMotionConstraint().matrix(),
121 ✗ self.getMotionConstraint().vector());
122 ✗ return cons;
123 }
124 ✗ static math::ConstraintInequality computeForceTask(
125 Contact6d &self, const double t, const Eigen::VectorXd &q,
126 const Eigen::VectorXd &v, const pinocchio::Data &data) {
127 ✗ self.computeForceTask(t, q, v, data);
128 ✗ math::ConstraintInequality cons(self.getForceConstraint().name(),
129 ✗ self.getForceConstraint().matrix(),
130 ✗ self.getForceConstraint().lowerBound(),
131 ✗ self.getForceConstraint().upperBound());
132 ✗ return cons;
133 }
134 ✗ static math::ConstraintEquality computeForceRegularizationTask(
135 Contact6d &self, const double t, const Eigen::VectorXd &q,
136 const Eigen::VectorXd &v, const pinocchio::Data &data) {
137 ✗ self.computeForceRegularizationTask(t, q, v, data);
138 ✗ math::ConstraintEquality cons(self.getForceRegularizationTask().name(),
139 ✗ self.getForceRegularizationTask().matrix(),
140 ✗ self.getForceRegularizationTask().vector());
141 ✗ return cons;
142 }
143 ✗ static tsid::tasks::TaskSE3Equality getMotionTask(Contact6d &self) {
144 ✗ tsid::tasks::TaskSE3Equality t = self.getMotionTask();
145 ✗ return t;
146 }
147
148 ✗ static const Eigen::MatrixXd &getForceGeneratorMatrix(Contact6d &self) {
149 ✗ return self.getForceGeneratorMatrix();
150 }
151 ✗ static const Eigen::VectorXd &Kp(Contact6d &self) { return self.Kp(); }
152 ✗ static const Eigen::VectorXd &Kd(Contact6d &self) { return self.Kd(); }
153 2 static void setKp(Contact6d &self, const ::Eigen::VectorXd Kp) {
154
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 return self.Kp(Kp);
155 }
156 2 static void setKd(Contact6d &self, const ::Eigen::VectorXd Kd) {
157
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 return self.Kd(Kd);
158 }
159 ✗ static bool setContactPoints(Contact6d &self,
160 const ::Eigen::MatrixXd contactpoints) {
161 ✗ return self.setContactPoints(contactpoints);
162 }
163 ✗ static bool setContactNormal(Contact6d &self,
164 const ::Eigen::VectorXd contactNormal) {
165 ✗ return self.setContactNormal(contactNormal);
166 }
167 ✗ static bool setFrictionCoefficient(Contact6d &self,
168 const double frictionCoefficient) {
169 ✗ return self.setFrictionCoefficient(frictionCoefficient);
170 }
171 ✗ static bool setMinNormalForce(Contact6d &self, const double minNormalForce) {
172 ✗ return self.setMinNormalForce(minNormalForce);
173 }
174 ✗ static bool setMaxNormalForce(Contact6d &self, const double maxNormalForce) {
175 ✗ return self.setMaxNormalForce(maxNormalForce);
176 }
177 2 static void setReference(Contact6d &self, const pinocchio::SE3 &ref) {
178 2 self.setReference(ref);
179 2 }
180 ✗ static void setForceReference(Contact6d &self,
181 const ::Eigen::VectorXd f_ref) {
182 ✗ self.setForceReference(f_ref);
183 }
184 ✗ static void setRegularizationTaskWeightVector(Contact6d &self,
185 const ::Eigen::VectorXd w) {
186 ✗ self.setRegularizationTaskWeightVector(w);
187 }
188 10 static double getNormalForce(Contact6d &self, Eigen::VectorXd f) {
189
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 return self.getNormalForce(f);
190 }
191
192 8 static void expose(const std::string &class_name) {
193
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 std::string doc = "Contact6d info.";
194
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 8 bp::class_<Contact6d>(class_name.c_str(), doc.c_str(), bp::no_init)
195
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 .def(Contact6DPythonVisitor<Contact6d>());
196 8 }
197 };
198 } // namespace python
199 } // namespace tsid
200
201 #endif // ifndef __tsid_python_contact_hpp__
202