Loading...
Searching...
No Matches
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
38namespace 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__
Definition: contact-6d.hpp:32
Definition: contact-point.hpp:31
Definition: container.hpp:76
bool set(HQPData data)
Definition: container.hpp:117
Definition: solver-HQP-output.hpp:33
Definition: task-angular-momentum-equality.hpp:35
Definition: task-actuation-bounds.hpp:31
Definition: task-com-equality.hpp:32
Definition: task-cop-equality.hpp:33
Definition: task-joint-bounds.hpp:30
Definition: task-joint-posture.hpp:32
Definition: task-se3-equality.hpp:34
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: fwd.hpp:37
Definition: constraint-bound.hpp:27
Definition: formulation.hpp:47
static bool checkContact(T &self, const std::string &name, const solvers::HQPOutput &sol)
Definition: formulation.hpp:165
void visit(PyClass &cl) const
Definition: formulation.hpp:50
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
static bool addActuationTask_Bounds(T &self, tasks::TaskActuationBounds &task, double weight, unsigned int priorityLevel, double transition_duration)
Definition: formulation.hpp:109
static bool updateRigidContactWeights(T &self, const std::string &contact_name, double force_regularization_weight)
Definition: formulation.hpp:115
static bool addRigidContact6dDeprecated(T &self, contacts::Contact6d &contact)
Definition: formulation.hpp:121
static bool updateTaskWeight(T &self, const std::string &task_name, double weight)
Definition: formulation.hpp:112
static bool addRigidContact6d(T &self, contacts::Contact6d &contact, double force_regularization_weight)
Definition: formulation.hpp:124
static bool removeRigidContact(T &self, const std::string &contactName, double transition_duration)
Definition: formulation.hpp:145
static Eigen::VectorXd getContactForce(T &self, const std::string &name, const solvers::HQPOutput &sol)
Definition: formulation.hpp:171
static bool addMotionTask_Joint(T &self, tasks::TaskJointPosture &task, double weight, unsigned int priorityLevel, double transition_duration)
Definition: formulation.hpp:97
static bool addMotionTask_SE3(T &self, tasks::TaskSE3Equality &task, double weight, unsigned int priorityLevel, double transition_duration)
Definition: formulation.hpp:91
static bool removeFromHqpData(T &self, const std::string &constraintName)
Definition: formulation.hpp:148
static bool addRigidContactPoint(T &self, contacts::ContactPoint &contact, double force_regularization_weight)
Definition: formulation.hpp:133
static bool addForceTask_COP(T &self, tasks::TaskCopEquality &task, double weight, unsigned int priorityLevel, double transition_duration)
Definition: formulation.hpp:106
static bool addMotionTask_AM(T &self, tasks::TaskAMEquality &task, double weight, unsigned int priorityLevel, double transition_duration)
Definition: formulation.hpp:103
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 removeTask(T &self, const std::string &task_name, double transition_duration)
Definition: formulation.hpp:142
static Eigen::VectorXd getContactForces(T &self, const solvers::HQPOutput &sol)
Definition: formulation.hpp:162
static bool addRigidContact6dWithPriorityLevel(T &self, contacts::Contact6d &contact, double force_regularization_weight, double motion_weight, const bool priority_level)
Definition: formulation.hpp:127
static bool addMotionTask_COM(T &self, tasks::TaskComEquality &task, double weight, unsigned int priorityLevel, double transition_duration)
Definition: formulation.hpp:94
static Eigen::VectorXd getAccelerations(T &self, const solvers::HQPOutput &sol)
Definition: formulation.hpp:159
static pinocchio::Data data(T &self)
Definition: formulation.hpp:87
static Eigen::VectorXd getActuatorForces(T &self, const solvers::HQPOutput &sol)
Definition: formulation.hpp:156
static void expose(const std::string &class_name)
Definition: formulation.hpp:176
static HQPDatas computeProblemData(T &self, double time, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
Definition: formulation.hpp:151