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_task_am_hpp__ |
19 |
|
|
#define __tsid_python_task_am_hpp__ |
20 |
|
|
|
21 |
|
|
#include "tsid/bindings/python/fwd.hpp" |
22 |
|
|
|
23 |
|
|
#include "tsid/tasks/task-angular-momentum-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 TaskAM> |
33 |
|
|
struct TaskAMEqualityPythonVisitor |
34 |
|
|
: public boost::python::def_visitor<TaskAMEqualityPythonVisitor<TaskAM> > { |
35 |
|
|
template <class PyClass> |
36 |
|
|
|
37 |
|
7 |
void visit(PyClass& cl) const { |
38 |
|
7 |
cl.def(bp::init<std::string, robots::RobotWrapper&>( |
39 |
|
|
(bp::arg("name"), bp::arg("robot")), "Default Constructor")) |
40 |
✓✗✓✗ ✓✗✓✗
|
14 |
.add_property("dim", &TaskAM::dim, "return dimension size") |
41 |
|
14 |
.def("setReference", &TaskAMEqualityPythonVisitor::setReference, |
42 |
|
|
bp::arg("ref")) |
43 |
✓✗✓✗ ✓✗ |
14 |
.add_property( |
44 |
|
|
"getDesiredMomentumDerivative", |
45 |
|
|
bp::make_function( |
46 |
|
|
&TaskAMEqualityPythonVisitor::getDesiredMomentumDerivative, |
47 |
|
|
bp::return_value_policy<bp::copy_const_reference>()), |
48 |
|
|
"Return dL_desired") |
49 |
✓✗✓✗
|
14 |
.def("getdMomentum", &TaskAMEqualityPythonVisitor::getdMomentum, |
50 |
|
|
bp::arg("dv")) |
51 |
✓✗✓✗
|
14 |
.add_property("momentum_error", |
52 |
|
|
bp::make_function( |
53 |
|
|
&TaskAMEqualityPythonVisitor::momentum_error, |
54 |
|
|
bp::return_value_policy<bp::copy_const_reference>())) |
55 |
✓✗✓✗
|
14 |
.add_property("momentum", |
56 |
|
|
bp::make_function( |
57 |
|
|
&TaskAMEqualityPythonVisitor::momentum, |
58 |
|
|
bp::return_value_policy<bp::copy_const_reference>())) |
59 |
✓✗✓✗
|
14 |
.add_property("momentum_ref", |
60 |
|
|
bp::make_function( |
61 |
|
|
&TaskAMEqualityPythonVisitor::momentum_ref, |
62 |
|
|
bp::return_value_policy<bp::copy_const_reference>())) |
63 |
✓✗✓✗
|
14 |
.add_property("dmomentum_ref", |
64 |
|
|
bp::make_function( |
65 |
|
|
&TaskAMEqualityPythonVisitor::dmomentum_ref, |
66 |
|
|
bp::return_value_policy<bp::copy_const_reference>())) |
67 |
✓✗✓✗
|
14 |
.add_property("Kp", |
68 |
|
|
bp::make_function( |
69 |
|
|
&TaskAMEqualityPythonVisitor::Kp, |
70 |
|
|
bp::return_value_policy<bp::copy_const_reference>())) |
71 |
✓✗✓✗
|
14 |
.add_property("Kd", |
72 |
|
|
bp::make_function( |
73 |
|
|
&TaskAMEqualityPythonVisitor::Kd, |
74 |
|
|
bp::return_value_policy<bp::copy_const_reference>())) |
75 |
✓✗✓✗
|
14 |
.def("setKp", &TaskAMEqualityPythonVisitor::setKp, bp::arg("Kp")) |
76 |
✓✗✓✗
|
14 |
.def("setKd", &TaskAMEqualityPythonVisitor::setKd, bp::arg("Kd")) |
77 |
✓✗✓✗
|
14 |
.def("compute", &TaskAMEqualityPythonVisitor::compute, |
78 |
|
|
bp::args("t", "q", "v", "data")) |
79 |
✓✗✓✗
|
14 |
.def("getConstraint", &TaskAMEqualityPythonVisitor::getConstraint) |
80 |
✓✗✓✗
|
7 |
.add_property("name", &TaskAMEqualityPythonVisitor::name); |
81 |
|
7 |
} |
82 |
|
|
static std::string name(TaskAM& self) { |
83 |
|
|
std::string name = self.name(); |
84 |
|
|
return name; |
85 |
|
|
} |
86 |
|
1000 |
static math::ConstraintEquality compute(TaskAM& self, const double t, |
87 |
|
|
const Eigen::VectorXd& q, |
88 |
|
|
const Eigen::VectorXd& v, |
89 |
|
|
pinocchio::Data& data) { |
90 |
✓✗✓✗
|
1000 |
self.compute(t, q, v, data); |
91 |
✓✗✓✗ ✓✗✓✗
|
3000 |
math::ConstraintEquality cons(self.getConstraint().name(), |
92 |
✓✗✓✗
|
1000 |
self.getConstraint().matrix(), |
93 |
|
1000 |
self.getConstraint().vector()); |
94 |
|
1000 |
return cons; |
95 |
|
|
} |
96 |
|
|
static math::ConstraintEquality getConstraint(const TaskAM& self) { |
97 |
|
|
math::ConstraintEquality cons(self.getConstraint().name(), |
98 |
|
|
self.getConstraint().matrix(), |
99 |
|
|
self.getConstraint().vector()); |
100 |
|
|
return cons; |
101 |
|
|
} |
102 |
|
1000 |
static void setReference(TaskAM& self, |
103 |
|
|
const trajectories::TrajectorySample& ref) { |
104 |
|
1000 |
self.setReference(ref); |
105 |
|
1000 |
} |
106 |
|
|
static const Eigen::Vector3d& getDesiredMomentumDerivative( |
107 |
|
|
const TaskAM& self) { |
108 |
|
|
return self.getDesiredMomentumDerivative(); |
109 |
|
|
} |
110 |
|
|
static Eigen::Vector3d getdMomentum(TaskAM& self, const Eigen::VectorXd dv) { |
111 |
|
|
return self.getdMomentum(dv); |
112 |
|
|
} |
113 |
|
1000 |
static const Eigen::Vector3d& momentum_error(const TaskAM& self) { |
114 |
|
1000 |
return self.momentum_error(); |
115 |
|
|
} |
116 |
|
|
static const Eigen::Vector3d& momentum(const TaskAM& self) { |
117 |
|
|
return self.momentum(); |
118 |
|
|
} |
119 |
|
|
static const Eigen::VectorXd& momentum_ref(const TaskAM& self) { |
120 |
|
|
return self.momentum_ref(); |
121 |
|
|
} |
122 |
|
|
static const Eigen::VectorXd& dmomentum_ref(const TaskAM& self) { |
123 |
|
|
return self.dmomentum_ref(); |
124 |
|
|
} |
125 |
|
1 |
static const Eigen::Vector3d& Kp(TaskAM& self) { return self.Kp(); } |
126 |
|
1 |
static const Eigen::Vector3d& Kd(TaskAM& self) { return self.Kd(); } |
127 |
|
1 |
static void setKp(TaskAM& self, const ::Eigen::VectorXd Kp) { |
128 |
✓✗ |
1 |
return self.Kp(Kp); |
129 |
|
|
} |
130 |
|
1 |
static void setKd(TaskAM& self, const ::Eigen::VectorXd Kv) { |
131 |
✓✗ |
1 |
return self.Kd(Kv); |
132 |
|
|
} |
133 |
|
7 |
static void expose(const std::string& class_name) { |
134 |
✓✗ |
7 |
std::string doc = "TaskAMEqualityPythonVisitor info."; |
135 |
✓✗✓✗
|
7 |
bp::class_<TaskAM>(class_name.c_str(), doc.c_str(), bp::no_init) |
136 |
|
|
.def(TaskAMEqualityPythonVisitor<TaskAM>()); |
137 |
|
7 |
} |
138 |
|
|
}; |
139 |
|
|
} // namespace python |
140 |
|
|
} // namespace tsid |
141 |
|
|
|
142 |
|
|
#endif // ifndef __tsid_python_task_am_hpp__ |