1 |
|
|
// |
2 |
|
|
// Copyright (c) 2021 LAAS-CNRS, University of Trento |
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_task_contact_force_equality_hpp__ |
19 |
|
|
#define __tsid_python_task_contact_force_equality_hpp__ |
20 |
|
|
|
21 |
|
|
#include "tsid/bindings/python/fwd.hpp" |
22 |
|
|
|
23 |
|
|
#include "tsid/tasks/task-contact-force-equality.hpp" |
24 |
|
|
#include "tsid/robots/robot-wrapper.hpp" |
25 |
|
|
#include "tsid/trajectories/trajectory-base.hpp" |
26 |
|
|
#include "tsid/math/constraint-equality.hpp" |
27 |
|
|
#include "tsid/math/constraint-base.hpp" |
28 |
|
|
namespace tsid { |
29 |
|
|
namespace python { |
30 |
|
|
namespace bp = boost::python; |
31 |
|
|
|
32 |
|
|
template <typename TaskContactForceEquality> |
33 |
|
|
struct TaskContactForceEqualityPythonVisitor |
34 |
|
|
: public boost::python::def_visitor< |
35 |
|
|
TaskContactForceEqualityPythonVisitor<TaskContactForceEquality> > { |
36 |
|
|
template <class PyClass> |
37 |
|
|
|
38 |
|
|
void visit(PyClass &cl) const { |
39 |
|
|
cl.def( |
40 |
|
|
bp::init<std::string, robots::RobotWrapper &, double, |
41 |
|
|
contacts::ContactBase &>((bp::arg("name"), bp::arg("robot"), |
42 |
|
|
bp::arg("dt"), bp::arg("contact")), |
43 |
|
|
"Default Constructor")) |
44 |
|
|
.add_property("dim", &TaskContactForceEquality::dim, |
45 |
|
|
"return dimension size") |
46 |
|
|
.def("setReference", |
47 |
|
|
&TaskContactForceEqualityPythonVisitor::setReference, |
48 |
|
|
bp::arg("ref")) |
49 |
|
|
.def("setExternalForce", |
50 |
|
|
&TaskContactForceEqualityPythonVisitor::setExternalForce, |
51 |
|
|
bp::arg("f_ext")) |
52 |
|
|
.def("compute", &TaskContactForceEqualityPythonVisitor::compute, |
53 |
|
|
bp::args("t", "q", "v", "data")) |
54 |
|
|
.def("getConstraint", |
55 |
|
|
&TaskContactForceEqualityPythonVisitor::getConstraint) |
56 |
|
|
.add_property("name", &TaskContactForceEqualityPythonVisitor::name) |
57 |
|
|
.add_property("Kp", |
58 |
|
|
bp::make_function( |
59 |
|
|
&TaskContactForceEqualityPythonVisitor::Kp, |
60 |
|
|
bp::return_value_policy<bp::copy_const_reference>())) |
61 |
|
|
.add_property("Kd", |
62 |
|
|
bp::make_function( |
63 |
|
|
&TaskContactForceEqualityPythonVisitor::Kd, |
64 |
|
|
bp::return_value_policy<bp::copy_const_reference>())) |
65 |
|
|
.add_property("Ki", |
66 |
|
|
bp::make_function( |
67 |
|
|
&TaskContactForceEqualityPythonVisitor::Kd, |
68 |
|
|
bp::return_value_policy<bp::copy_const_reference>())) |
69 |
|
|
.add_property("getLeakRate", |
70 |
|
|
bp::make_function( |
71 |
|
|
&TaskContactForceEqualityPythonVisitor::getLeakRate, |
72 |
|
|
bp::return_value_policy<bp::copy_const_reference>())) |
73 |
|
|
.def("setKp", &TaskContactForceEqualityPythonVisitor::setKp, |
74 |
|
|
bp::arg("Kp")) |
75 |
|
|
.def("setKd", &TaskContactForceEqualityPythonVisitor::setKd, |
76 |
|
|
bp::arg("Kd")) |
77 |
|
|
.def("setKi", &TaskContactForceEqualityPythonVisitor::setKi, |
78 |
|
|
bp::arg("Ki")) |
79 |
|
|
.def("setLeakRate", &TaskContactForceEqualityPythonVisitor::setLeakRate, |
80 |
|
|
bp::arg("leak")); |
81 |
|
|
} |
82 |
|
|
static std::string name(TaskContactForceEquality &self) { |
83 |
|
|
std::string name = self.name(); |
84 |
|
|
return name; |
85 |
|
|
} |
86 |
|
|
static math::ConstraintEquality compute(TaskContactForceEquality &self, |
87 |
|
|
const double t, |
88 |
|
|
const Eigen::VectorXd &q, |
89 |
|
|
const Eigen::VectorXd &v, |
90 |
|
|
pinocchio::Data &data) { |
91 |
|
|
self.compute(t, q, v, data); |
92 |
|
|
math::ConstraintEquality cons(self.getConstraint().name(), |
93 |
|
|
self.getConstraint().matrix(), |
94 |
|
|
self.getConstraint().vector()); |
95 |
|
|
return cons; |
96 |
|
|
} |
97 |
|
|
static math::ConstraintEquality getConstraint( |
98 |
|
|
const TaskContactForceEquality &self) { |
99 |
|
|
math::ConstraintEquality cons(self.getConstraint().name(), |
100 |
|
|
self.getConstraint().matrix(), |
101 |
|
|
self.getConstraint().vector()); |
102 |
|
|
return cons; |
103 |
|
|
} |
104 |
|
|
static void setReference(TaskContactForceEquality &self, |
105 |
|
|
trajectories::TrajectorySample &ref) { |
106 |
|
|
self.setReference(ref); |
107 |
|
|
} |
108 |
|
|
static void setExternalForce(TaskContactForceEquality &self, |
109 |
|
|
trajectories::TrajectorySample &f_ext) { |
110 |
|
|
self.setExternalForce(f_ext); |
111 |
|
|
} |
112 |
|
|
static const Eigen::VectorXd &Kp(TaskContactForceEquality &self) { |
113 |
|
|
return self.Kp(); |
114 |
|
|
} |
115 |
|
|
static const Eigen::VectorXd &Kd(TaskContactForceEquality &self) { |
116 |
|
|
return self.Kd(); |
117 |
|
|
} |
118 |
|
|
static const Eigen::VectorXd &Ki(TaskContactForceEquality &self) { |
119 |
|
|
return self.Ki(); |
120 |
|
|
} |
121 |
|
|
static const double &getLeakRate(TaskContactForceEquality &self) { |
122 |
|
|
return self.getLeakRate(); |
123 |
|
|
} |
124 |
|
|
static void setKp(TaskContactForceEquality &self, |
125 |
|
|
const ::Eigen::VectorXd Kp) { |
126 |
|
|
return self.Kp(Kp); |
127 |
|
|
} |
128 |
|
|
static void setKd(TaskContactForceEquality &self, |
129 |
|
|
const ::Eigen::VectorXd Kd) { |
130 |
|
|
return self.Kd(Kd); |
131 |
|
|
} |
132 |
|
|
static void setKi(TaskContactForceEquality &self, |
133 |
|
|
const ::Eigen::VectorXd Ki) { |
134 |
|
|
return self.Ki(Ki); |
135 |
|
|
} |
136 |
|
|
static void setLeakRate(TaskContactForceEquality &self, const double leak) { |
137 |
|
|
return self.setLeakRate(leak); |
138 |
|
|
} |
139 |
|
|
static void expose(const std::string &class_name) { |
140 |
|
|
std::string doc = "TaskContactForceEqualityPythonVisitor info."; |
141 |
|
|
bp::class_<TaskContactForceEquality>(class_name.c_str(), doc.c_str(), |
142 |
|
|
bp::no_init) |
143 |
|
|
.def(TaskContactForceEqualityPythonVisitor<TaskContactForceEquality>()); |
144 |
|
|
} |
145 |
|
|
}; |
146 |
|
|
} // namespace python |
147 |
|
|
} // namespace tsid |
148 |
|
|
|
149 |
|
|
#endif // ifndef __tsid_python_task_contact_force_equality_hpp__ |