GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/tsid/bindings/python/contacts/contact-6d.hpp Lines: 43 83 51.8 %
Date: 2024-02-02 08:47:34 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
7
  void visit(PyClass &cl) const {
40
7
    cl.def(bp::init<std::string, robots::RobotWrapper &, std::string,
41
                    Eigen::MatrixXd, Eigen::VectorXd, double, double, double>(
42
               (bp::arg("name"), bp::arg("robot"), bp::arg("framename"),
43
                bp::arg("contactPoint"), bp::arg("contactNormal"),
44
                bp::arg("frictionCoeff"), bp::arg("minForce"),
45
                bp::arg("maxForce")),
46
               "Default Constructor"))
47








14
        .def(bp::init<std::string, robots::RobotWrapper &, std::string,
48
                      Eigen::MatrixXd, Eigen::VectorXd, double, double, double,
49
                      double>(
50
            (bp::arg("name"), bp::arg("robot"), bp::arg("framename"),
51
             bp::arg("contactPoint"), bp::arg("contactNormal"),
52
             bp::arg("frictionCoeff"), bp::arg("minForce"), bp::arg("maxForce"),
53
             bp::arg("wForceReg")),
54
            "Deprecated Constructor"))
55









14
        .add_property("n_motion", &Contact6d::n_motion,
56
                      "return number of motion")
57
14
        .add_property("n_force", &Contact6d::n_force, "return number of force")
58
7
        .add_property("name", &Contact6DPythonVisitor::name, "return name")
59

7
        .def("computeMotionTask", &Contact6DPythonVisitor::computeMotionTask,
60
             bp::args("t", "q", "v", "data"))
61

14
        .def("computeForceTask", &Contact6DPythonVisitor::computeForceTask,
62
             bp::args("t", "q", "v", "data"))
63

14
        .def("computeForceRegularizationTask",
64
             &Contact6DPythonVisitor::computeForceRegularizationTask,
65
             bp::args("t", "q", "v", "data"))
66

14
        .def("getMotionTask", &Contact6DPythonVisitor::getMotionTask)
67
68
7
        .add_property("getForceGeneratorMatrix",
69
                      bp::make_function(
70
                          &Contact6DPythonVisitor::getForceGeneratorMatrix,
71
                          bp::return_value_policy<bp::copy_const_reference>()))
72
73

14
        .def("getNormalForce", &Contact6DPythonVisitor::getNormalForce,
74
             bp::arg("vec"))
75

14
        .add_property("getMinNormalForce", &Contact6d::getMinNormalForce)
76
14
        .add_property("getMaxNormalForce", &Contact6d::getMaxNormalForce)
77
78
7
        .add_property("Kp",
79
                      bp::make_function(
80
                          &Contact6DPythonVisitor::Kp,
81
                          bp::return_value_policy<bp::copy_const_reference>()))
82

14
        .add_property("Kd",
83
                      bp::make_function(
84
                          &Contact6DPythonVisitor::Kd,
85
                          bp::return_value_policy<bp::copy_const_reference>()))
86

14
        .def("setKp", &Contact6DPythonVisitor::setKp, bp::arg("Kp"))
87

14
        .def("setKd", &Contact6DPythonVisitor::setKd, bp::arg("Kd"))
88
89

14
        .def("setContactPoints", &Contact6DPythonVisitor::setContactPoints,
90
             bp::args("vec"))
91

14
        .def("setContactNormal", &Contact6DPythonVisitor::setContactNormal,
92
             bp::args("vec"))
93

14
        .def("setFrictionCoefficient",
94
             &Contact6DPythonVisitor::setFrictionCoefficient,
95
             bp::args("friction_coeff"))
96

14
        .def("setMinNormalForce", &Contact6DPythonVisitor::setMinNormalForce,
97
             bp::args("min_force"))
98

14
        .def("setMaxNormalForce", &Contact6DPythonVisitor::setMaxNormalForce,
99
             bp::args("max_force"))
100

14
        .def("setReference", &Contact6DPythonVisitor::setReference,
101
             bp::args("SE3"))
102

14
        .def("setForceReference", &Contact6DPythonVisitor::setForceReference,
103
             bp::args("f_vec"))
104

14
        .def("setRegularizationTaskWeightVector",
105
             &Contact6DPythonVisitor::setRegularizationTaskWeightVector,
106
             bp::args("w_vec"));
107
7
  }
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
2
    return self.Kp(Kp);
155
  }
156
2
  static void setKd(Contact6d &self, const ::Eigen::VectorXd Kd) {
157
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
10
    return self.getNormalForce(f);
190
  }
191
192
7
  static void expose(const std::string &class_name) {
193
7
    std::string doc = "Contact6d info.";
194

7
    bp::class_<Contact6d>(class_name.c_str(), doc.c_str(), bp::no_init)
195
        .def(Contact6DPythonVisitor<Contact6d>());
196
7
  }
197
};
198
}  // namespace python
199
}  // namespace tsid
200
201
#endif  // ifndef __tsid_python_contact_hpp__