sot-torque-control  1.6.2
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
ddp-actuator-solver.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2018-, Olivier Stasse LAAS-CNRS
3  *
4  */
5 #define EIGEN_RUNTIME_NO_MALLOC
6 
7 #include <Eigen/Dense>
8 #include <dynamic-graph/factory.h>
9 #include <sot/core/debug.hh>
12 
13 
14 #if DEBUG
15 #define ODEBUG(x) std::cout << x << std::endl
16 #else
17 #define ODEBUG(x)
18 #endif
19 #define ODEBUG3(x) std::cout << x << std::endl
20 
21 #define DBGFILE "/tmp/debug-ddp_actuator_solver.dat"
22 
23 #define RESETDEBUG5() \
24  { \
25  std::ofstream DebugFile; \
26  DebugFile.open(DBGFILE, std::ofstream::out); \
27  DebugFile.close(); \
28  }
29 #define ODEBUG5FULL(x) \
30  { \
31  std::ofstream DebugFile; \
32  DebugFile.open(DBGFILE, std::ofstream::app); \
33  DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ << "):" << x << std::endl; \
34  DebugFile.close(); \
35  }
36 #define ODEBUG5(x) \
37  { \
38  std::ofstream DebugFile; \
39  DebugFile.open(DBGFILE, std::ofstream::app); \
40  DebugFile << x << std::endl; \
41  DebugFile.close(); \
42  }
43 
44 #define RESETDEBUG4()
45 #define ODEBUG4FULL(x)
46 #define ODEBUG4(x)
47 
48 namespace dynamicgraph {
49 namespace sot {
50 namespace torque_control {
51 
52 namespace dynamicgraph = ::dynamicgraph;
53 using namespace dynamicgraph;
54 using namespace dynamicgraph::command;
55 using namespace Eigen;
56 
59 typedef DdpActuatorSolver EntityClassName;
60 
61 /* --- DG FACTORY ------------------------------------------------------- */
62 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DdpActuatorSolver, "DdpActuatorSolver");
63 
64 DdpActuatorSolver::DdpActuatorSolver(const std::string &name)
65  : Entity(name),
66  CONSTRUCT_SIGNAL_IN(pos_des, dynamicgraph::Vector),
67  CONSTRUCT_SIGNAL_IN(pos_motor_measure, dynamicgraph::Vector),
68  CONSTRUCT_SIGNAL_IN(pos_joint_measure, dynamicgraph::Vector),
69  CONSTRUCT_SIGNAL_IN(dx_measure, dynamicgraph::Vector),
70  CONSTRUCT_SIGNAL_IN(tau_measure, dynamicgraph::Vector),
71  CONSTRUCT_SIGNAL_IN(tau_des, dynamicgraph::Vector),
72  CONSTRUCT_SIGNAL_IN(temp_measure, dynamicgraph::Vector),
73  CONSTRUCT_SIGNAL_OUT(tau, dynamicgraph::Vector, m_pos_desSIN),
74  m_dt(1e-3),
75  m_solver(m_model, m_cost, DISABLE_FULLDDP, DISABLE_QPBOX),
76  m_T(3000),
77  m_stopCrit(1e-5),
78  m_iterMax(100) {
79  RESETDEBUG5();
80  Entity::signalRegistration(ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS);
81 
82  m_zeroState.setZero();
83 
84  /* Commands. */
85  addCommand("init", makeCommandVoid4(*this, &DdpActuatorSolver::param_init,
86  docCommandVoid4("Initialize the DDP solver.", "Control timestep [s].",
87  "Size of the preview window (in nb of samples)",
88  "Max. nb. of iterations", "Stopping criteria")));
89 }
90 
91 /* --- COMMANDS ---------------------------------------------------------- */
92 DEFINE_SIGNAL_OUT_FUNCTION(tau, dynamicgraph::Vector) {
95  const dynamicgraph::Vector &pos_des = m_pos_desSIN(iter);
97  const dynamicgraph::Vector &pos_joint_measure = m_pos_joint_measureSIN(iter);
99  const dynamicgraph::Vector &dx_measure = m_dx_measureSIN(iter);
101  const dynamicgraph::Vector &temp_measure = m_temp_measureSIN(iter);
103  const dynamicgraph::Vector &tau_measure = m_tau_measureSIN(iter);
105  const dynamicgraph::Vector &tau_des = m_tau_desSIN(iter);
106 
107  DDPSolver<double, 5, 1>::stateVec_t xinit, xDes;
108 
110  xinit << pos_joint_measure(0), dx_measure(0), temp_measure(0), tau_measure(0),
111  // m_ambiant_temperature;
112  25.0;
113 
114  xDes << pos_des, 0.0, 25.0, tau_des, 25.0;
115  ODEBUG5(xinit);
116  ODEBUG5("");
117  ODEBUG5(xDes);
118 
119  DCTemp model;
120  CostTemp cost;
121  DDPSolver<double, 5, 1> m_solver(model, cost, 0, 0);
122 
123  m_solver.FirstInitSolver(xinit, xDes, m_T, m_dt, m_iterMax, m_stopCrit);
124  ODEBUG5("FirstInitSolver");
125 
127  m_solver.solveTrajectory();
128  ODEBUG5("Trajectory solved");
129 
131  DDPSolver<double, 5, 1>::traj lastTraj;
132  lastTraj = m_solver.getLastSolvedTrajectory();
133  ODEBUG5("getLastSolvedTrajectory");
134 
135  DDPSolver<double, 5, 1>::commandVecTab_t uList;
136  uList = lastTraj.uList;
137  ODEBUG5("uList");
138 
139  // s = uList[0];
140  s.resize(32);
141  s << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, uList[0], 0.0, 0.0, 0.0,
142  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
143  // s.setZero(32);
144  ODEBUG5(s);
145  return s;
146 }
147 
148 void DdpActuatorSolver::param_init(const double &timestep, const int &T, const int &nbItMax,
149  const double &stopCriteria) {
150  m_T = T;
151  m_dt = timestep;
152  m_iterMax = nbItMax;
153  m_stopCrit = stopCriteria;
155 }
156 
157 void DdpActuatorSolver::display(std::ostream &os) const {
158  os << " T: " << m_T << " timestep: " << m_dt << " nbItMax: " << m_iterMax << " stopCriteria: " << m_stopCrit
159  << std::endl;
160 }
161 } // namespace torque_control
162 } // namespace sot
163 } // namespace dynamicgraph
#define ALL_OUTPUT_SIGNALS
#define ODEBUG5(x)
void param_init(const double &timestep, const int &T, const int &nbItMax, const double &stopCriteria)
#define ALL_INPUT_SIGNALS
#define RESETDEBUG5()
DDPSolver< double, 5, 1 >::stateVec_t m_zeroState
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW DdpActuatorSolver(const std::string &name)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
to read text file
Definition: treeview.dox:22