formulation.hpp
Go to the documentation of this file.
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 
22 
23 #include <pinocchio/bindings/python/utils/deprecation.hpp>
24 
36 
37 
38 namespace tsid
39 {
40  namespace python
41  {
42  namespace bp = boost::python;
43 
44  template<typename T>
46  : public boost::python::def_visitor< InvDynPythonVisitor<T> >
47  {
48  template<class PyClass>
49 
50  void visit(PyClass& cl) const
51  {
52  cl
53  .def(bp::init<std::string, robots::RobotWrapper &, bool>((bp::args("name", "robot", "verbose"))))
54  .def("data", &InvDynPythonVisitor::data)
55  .add_property("nVar", &T::nVar)
56  .add_property("nEq", &T::nEq)
57  .add_property("nIn", &T::nIn)
58 
59  .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_SE3, bp::args("task", "weight", "priorityLevel", "transition duration"))
60  .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_COM, bp::args("task", "weight", "priorityLevel", "transition duration"))
61  .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_Joint, bp::args("task", "weight", "priorityLevel", "transition duration"))
62  .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_JointBounds, bp::args("task", "weight", "priorityLevel", "transition duration"))
63  .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_AM, bp::args("task", "weight", "priorityLevel", "transition duration"))
64  .def("addForceTask", &InvDynPythonVisitor::addForceTask_COP, bp::args("task", "weight", "priorityLevel", "transition duration"))
65  .def("addActuationTask", &InvDynPythonVisitor::addActuationTask_Bounds, bp::args("task", "weight", "priorityLevel", "transition duration"))
66  .def("updateTaskWeight", &InvDynPythonVisitor::updateTaskWeight, bp::args("task_name", "weight"))
67  .def("updateRigidContactWeights", &InvDynPythonVisitor::updateRigidContactWeights, bp::args("contact_name", "force_regularization_weight"))
68  .def("updateRigidContactWeights", &InvDynPythonVisitor::updateRigidContactWeightsWithMotionWeight, bp::args("contact_name", "force_regularization_weight", "motion_weight"))
69  .def("addRigidContact", &InvDynPythonVisitor::addRigidContact6dDeprecated, bp::args("contact"), pinocchio::python::deprecated_function<>(
70  "Method addRigidContact(ContactBase) is deprecated. You should use addRigidContact(ContactBase, double) instead"))
71  .def("addRigidContact", &InvDynPythonVisitor::addRigidContact6d, bp::args("contact", "force_reg_weight"))
72  .def("addRigidContact", &InvDynPythonVisitor::addRigidContact6dWithPriorityLevel, bp::args("contact", "force_reg_weight", "motion_weight", "priority_level"))
73  .def("addRigidContact", &InvDynPythonVisitor::addRigidContactPoint, bp::args("contact", "force_reg_weight"))
74  .def("addRigidContact", &InvDynPythonVisitor::addRigidContactPointWithPriorityLevel, bp::args("contact", "force_reg_weight", "motion_weight", "priority_level"))
75  .def("removeTask", &InvDynPythonVisitor::removeTask, bp::args("task_name", "duration"))
76  .def("removeRigidContact", &InvDynPythonVisitor::removeRigidContact, bp::args("contact_name", "duration"))
77  .def("removeFromHqpData", &InvDynPythonVisitor::removeFromHqpData, bp::args("constraint_name"))
78  .def("computeProblemData", &InvDynPythonVisitor::computeProblemData, bp::args("time", "q", "v"))
79 
80  .def("getActuatorForces", &InvDynPythonVisitor::getActuatorForces, bp::args("HQPOutput"))
81  .def("getAccelerations", &InvDynPythonVisitor::getAccelerations, bp::args("HQPOutput"))
82  .def("getContactForces", &InvDynPythonVisitor::getContactForces, bp::args("HQPOutput"))
83  .def("checkContact", &InvDynPythonVisitor::checkContact, bp::args("name", "HQPOutput"))
84  .def("getContactForce", &InvDynPythonVisitor::getContactForce, bp::args("name", "HQPOutput"))
85  ;
86  }
87  static pinocchio::Data data(T & self){
88  pinocchio::Data data = self.data();
89  return data;
90  }
91  static bool addMotionTask_SE3(T & self, tasks::TaskSE3Equality & task, double weight, unsigned int priorityLevel, double transition_duration){
92  return self.addMotionTask(task, weight, priorityLevel, transition_duration);
93  }
94  static bool addMotionTask_COM(T & self, tasks::TaskComEquality & task, double weight, unsigned int priorityLevel, double transition_duration){
95  return self.addMotionTask(task, weight, priorityLevel, transition_duration);
96  }
97  static bool addMotionTask_Joint(T & self, tasks::TaskJointPosture & task, double weight, unsigned int priorityLevel, double transition_duration){
98  return self.addMotionTask(task, weight, priorityLevel, transition_duration);
99  }
100  static bool addMotionTask_JointBounds(T & self, tasks::TaskJointBounds & task, double weight, unsigned int priorityLevel, double transition_duration){
101  return self.addMotionTask(task, weight, priorityLevel, transition_duration);
102  }
103  static bool addMotionTask_AM(T & self, tasks::TaskAMEquality & task, double weight, unsigned int priorityLevel, double transition_duration){
104  return self.addMotionTask(task, weight, priorityLevel, transition_duration);
105  }
106  static bool addForceTask_COP(T & self, tasks::TaskCopEquality & task, double weight, unsigned int priorityLevel, double transition_duration){
107  return self.addForceTask(task, weight, priorityLevel, transition_duration);
108  }
109  static bool addActuationTask_Bounds(T & self, tasks::TaskActuationBounds & task, double weight, unsigned int priorityLevel, double transition_duration){
110  return self.addActuationTask(task, weight, priorityLevel, transition_duration);
111  }
112  static bool updateTaskWeight(T& self, const std::string & task_name, double weight){
113  return self.updateTaskWeight(task_name, weight);
114  }
115  static bool updateRigidContactWeights(T& self, const std::string & contact_name, double force_regularization_weight){
116  return self.updateRigidContactWeights(contact_name, force_regularization_weight);
117  }
118  static bool updateRigidContactWeightsWithMotionWeight(T& self, const std::string & contact_name, double force_regularization_weight, double motion_weight){
119  return self.updateRigidContactWeights(contact_name, force_regularization_weight, motion_weight);
120  }
121  static bool addRigidContact6dDeprecated(T& self, contacts::Contact6d & contact){
122  return self.addRigidContact(contact, 1e-5);
123  }
124  static bool addRigidContact6d(T& self, contacts::Contact6d & contact, double force_regularization_weight){
125  return self.addRigidContact(contact, force_regularization_weight);
126  }
128  double force_regularization_weight,
129  double motion_weight,
130  const bool priority_level){
131  return self.addRigidContact(contact, force_regularization_weight, motion_weight, priority_level);
132  }
133  static bool addRigidContactPoint(T& self, contacts::ContactPoint & contact, double force_regularization_weight){
134  return self.addRigidContact(contact, force_regularization_weight);
135  }
137  double force_regularization_weight,
138  double motion_weight,
139  const bool priority_level){
140  return self.addRigidContact(contact, force_regularization_weight, motion_weight, priority_level);
141  }
142  static bool removeTask(T& self, const std::string & task_name, double transition_duration){
143  return self.removeTask(task_name, transition_duration);
144  }
145  static bool removeRigidContact(T& self, const std::string & contactName, double transition_duration){
146  return self.removeRigidContact(contactName, transition_duration);
147  }
148  static bool removeFromHqpData(T& self, const std::string & constraintName){
149  return self.removeFromHqpData(constraintName);
150  }
151  static HQPDatas computeProblemData(T& self, double time, const Eigen::VectorXd & q, const Eigen::VectorXd & v){
152  HQPDatas Hqp;
153  Hqp.set(self.computeProblemData(time, q, v));
154  return Hqp;
155  }
156  static Eigen::VectorXd getActuatorForces (T & self, const solvers::HQPOutput & sol){
157  return self.getActuatorForces(sol);
158  }
159  static Eigen::VectorXd getAccelerations (T & self, const solvers::HQPOutput & sol){
160  return self.getAccelerations(sol);
161  }
162  static Eigen::VectorXd getContactForces (T & self, const solvers::HQPOutput & sol){
163  return self.getContactForces(sol);
164  }
165  static bool checkContact(T& self, const std::string & name, const solvers::HQPOutput & sol){
166  tsid::math::Vector f = self.getContactForces(name, sol);
167  if(f.size()>0)
168  return true;
169  return false;
170  }
171  static Eigen::VectorXd getContactForce (T & self, const std::string & name, const solvers::HQPOutput & sol){
172  return self.getContactForces(name, sol);
173  }
174 
175 
176  static void expose(const std::string & class_name)
177  {
178  std::string doc = "InvDyn info.";
179  bp::class_<T>(class_name.c_str(),
180  doc.c_str(),
181  bp::no_init)
182  .def(InvDynPythonVisitor<T>());
183  }
184  };
185  }
186 }
187 
188 
189 #endif // ifndef __tsid_python_HQPOutput_hpp__
static bool addMotionTask_AM(T &self, tasks::TaskAMEquality &task, double weight, unsigned int priorityLevel, double transition_duration)
Definition: formulation.hpp:103
Definition: solver-HQP-output.hpp:32
static HQPDatas computeProblemData(T &self, double time, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
Definition: formulation.hpp:151
Definition: task-joint-posture.hpp:31
Definition: task-angular-momentum-equality.hpp:34
static bool addActuationTask_Bounds(T &self, tasks::TaskActuationBounds &task, double weight, unsigned int priorityLevel, double transition_duration)
Definition: formulation.hpp:109
static bool removeRigidContact(T &self, const std::string &contactName, double transition_duration)
Definition: formulation.hpp:145
static bool addRigidContactPointWithPriorityLevel(T &self, contacts::ContactPoint &contact, double force_regularization_weight, double motion_weight, const bool priority_level)
Definition: formulation.hpp:136
static bool addMotionTask_SE3(T &self, tasks::TaskSE3Equality &task, double weight, unsigned int priorityLevel, double transition_duration)
Definition: formulation.hpp:91
static bool updateTaskWeight(T &self, const std::string &task_name, double weight)
Definition: formulation.hpp:112
static bool removeTask(T &self, const std::string &task_name, double transition_duration)
Definition: formulation.hpp:142
Definition: task-se3-equality.hpp:33
Definition: contact-point.hpp:30
static Eigen::VectorXd getAccelerations(T &self, const solvers::HQPOutput &sol)
Definition: formulation.hpp:159
static bool updateRigidContactWeights(T &self, const std::string &contact_name, double force_regularization_weight)
Definition: formulation.hpp:115
Definition: contact-6d.hpp:31
static pinocchio::Data data(T &self)
Definition: formulation.hpp:87
static bool addMotionTask_Joint(T &self, tasks::TaskJointPosture &task, double weight, unsigned int priorityLevel, double transition_duration)
Definition: formulation.hpp:97
Definition: container.hpp:75
static void expose(const std::string &class_name)
Definition: formulation.hpp:176
static bool addForceTask_COP(T &self, tasks::TaskCopEquality &task, double weight, unsigned int priorityLevel, double transition_duration)
Definition: formulation.hpp:106
static bool removeFromHqpData(T &self, const std::string &constraintName)
Definition: formulation.hpp:148
bool set(HQPData data)
Definition: container.hpp:117
Definition: task-joint-bounds.hpp:29
static bool addMotionTask_JointBounds(T &self, tasks::TaskJointBounds &task, double weight, unsigned int priorityLevel, double transition_duration)
Definition: formulation.hpp:100
static bool updateRigidContactWeightsWithMotionWeight(T &self, const std::string &contact_name, double force_regularization_weight, double motion_weight)
Definition: formulation.hpp:118
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: fwd.hpp:37
static bool addRigidContact6dWithPriorityLevel(T &self, contacts::Contact6d &contact, double force_regularization_weight, double motion_weight, const bool priority_level)
Definition: formulation.hpp:127
static Eigen::VectorXd getContactForce(T &self, const std::string &name, const solvers::HQPOutput &sol)
Definition: formulation.hpp:171
static bool addMotionTask_COM(T &self, tasks::TaskComEquality &task, double weight, unsigned int priorityLevel, double transition_duration)
Definition: formulation.hpp:94
Definition: formulation.hpp:45
static bool addRigidContact6d(T &self, contacts::Contact6d &contact, double force_regularization_weight)
Definition: formulation.hpp:124
void visit(PyClass &cl) const
Definition: formulation.hpp:50
static Eigen::VectorXd getActuatorForces(T &self, const solvers::HQPOutput &sol)
Definition: formulation.hpp:156
Definition: constraint-bound.hpp:26
static Eigen::VectorXd getContactForces(T &self, const solvers::HQPOutput &sol)
Definition: formulation.hpp:162
static bool addRigidContactPoint(T &self, contacts::ContactPoint &contact, double force_regularization_weight)
Definition: formulation.hpp:133
Definition: task-actuation-bounds.hpp:30
static bool addRigidContact6dDeprecated(T &self, contacts::Contact6d &contact)
Definition: formulation.hpp:121
Definition: task-cop-equality.hpp:32
Definition: task-com-equality.hpp:31
pinocchio::Data Data
Definition: inverse-dynamics-formulation-acc-force.cpp:29
static bool checkContact(T &self, const std::string &name, const solvers::HQPOutput &sol)
Definition: formulation.hpp:165