sot-torque-control  1.5.2
ddp_pyrene_actuator_solver.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2018-, Olivier Stasse LAAS-CNRS
3  *
4  * This file is part of sot-torque-control.
5  * sot-torque-control is free software: you can redistribute it and/or
6  * modify it under the terms of the GNU Lesser General Public License
7  * as published by the Free Software Foundation, either version 3 of
8  * the License, or (at your option) any later version.
9  * sot-torque-control 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
12  * GNU Lesser General Public License for more details. You should
13  * have received a copy of the GNU Lesser General Public License along
14  * with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
15  */
16 
17 #define EIGEN_NO_MALLOC
18 #include <Eigen/Dense>
19 #include <dynamic-graph/factory.h>
20 #include <sot/core/debug.hh>
23 #include <math.h>
24 
25 #if DEBUG
26 #define ODEBUG(x) std::cout << x << std::endl
27 #else
28 #define ODEBUG(x)
29 #endif
30 #define ODEBUG3(x) std::cout << x << std::endl
31 
32 #define DBGFILE "/tmp/debug-ddp_pyrene_actuator_solver.dat"
33 
34 #define RESETDEBUG5() { std::ofstream DebugFile; \
35  DebugFile.open(DBGFILE,std::ofstream::out); \
36  DebugFile.close();}
37 #define ODEBUG5FULL(x) { std::ofstream DebugFile; \
38  DebugFile.open(DBGFILE,std::ofstream::app); \
39  DebugFile << __FILE__ << ":" \
40  << __FUNCTION__ << "(#" \
41  << __LINE__ << "):" << x << std::endl; \
42  DebugFile.close();}
43 #define ODEBUG5(x) { std::ofstream DebugFile; \
44  DebugFile.open(DBGFILE,std::ofstream::app); \
45  DebugFile << x << std::endl; \
46  DebugFile.close();}
47 
48 #define RESETDEBUG4()
49 #define ODEBUG4FULL(x)
50 #define ODEBUG4(x)
51 
52 namespace dynamicgraph
53 {
54 namespace sot
55 {
56 namespace torque_control
57 {
58 
59 namespace dynamicgraph = ::dynamicgraph;
60 using namespace dynamicgraph;
61 using namespace dynamicgraph::command;
62 using namespace Eigen;
63 
64 #define ALL_INPUT_SIGNALS m_pos_desSIN << m_pos_joint_measureSIN << m_dx_joint_measureSIN << m_tau_desSIN
65 
66 #define ALL_OUTPUT_SIGNALS m_tauSOUT
67 
70 typedef DdpPyreneActuatorSolver EntityClassName;
71 
72 /* --- DG FACTORY ------------------------------------------------------- */
73 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DdpPyreneActuatorSolver, "DdpPyreneActuatorSolver");
74 
76 DdpPyreneActuatorSolver(const std::string &name)
77  : Entity(name),
78  CONSTRUCT_SIGNAL_IN (pos_des, dynamicgraph::Vector),
79  CONSTRUCT_SIGNAL_IN (pos_joint_measure, dynamicgraph::Vector),
80  CONSTRUCT_SIGNAL_IN (dx_joint_measure, dynamicgraph::Vector),
81  CONSTRUCT_SIGNAL_IN (tau_des, dynamicgraph::Vector),
82  CONSTRUCT_SIGNAL_OUT(tau, dynamicgraph::Vector, ALL_INPUT_SIGNALS),
83  m_dt(1e-3),
84  m_T(50),
85  m_stopCrit(1e-3),
86  m_iterMax(10),
87  m_solver(m_model, m_cost,
88  DISABLE_FULLDDP,
89  DISABLE_QPBOX)
90  {
91 
92  RESETDEBUG5();
93  Entity::signalRegistration( ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS );
94 
95  m_zeroState.setZero();
96 
97  /* Commands. */
98  addCommand("init",
99  makeCommandVoid4(*this, &DdpPyreneActuatorSolver::param_init,
100  docCommandVoid4("Initialize the DDP solver.",
101  "Control timestep [s].",
102  "Size of the preview window (in nb of samples)",
103  "Max. nb. of iterations",
104  "Stopping criteria")));
105  addCommand("setTorqueLimit",
106  makeCommandVoid1(*this, &DdpPyreneActuatorSolver::setTorqueLimit,
107  docCommandVoid1("Set the Torque limit.",
108  "Limit of the motor torque.")));
109  addCommand("setJointLimit",
110  makeCommandVoid2(*this, &DdpPyreneActuatorSolver::setJointLimit,
111  docCommandVoid2("Set the angular limits of the joint.",
112  "Upper limit",
113  "Lower limit.")));
114  addCommand("setJointVelLimit",
115  makeCommandVoid2(*this, &DdpPyreneActuatorSolver::setJointVelLimit,
116  docCommandVoid2("Set the angular velocity limits of the joint.",
117  "Upper limit",
118  "Lower limit.")));
119  addCommand("setLoadParam",
120  makeCommandVoid3(*this, &DdpPyreneActuatorSolver::setLoadParam,
121  docCommandVoid3("Setter of the Load parameters.",
122  "Mass of the load [g].",
123  "X coordinate of the Load",
124  "Y coordinate of the Load")));
125  addCommand("setLoadMass",
126  makeCommandVoid1(*this, &DdpPyreneActuatorSolver::setLoadMass,
127  docCommandVoid1("Set the Load mass.",
128  "Mass of the load [g].")));
129  addCommand("removeLoad",
130  makeCommandVoid0(*this, &DdpPyreneActuatorSolver::removeLoad,
131  docCommandVoid0("Remove the Load.")));
132 
133  addCommand("setCostGainState",
134  makeCommandVoid1(*this, &DdpPyreneActuatorSolver::setCostGainState,
135  docCommandVoid1("Set the Gain of the state cost matrix.",
136  "Matrix of Gains.")));
137  addCommand("setCostGainCommand",
138  makeCommandVoid1(*this, &DdpPyreneActuatorSolver::setCostGainCommand,
139  docCommandVoid1("Set the Gain of the command cost matrix.",
140  "Matrix of Gains.")));
141  addCommand("setCostGainStateConstraint",
143  docCommandVoid1("Set the Gain of the constraints on the state.",
144  "Matrix of Gains.")));
145  addCommand("setCostGainTorqueConstraint",
147  docCommandVoid1("Set the Gain of the torque constraints.",
148  "Matrix of Gains.")));
149 
150  m_initSucceeded = true;
151  }
152 
153 /* --- COMMANDS ---------------------------------------------------------- */
154 DEFINE_SIGNAL_OUT_FUNCTION(tau, dynamicgraph::Vector)
155 {
156  if (!m_initSucceeded)
157  {
158  SEND_WARNING_STREAM_MSG("Cannot compute signal tau before initialization!");
159  return s;
160  }
161 
164  const dynamicgraph::Vector &
165  pos_des = m_pos_desSIN(iter);
167  const dynamicgraph::Vector &
168  pos_joint_measure = m_pos_joint_measureSIN(iter);
170  const dynamicgraph::Vector &
171  dx_joint_measure = m_dx_joint_measureSIN(iter);
173  const dynamicgraph::Vector &
174  tau_des = m_tau_desSIN(iter);
175 
176  DDPSolver<double, 2, 1>::stateVec_t xinit, xDes;
177 
178  xinit << pos_joint_measure(0),
179  dx_joint_measure(0);
180 
181  xDes << pos_des, 0.0;
182  //ODEBUG(xDes);
183 
184  m_solver.initSolver(xinit, xDes);
185  //ODEBUG("FirstInitSolver");
186 
188  m_solver.solveTrajectory();
189  //ODEBUG("Trajectory solved");
190 
192  DDPSolver<double, 2, 1>::traj lastTraj;
193  lastTraj = m_solver.getLastSolvedTrajectory();
194  //ODEBUG("getLastSolvedTrajectory");
195 
196  DDPSolver<double, 2, 1>::commandVecTab_t uList;
197  uList = lastTraj.uList;
198 
199  s = m_previous_tau;
200  double tau_ddp = uList[0](0,0);
201  if (!isnan(tau_ddp))
202  {
203  s[25] = tau_ddp;
204  }
205 
206  m_previous_tau = s;
207  //ODEBUG(s);
208 
209  return s;
210 }
211 
212 void DdpPyreneActuatorSolver::param_init(const double &timestep,
213  const int &T,
214  const int &nbItMax,
215  const double &stopCriteria)
216 {
217  if (!m_pos_desSIN.isPlugged())
218  return SEND_MSG("Init failed: signal pos_des is not plugged", MSG_TYPE_ERROR);
219  if (!m_pos_joint_measureSIN.isPlugged())
220  return SEND_MSG("Init failed: signal pos_joint_measure is not plugged", MSG_TYPE_ERROR);
221  if (!m_dx_joint_measureSIN.isPlugged())
222  return SEND_MSG("Init failed: signal dx_joint_measure is not plugged", MSG_TYPE_ERROR);
223 
224  m_previous_tau.resize(32);
225  m_previous_tau.setZero();
226  m_T = T;
227  m_dt = timestep;
228  m_iterMax = nbItMax;
229  m_stopCrit = stopCriteria;
230  m_cost.setTauLimit(70.0);
231  m_cost.setJointLimit(0.0, -2.35619449019);
232  m_cost.setJointVelLimit(30.0, -30.0);
233  m_solver.FirstInitSolver( m_zeroState, m_zeroState,
235 }
236 
238 {
239  m_cost.setTauLimit(tau);
240 }
241 
242 void DdpPyreneActuatorSolver::setJointLimit(const double& upperLim, const double& lowerLim)
243 {
244  m_cost.setJointLimit(upperLim, lowerLim);
245 }
246 
247 void DdpPyreneActuatorSolver::setJointVelLimit(const double& upperLim, const double& lowerLim)
248 {
249  m_cost.setJointVelLimit(upperLim, lowerLim);
250 }
251 
252 void DdpPyreneActuatorSolver::setLoadParam(const double& mass, const double& coordX, const double& coordY)
253 {
254  m_model.setLoadParam(mass, coordX, coordY);
255 }
256 
257 void DdpPyreneActuatorSolver::setLoadMass(const double& mass)
258 {
259  m_model.setLoadMass(mass);
260 }
261 
263 {
264  m_model.removeLoad();
265 }
266 
267 void DdpPyreneActuatorSolver::setCostGainState(const dynamicgraph::Vector& Q)
268 {
269  const CostFunction<double,2,1>::stateMat_t Q_new = Eigen::Map<const CostFunction<double,2,1>::stateMat_t, Eigen::Unaligned >(Q.data(),2,1);
270  m_cost.setCostGainState(Q_new);
271 }
272 
273 void DdpPyreneActuatorSolver::setCostGainStateConstraint(const dynamicgraph::Vector& W)
274 {
275  const CostFunction<double,2,1>::stateMat_t W_new = Eigen::Map<const CostFunction<double,2,1>::stateMat_t, Eigen::Unaligned >(W.data(),2,1);
276  m_cost.setCostGainStateConstraint(W_new);
277 }
278 
279 void DdpPyreneActuatorSolver::setCostGainCommand(const dynamicgraph::Vector& R)
280 {
281  const CostFunction<double,2,1>::commandMat_t R_new = Eigen::Map<const CostFunction<double,2,1>::commandMat_t, Eigen::Unaligned >(R.data(),1);
282  m_cost.setCostGainCommand(R_new);
283 }
284 
285 void DdpPyreneActuatorSolver::setCostGainTorqueConstraint(const dynamicgraph::Vector& P)
286 {
287  const CostFunction<double,2,1>::commandMat_t P_new = Eigen::Map<const CostFunction<double,2,1>::commandMat_t, Eigen::Unaligned >(P.data(),1);
288  m_cost.setCostGainTorqueConstraint(P_new);
289 }
290 
291 void DdpPyreneActuatorSolver::display(std::ostream &os) const
292 {
293  os << " T: " << m_T
294  << " timestep: " << m_dt
295  << " nbItMax: " << m_iterMax
296  << " stopCriteria: " << m_stopCrit << std::endl;
297 }
298 
299 } // namespace torque_control
300 } // namespace sot
301 } // namespace dynamicgraph
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::m_initSucceeded
bool m_initSucceeded
Definition: ddp_pyrene_actuator_solver.hh:69
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::setLoadMass
void setLoadMass(const double &mass)
Definition: ddp_pyrene_actuator_solver.cpp:257
ALL_OUTPUT_SIGNALS
#define ALL_OUTPUT_SIGNALS
Definition: ddp_pyrene_actuator_solver.cpp:66
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::removeLoad
void removeLoad()
Definition: ddp_pyrene_actuator_solver.cpp:262
dynamicgraph
to read text file
Definition: treeview.dox:22
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::m_stopCrit
double m_stopCrit
Definition: ddp_pyrene_actuator_solver.hh:73
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::m_dt
double m_dt
Definition: ddp_pyrene_actuator_solver.hh:68
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::m_solver
DDPSolver< double, 2, 1 > m_solver
Definition: ddp_pyrene_actuator_solver.hh:77
dynamicgraph::command
Definition: commands-helper.hh:38
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::m_iterMax
unsigned int m_iterMax
Definition: ddp_pyrene_actuator_solver.hh:74
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::setJointVelLimit
void setJointVelLimit(const double &upperLim, const double &lowerLim)
Definition: ddp_pyrene_actuator_solver.cpp:247
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::setTorqueLimit
void setTorqueLimit(const double &tau)
Definition: ddp_pyrene_actuator_solver.cpp:237
commands-helper.hh
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::setCostGainStateConstraint
void setCostGainStateConstraint(const dynamicgraph::Vector &W)
Definition: ddp_pyrene_actuator_solver.cpp:273
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::DdpPyreneActuatorSolver
EIGEN_MAKE_ALIGNED_OPERATOR_NEW DdpPyreneActuatorSolver(const std::string &name)
Definition: ddp_pyrene_actuator_solver.cpp:76
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::setCostGainCommand
void setCostGainCommand(const dynamicgraph::Vector &R)
Definition: ddp_pyrene_actuator_solver.cpp:279
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::m_model
pyreneActuator m_model
Definition: ddp_pyrene_actuator_solver.hh:75
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::display
virtual void display(std::ostream &os) const
Definition: ddp_pyrene_actuator_solver.cpp:291
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::m_cost
CostFunctionPyreneActuator m_cost
Definition: ddp_pyrene_actuator_solver.hh:76
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::setCostGainTorqueConstraint
void setCostGainTorqueConstraint(const dynamicgraph::Vector &P)
Definition: ddp_pyrene_actuator_solver.cpp:285
torque_control
Definition: __init__.py:1
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::setJointLimit
void setJointLimit(const double &upperLim, const double &lowerLim)
Definition: ddp_pyrene_actuator_solver.cpp:242
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::param_init
void param_init(const double &timestep, const int &T, const int &nbItMax, const double &stopCriteria)
Definition: ddp_pyrene_actuator_solver.cpp:212
ddp_pyrene_actuator_solver.hh
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::m_zeroState
DDPSolver< double, 2, 1 >::stateVec_t m_zeroState
Definition: ddp_pyrene_actuator_solver.hh:70
RESETDEBUG5
#define RESETDEBUG5()
Definition: ddp_pyrene_actuator_solver.cpp:34
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::setCostGainState
void setCostGainState(const dynamicgraph::Vector &Q)
Definition: ddp_pyrene_actuator_solver.cpp:267
dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
dynamicgraph::sot::torque_control::EntityClassName
AdmittanceController EntityClassName
Definition: admittance-controller.cpp:44
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::m_T
unsigned int m_T
Definition: ddp_pyrene_actuator_solver.hh:72
dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
Definition: admittance-controller.cpp:160
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::setLoadParam
void setLoadParam(const double &mass, const double &coordX, const double &coordY)
Definition: ddp_pyrene_actuator_solver.cpp:252
ALL_INPUT_SIGNALS
#define ALL_INPUT_SIGNALS
Definition: ddp_pyrene_actuator_solver.cpp:64
dynamicgraph::sot::torque_control::DdpPyreneActuatorSolver::m_previous_tau
dynamicgraph::Vector m_previous_tau
Definition: ddp_pyrene_actuator_solver.hh:67