GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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_contact_6d_hpp__ |
||
19 |
#define __tsid_python_contact_6d_hpp__ |
||
20 |
|||
21 |
#include "tsid/bindings/python/fwd.hpp" |
||
22 |
|||
23 |
#include "tsid/contacts/contact-point.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 |
|||
29 |
namespace tsid { |
||
30 |
namespace python { |
||
31 |
namespace bp = boost::python; |
||
32 |
|||
33 |
template <typename ContactPoint> |
||
34 |
struct ContactPointPythonVisitor |
||
35 |
: public boost::python::def_visitor< |
||
36 |
ContactPointPythonVisitor<ContactPoint> > { |
||
37 |
template <class PyClass> |
||
38 |
|||
39 |
7 |
void visit(PyClass& cl) const { |
|
40 |
✓✗ | 7 |
cl.def(bp::init<std::string, robots::RobotWrapper&, std::string, |
41 |
Eigen::VectorXd, double, double, double>( |
||
42 |
(bp::arg("name"), bp::arg("robot"), bp::arg("framename"), |
||
43 |
bp::arg("contactNormal"), bp::arg("frictionCoeff"), |
||
44 |
bp::arg("minForce"), bp::arg("maxForce")), |
||
45 |
"Default Constructor")) |
||
46 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
14 |
.add_property("n_motion", &ContactPoint::n_motion, |
47 |
"return number of motion") |
||
48 |
14 |
.add_property("n_force", &ContactPoint::n_force, |
|
49 |
"return number of force") |
||
50 |
✓✗ | 7 |
.add_property("name", &ContactPointPythonVisitor::name, "return name") |
51 |
✓✗✓✗ |
7 |
.def("computeMotionTask", &ContactPointPythonVisitor::computeMotionTask, |
52 |
bp::args("t", "q", "v", "data")) |
||
53 |
✓✗✓✗ |
14 |
.def("computeForceTask", &ContactPointPythonVisitor::computeForceTask, |
54 |
bp::args("t", "q", "v", "data")) |
||
55 |
✓✗✓✗ |
14 |
.def("computeForceRegularizationTask", |
56 |
&ContactPointPythonVisitor::computeForceRegularizationTask, |
||
57 |
bp::args("t", "q", "v", "data")) |
||
58 |
|||
59 |
✓✗✓✗ |
14 |
.add_property("getForceGeneratorMatrix", |
60 |
bp::make_function( |
||
61 |
&ContactPointPythonVisitor::getForceGeneratorMatrix, |
||
62 |
bp::return_value_policy<bp::copy_const_reference>())) |
||
63 |
|||
64 |
✓✗✓✗ |
14 |
.def("getNormalForce", &ContactPointPythonVisitor::getNormalForce, |
65 |
bp::arg("vec")) |
||
66 |
✓✗✓✗ |
14 |
.add_property("getMinNormalForce", &ContactPoint::getMinNormalForce) |
67 |
14 |
.add_property("getMaxNormalForce", &ContactPoint::getMaxNormalForce) |
|
68 |
|||
69 |
✓✗ | 7 |
.add_property("Kp", |
70 |
bp::make_function( |
||
71 |
&ContactPointPythonVisitor::Kp, |
||
72 |
bp::return_value_policy<bp::copy_const_reference>())) |
||
73 |
✓✗✓✗ ✓✗ |
14 |
.add_property("Kd", |
74 |
bp::make_function( |
||
75 |
&ContactPointPythonVisitor::Kd, |
||
76 |
bp::return_value_policy<bp::copy_const_reference>())) |
||
77 |
✓✗✓✗ |
14 |
.def("setKp", &ContactPointPythonVisitor::setKp, bp::arg("Kp")) |
78 |
✓✗✓✗ |
14 |
.def("setKd", &ContactPointPythonVisitor::setKd, bp::arg("Kd")) |
79 |
|||
80 |
✓✗✓✗ |
14 |
.def("useLocalFrame", &ContactPointPythonVisitor::useLocalFrame, |
81 |
bp::arg("local_frame")) |
||
82 |
|||
83 |
✓✗✓✗ |
14 |
.def("setContactNormal", &ContactPointPythonVisitor::setContactNormal, |
84 |
bp::args("vec")) |
||
85 |
✓✗✓✗ |
14 |
.def("setFrictionCoefficient", |
86 |
&ContactPointPythonVisitor::setFrictionCoefficient, |
||
87 |
bp::args("friction_coeff")) |
||
88 |
✓✗✓✗ |
14 |
.def("setMinNormalForce", &ContactPointPythonVisitor::setMinNormalForce, |
89 |
bp::args("min_force")) |
||
90 |
✓✗✓✗ |
14 |
.def("setMaxNormalForce", &ContactPointPythonVisitor::setMaxNormalForce, |
91 |
bp::args("max_force")) |
||
92 |
✓✗✓✗ |
14 |
.def("setReference", &ContactPointPythonVisitor::setReference, |
93 |
bp::args("SE3")) |
||
94 |
✓✗✓✗ |
14 |
.def("setForceReference", &ContactPointPythonVisitor::setForceReference, |
95 |
bp::args("f_vec")) |
||
96 |
✓✗✓✗ ✓✗ |
14 |
.def("setRegularizationTaskWeightVector", |
97 |
&ContactPointPythonVisitor::setRegularizationTaskWeightVector, |
||
98 |
bp::args("w_vec")); |
||
99 |
7 |
} |
|
100 |
static std::string name(ContactPoint& self) { |
||
101 |
std::string name = self.name(); |
||
102 |
return name; |
||
103 |
} |
||
104 |
|||
105 |
static math::ConstraintEquality computeMotionTask(ContactPoint& self, |
||
106 |
const double t, |
||
107 |
const Eigen::VectorXd& q, |
||
108 |
const Eigen::VectorXd& v, |
||
109 |
pinocchio::Data& data) { |
||
110 |
self.computeMotionTask(t, q, v, data); |
||
111 |
math::ConstraintEquality cons(self.getMotionConstraint().name(), |
||
112 |
self.getMotionConstraint().matrix(), |
||
113 |
self.getMotionConstraint().vector()); |
||
114 |
return cons; |
||
115 |
} |
||
116 |
static math::ConstraintInequality computeForceTask( |
||
117 |
ContactPoint& self, const double t, const Eigen::VectorXd& q, |
||
118 |
const Eigen::VectorXd& v, const pinocchio::Data& data) { |
||
119 |
self.computeForceTask(t, q, v, data); |
||
120 |
math::ConstraintInequality cons(self.getForceConstraint().name(), |
||
121 |
self.getForceConstraint().matrix(), |
||
122 |
self.getForceConstraint().lowerBound(), |
||
123 |
self.getForceConstraint().upperBound()); |
||
124 |
return cons; |
||
125 |
} |
||
126 |
static math::ConstraintEquality computeForceRegularizationTask( |
||
127 |
ContactPoint& self, const double t, const Eigen::VectorXd& q, |
||
128 |
const Eigen::VectorXd& v, const pinocchio::Data& data) { |
||
129 |
self.computeForceRegularizationTask(t, q, v, data); |
||
130 |
math::ConstraintEquality cons(self.getForceRegularizationTask().name(), |
||
131 |
self.getForceRegularizationTask().matrix(), |
||
132 |
self.getForceRegularizationTask().vector()); |
||
133 |
return cons; |
||
134 |
} |
||
135 |
|||
136 |
static void useLocalFrame(ContactPoint& self, const bool local_frame) { |
||
137 |
self.useLocalFrame(local_frame); |
||
138 |
} |
||
139 |
static const Eigen::MatrixXd& getForceGeneratorMatrix(ContactPoint& self) { |
||
140 |
return self.getForceGeneratorMatrix(); |
||
141 |
} |
||
142 |
static const Eigen::VectorXd& Kp(ContactPoint& self) { return self.Kp(); } |
||
143 |
static const Eigen::VectorXd& Kd(ContactPoint& self) { return self.Kd(); } |
||
144 |
static void setKp(ContactPoint& self, const ::Eigen::VectorXd Kp) { |
||
145 |
return self.Kp(Kp); |
||
146 |
} |
||
147 |
static void setKd(ContactPoint& self, const ::Eigen::VectorXd Kd) { |
||
148 |
return self.Kd(Kd); |
||
149 |
} |
||
150 |
static bool setContactPoints(ContactPoint& self, |
||
151 |
const ::Eigen::MatrixXd contactpoints) { |
||
152 |
return self.setContactPoints(contactpoints); |
||
153 |
} |
||
154 |
static bool setContactNormal(ContactPoint& self, |
||
155 |
const ::Eigen::VectorXd contactNormal) { |
||
156 |
return self.setContactNormal(contactNormal); |
||
157 |
} |
||
158 |
static bool setFrictionCoefficient(ContactPoint& self, |
||
159 |
const double frictionCoefficient) { |
||
160 |
return self.setFrictionCoefficient(frictionCoefficient); |
||
161 |
} |
||
162 |
static bool setMinNormalForce(ContactPoint& self, |
||
163 |
const double minNormalForce) { |
||
164 |
return self.setMinNormalForce(minNormalForce); |
||
165 |
} |
||
166 |
static bool setMaxNormalForce(ContactPoint& self, |
||
167 |
const double maxNormalForce) { |
||
168 |
return self.setMaxNormalForce(maxNormalForce); |
||
169 |
} |
||
170 |
static void setReference(ContactPoint& self, const pinocchio::SE3& ref) { |
||
171 |
self.setReference(ref); |
||
172 |
} |
||
173 |
static void setForceReference(ContactPoint& self, |
||
174 |
const ::Eigen::VectorXd f_ref) { |
||
175 |
self.setForceReference(f_ref); |
||
176 |
} |
||
177 |
static void setRegularizationTaskWeightVector(ContactPoint& self, |
||
178 |
const ::Eigen::VectorXd w) { |
||
179 |
self.setRegularizationTaskWeightVector(w); |
||
180 |
} |
||
181 |
static double getNormalForce(ContactPoint& self, Eigen::VectorXd f) { |
||
182 |
return self.getNormalForce(f); |
||
183 |
} |
||
184 |
|||
185 |
7 |
static void expose(const std::string& class_name) { |
|
186 |
✓✗ | 7 |
std::string doc = "ContactPoint info."; |
187 |
✓✗✓✗ |
7 |
bp::class_<ContactPoint>(class_name.c_str(), doc.c_str(), bp::no_init) |
188 |
.def(ContactPointPythonVisitor<ContactPoint>()); |
||
189 |
7 |
} |
|
190 |
}; |
||
191 |
} // namespace python |
||
192 |
} // namespace tsid |
||
193 |
|||
194 |
#endif // ifndef __tsid_python_contact_hpp__ |
Generated by: GCOVR (Version 4.2) |