GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/tsid/bindings/python/formulations/formulation.hpp Lines: 63 93 67.7 %
Date: 2023-11-10 01:09:11 Branches: 69 136 50.7 %

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_HQPOutput_hpp__
19
#define __tsid_python_HQPOutput_hpp__
20
21
#include "tsid/bindings/python/fwd.hpp"
22
23
#include <pinocchio/bindings/python/utils/deprecation.hpp>
24
25
#include "tsid/formulations/inverse-dynamics-formulation-acc-force.hpp"
26
#include "tsid/bindings/python/solvers/HQPData.hpp"
27
#include "tsid/contacts/contact-6d.hpp"
28
#include "tsid/contacts/contact-point.hpp"
29
#include "tsid/tasks/task-joint-posture.hpp"
30
#include "tsid/tasks/task-se3-equality.hpp"
31
#include "tsid/tasks/task-com-equality.hpp"
32
#include "tsid/tasks/task-cop-equality.hpp"
33
#include "tsid/tasks/task-actuation-bounds.hpp"
34
#include "tsid/tasks/task-joint-bounds.hpp"
35
#include "tsid/tasks/task-joint-posVelAcc-bounds.hpp"
36
#include "tsid/tasks/task-angular-momentum-equality.hpp"
37
38
namespace tsid {
39
namespace python {
40
namespace bp = boost::python;
41
42
template <typename T>
43
struct InvDynPythonVisitor
44
    : public boost::python::def_visitor<InvDynPythonVisitor<T> > {
45
  template <class PyClass>
46
47
7
  void visit(PyClass& cl) const {
48
7
    cl.def(bp::init<std::string, robots::RobotWrapper&, bool>(
49
               (bp::args("name", "robot", "verbose"))))
50

7
        .def("data", &InvDynPythonVisitor::data)
51
7
        .add_property("nVar", &T::nVar)
52
14
        .add_property("nEq", &T::nEq)
53
7
        .add_property("nIn", &T::nIn)
54
55
7
        .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_SE3,
56
             bp::args("task", "weight", "priorityLevel", "transition duration"))
57

14
        .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_COM,
58
             bp::args("task", "weight", "priorityLevel", "transition duration"))
59

14
        .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_Joint,
60
             bp::args("task", "weight", "priorityLevel", "transition duration"))
61

14
        .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_JointBounds,
62
             bp::args("task", "weight", "priorityLevel", "transition duration"))
63

14
        .def("addMotionTask",
64
             &InvDynPythonVisitor::addMotionTask_JointPosVelAccBounds,
65
             bp::args("task", "weight", "priorityLevel", "transition duration"))
66

14
        .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_AM,
67
             bp::args("task", "weight", "priorityLevel", "transition duration"))
68

14
        .def("addForceTask", &InvDynPythonVisitor::addForceTask_COP,
69
             bp::args("task", "weight", "priorityLevel", "transition duration"))
70

14
        .def("addActuationTask", &InvDynPythonVisitor::addActuationTask_Bounds,
71
             bp::args("task", "weight", "priorityLevel", "transition duration"))
72

14
        .def("updateTaskWeight", &InvDynPythonVisitor::updateTaskWeight,
73
             bp::args("task_name", "weight"))
74

14
        .def("updateRigidContactWeights",
75
             &InvDynPythonVisitor::updateRigidContactWeights,
76
             bp::args("contact_name", "force_regularization_weight"))
77

14
        .def("updateRigidContactWeights",
78
             &InvDynPythonVisitor::updateRigidContactWeightsWithMotionWeight,
79
             bp::args("contact_name", "force_regularization_weight",
80
                      "motion_weight"))
81

14
        .def("addRigidContact",
82
             &InvDynPythonVisitor::addRigidContact6dDeprecated,
83
             bp::args("contact"),
84
             pinocchio::python::deprecated_function<>(
85
                 "Method addRigidContact(ContactBase) is deprecated. You "
86
                 "should use addRigidContact(ContactBase, double) instead"))
87


14
        .def("addRigidContact", &InvDynPythonVisitor::addRigidContact6d,
88
             bp::args("contact", "force_reg_weight"))
89

14
        .def("addRigidContact",
90
             &InvDynPythonVisitor::addRigidContact6dWithPriorityLevel,
91
             bp::args("contact", "force_reg_weight", "motion_weight",
92
                      "priority_level"))
93

14
        .def("addRigidContact", &InvDynPythonVisitor::addRigidContactPoint,
94
             bp::args("contact", "force_reg_weight"))
95

14
        .def("addRigidContact",
96
             &InvDynPythonVisitor::addRigidContactPointWithPriorityLevel,
97
             bp::args("contact", "force_reg_weight", "motion_weight",
98
                      "priority_level"))
99

14
        .def("removeTask", &InvDynPythonVisitor::removeTask,
100
             bp::args("task_name", "duration"))
101

14
        .def("removeRigidContact", &InvDynPythonVisitor::removeRigidContact,
102
             bp::args("contact_name", "duration"))
103

14
        .def("removeFromHqpData", &InvDynPythonVisitor::removeFromHqpData,
104
             bp::args("constraint_name"))
105

14
        .def("computeProblemData", &InvDynPythonVisitor::computeProblemData,
106
             bp::args("time", "q", "v"))
107
108

14
        .def("getActuatorForces", &InvDynPythonVisitor::getActuatorForces,
109
             bp::args("HQPOutput"))
110

14
        .def("getAccelerations", &InvDynPythonVisitor::getAccelerations,
111
             bp::args("HQPOutput"))
112

14
        .def("getContactForces", &InvDynPythonVisitor::getContactForces,
113
             bp::args("HQPOutput"))
114

14
        .def("checkContact", &InvDynPythonVisitor::checkContact,
115
             bp::args("name", "HQPOutput"))
116

14
        .def("getContactForce", &InvDynPythonVisitor::getContactForce,
117
             bp::args("name", "HQPOutput"));
118
7
  }
119
2
  static pinocchio::Data data(T& self) {
120
2
    pinocchio::Data data = self.data();
121
2
    return data;
122
  }
123
1
  static bool addMotionTask_SE3(T& self, tasks::TaskSE3Equality& task,
124
                                double weight, unsigned int priorityLevel,
125
                                double transition_duration) {
126
1
    return self.addMotionTask(task, weight, priorityLevel, transition_duration);
127
  }
128
1
  static bool addMotionTask_COM(T& self, tasks::TaskComEquality& task,
129
                                double weight, unsigned int priorityLevel,
130
                                double transition_duration) {
131
1
    return self.addMotionTask(task, weight, priorityLevel, transition_duration);
132
  }
133
1
  static bool addMotionTask_Joint(T& self, tasks::TaskJointPosture& task,
134
                                  double weight, unsigned int priorityLevel,
135
                                  double transition_duration) {
136
1
    return self.addMotionTask(task, weight, priorityLevel, transition_duration);
137
  }
138
  static bool addMotionTask_JointBounds(T& self, tasks::TaskJointBounds& task,
139
                                        double weight,
140
                                        unsigned int priorityLevel,
141
                                        double transition_duration) {
142
    return self.addMotionTask(task, weight, priorityLevel, transition_duration);
143
  }
144
  static bool addMotionTask_JointPosVelAccBounds(
145
      T& self, tasks::TaskJointPosVelAccBounds& task, double weight,
146
      unsigned int priorityLevel, double transition_duration) {
147
    return self.addMotionTask(task, weight, priorityLevel, transition_duration);
148
  }
149
  static bool addMotionTask_AM(T& self, tasks::TaskAMEquality& task,
150
                               double weight, unsigned int priorityLevel,
151
                               double transition_duration) {
152
    return self.addMotionTask(task, weight, priorityLevel, transition_duration);
153
  }
154
  static bool addForceTask_COP(T& self, tasks::TaskCopEquality& task,
155
                               double weight, unsigned int priorityLevel,
156
                               double transition_duration) {
157
    return self.addForceTask(task, weight, priorityLevel, transition_duration);
158
  }
159
  static bool addActuationTask_Bounds(T& self, tasks::TaskActuationBounds& task,
160
                                      double weight, unsigned int priorityLevel,
161
                                      double transition_duration) {
162
    return self.addActuationTask(task, weight, priorityLevel,
163
                                 transition_duration);
164
  }
165
  static bool updateTaskWeight(T& self, const std::string& task_name,
166
                               double weight) {
167
    return self.updateTaskWeight(task_name, weight);
168
  }
169
  static bool updateRigidContactWeights(T& self,
170
                                        const std::string& contact_name,
171
                                        double force_regularization_weight) {
172
    return self.updateRigidContactWeights(contact_name,
173
                                          force_regularization_weight);
174
  }
175
  static bool updateRigidContactWeightsWithMotionWeight(
176
      T& self, const std::string& contact_name,
177
      double force_regularization_weight, double motion_weight) {
178
    return self.updateRigidContactWeights(
179
        contact_name, force_regularization_weight, motion_weight);
180
  }
181
2
  static bool addRigidContact6dDeprecated(T& self,
182
                                          contacts::Contact6d& contact) {
183
2
    return self.addRigidContact(contact, 1e-5);
184
  }
185
  static bool addRigidContact6d(T& self, contacts::Contact6d& contact,
186
                                double force_regularization_weight) {
187
    return self.addRigidContact(contact, force_regularization_weight);
188
  }
189
  static bool addRigidContact6dWithPriorityLevel(
190
      T& self, contacts::Contact6d& contact, double force_regularization_weight,
191
      double motion_weight, const bool priority_level) {
192
    return self.addRigidContact(contact, force_regularization_weight,
193
                                motion_weight, priority_level);
194
  }
195
  static bool addRigidContactPoint(T& self, contacts::ContactPoint& contact,
196
                                   double force_regularization_weight) {
197
    return self.addRigidContact(contact, force_regularization_weight);
198
  }
199
  static bool addRigidContactPointWithPriorityLevel(
200
      T& self, contacts::ContactPoint& contact,
201
      double force_regularization_weight, double motion_weight,
202
      const bool priority_level) {
203
    return self.addRigidContact(contact, force_regularization_weight,
204
                                motion_weight, priority_level);
205
  }
206
  static bool removeTask(T& self, const std::string& task_name,
207
                         double transition_duration) {
208
    return self.removeTask(task_name, transition_duration);
209
  }
210
1
  static bool removeRigidContact(T& self, const std::string& contactName,
211
                                 double transition_duration) {
212
1
    return self.removeRigidContact(contactName, transition_duration);
213
  }
214
  static bool removeFromHqpData(T& self, const std::string& constraintName) {
215
    return self.removeFromHqpData(constraintName);
216
  }
217
1001
  static HQPDatas computeProblemData(T& self, double time,
218
                                     const Eigen::VectorXd& q,
219
                                     const Eigen::VectorXd& v) {
220
1001
    HQPDatas Hqp;
221


1001
    Hqp.set(self.computeProblemData(time, q, v));
222
1001
    return Hqp;
223
  }
224
1000
  static Eigen::VectorXd getActuatorForces(T& self,
225
                                           const solvers::HQPOutput& sol) {
226
1000
    return self.getActuatorForces(sol);
227
  }
228
1000
  static Eigen::VectorXd getAccelerations(T& self,
229
                                          const solvers::HQPOutput& sol) {
230
1000
    return self.getAccelerations(sol);
231
  }
232
  static Eigen::VectorXd getContactForces(T& self,
233
                                          const solvers::HQPOutput& sol) {
234
    return self.getContactForces(sol);
235
  }
236
18
  static bool checkContact(T& self, const std::string& name,
237
                           const solvers::HQPOutput& sol) {
238
36
    tsid::math::Vector f = self.getContactForces(name, sol);
239

18
    if (f.size() > 0) return true;
240
8
    return false;
241
  }
242
10
  static Eigen::VectorXd getContactForce(T& self, const std::string& name,
243
                                         const solvers::HQPOutput& sol) {
244
10
    return self.getContactForces(name, sol);
245
  }
246
247
7
  static void expose(const std::string& class_name) {
248
7
    std::string doc = "InvDyn info.";
249

7
    bp::class_<T>(class_name.c_str(), doc.c_str(), bp::no_init)
250
        .def(InvDynPythonVisitor<T>());
251
7
  }
252
};
253
}  // namespace python
254
}  // namespace tsid
255
256
#endif  // ifndef __tsid_python_HQPOutput_hpp__