sot-torque-control  1.6.4
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
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 <dynamic-graph/factory.h>
8
9#include <Eigen/Dense>
10#include <sot/core/debug.hh>
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__ \
34 << "):" << x << std::endl; \
35 DebugFile.close(); \
36 }
37#define ODEBUG5(x) \
38 { \
39 std::ofstream DebugFile; \
40 DebugFile.open(DBGFILE, std::ofstream::app); \
41 DebugFile << x << std::endl; \
42 DebugFile.close(); \
43 }
44
45#define RESETDEBUG4()
46#define ODEBUG4FULL(x)
47#define ODEBUG4(x)
48
49namespace dynamicgraph {
50namespace sot {
51namespace torque_control {
52
53namespace dynamicgraph = ::dynamicgraph;
54using namespace dynamicgraph;
55using namespace dynamicgraph::command;
56using namespace Eigen;
57
60typedef DdpActuatorSolver EntityClassName;
61
62/* --- DG FACTORY ------------------------------------------------------- */
64
66 : Entity(name),
67 CONSTRUCT_SIGNAL_IN(pos_des, dynamicgraph::Vector),
68 CONSTRUCT_SIGNAL_IN(pos_motor_measure, dynamicgraph::Vector),
69 CONSTRUCT_SIGNAL_IN(pos_joint_measure, dynamicgraph::Vector),
70 CONSTRUCT_SIGNAL_IN(dx_measure, dynamicgraph::Vector),
71 CONSTRUCT_SIGNAL_IN(tau_measure, dynamicgraph::Vector),
72 CONSTRUCT_SIGNAL_IN(tau_des, dynamicgraph::Vector),
73 CONSTRUCT_SIGNAL_IN(temp_measure, dynamicgraph::Vector),
74 CONSTRUCT_SIGNAL_OUT(tau, dynamicgraph::Vector, m_pos_desSIN),
75 m_dt(1e-3),
76 m_solver(m_model, m_cost, DISABLE_FULLDDP, DISABLE_QPBOX),
77 m_T(3000),
78 m_stopCrit(1e-5),
79 m_iterMax(100) {
81 Entity::signalRegistration(ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS);
82
83 m_zeroState.setZero();
84
85 /* Commands. */
86 addCommand(
87 "init",
88 makeCommandVoid4(
90 docCommandVoid4("Initialize the DDP solver.", "Control timestep [s].",
91 "Size of the preview window (in nb of samples)",
92 "Max. nb. of iterations", "Stopping criteria")));
93}
94
95/* --- COMMANDS ---------------------------------------------------------- */
96DEFINE_SIGNAL_OUT_FUNCTION(tau, dynamicgraph::Vector) {
99 const dynamicgraph::Vector &pos_des = m_pos_desSIN(iter);
101 const dynamicgraph::Vector &pos_joint_measure = m_pos_joint_measureSIN(iter);
103 const dynamicgraph::Vector &dx_measure = m_dx_measureSIN(iter);
105 const dynamicgraph::Vector &temp_measure = m_temp_measureSIN(iter);
107 const dynamicgraph::Vector &tau_measure = m_tau_measureSIN(iter);
109 const dynamicgraph::Vector &tau_des = m_tau_desSIN(iter);
110
111 DDPSolver<double, 5, 1>::stateVec_t xinit, xDes;
112
114 xinit << pos_joint_measure(0), dx_measure(0), temp_measure(0), tau_measure(0),
115 // m_ambiant_temperature;
116 25.0;
117
118 xDes << pos_des, 0.0, 25.0, tau_des, 25.0;
119 ODEBUG5(xinit);
120 ODEBUG5("");
121 ODEBUG5(xDes);
122
123 DCTemp model;
124 CostTemp cost;
125 DDPSolver<double, 5, 1> m_solver(model, cost, 0, 0);
126
127 m_solver.FirstInitSolver(xinit, xDes, m_T, m_dt, m_iterMax, m_stopCrit);
128 ODEBUG5("FirstInitSolver");
129
131 m_solver.solveTrajectory();
132 ODEBUG5("Trajectory solved");
133
135 DDPSolver<double, 5, 1>::traj lastTraj;
136 lastTraj = m_solver.getLastSolvedTrajectory();
137 ODEBUG5("getLastSolvedTrajectory");
138
139 DDPSolver<double, 5, 1>::commandVecTab_t uList;
140 uList = lastTraj.uList;
141 ODEBUG5("uList");
142
143 // s = uList[0];
144 s.resize(32);
145 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,
146 0.0, 0.0, 0.0, uList[0], 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
147 0.0, 0.0, 0.0, 0.0;
148 // s.setZero(32);
149 ODEBUG5(s);
150 return s;
151}
152
153void DdpActuatorSolver::param_init(const double &timestep, const int &T,
154 const int &nbItMax,
155 const double &stopCriteria) {
156 m_T = T;
157 m_dt = timestep;
158 m_iterMax = nbItMax;
159 m_stopCrit = stopCriteria;
160 m_solver.FirstInitSolver(m_zeroState, m_zeroState, m_T, m_dt, m_iterMax,
161 m_stopCrit);
162}
163
164void DdpActuatorSolver::display(std::ostream &os) const {
165 os << " T: " << m_T << " timestep: " << m_dt << " nbItMax: " << m_iterMax
166 << " stopCriteria: " << m_stopCrit << std::endl;
167}
168} // namespace torque_control
169} // namespace sot
170} // namespace dynamicgraph
EIGEN_MAKE_ALIGNED_OPERATOR_NEW DdpActuatorSolver(const std::string &name)
void param_init(const double &timestep, const int &T, const int &nbItMax, const double &stopCriteria)
DDPSolver< double, 5, 1 >::stateVec_t m_zeroState
#define RESETDEBUG5()
#define ODEBUG5(x)
#define ALL_OUTPUT_SIGNALS
#define ALL_INPUT_SIGNALS
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
to read text file
Definition: treeview.dox:22