5 #define EIGEN_RUNTIME_NO_MALLOC
8 #include <dynamic-graph/factory.h>
9 #include <sot/core/debug.hh>
15 #define ODEBUG(x) std::cout << x << std::endl
19 #define ODEBUG3(x) std::cout << x << std::endl
21 #define DBGFILE "/tmp/debug-ddp_actuator_solver.dat"
23 #define RESETDEBUG5() \
25 std::ofstream DebugFile; \
26 DebugFile.open(DBGFILE, std::ofstream::out); \
29 #define ODEBUG5FULL(x) \
31 std::ofstream DebugFile; \
32 DebugFile.open(DBGFILE, std::ofstream::app); \
33 DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ << "):" << x << std::endl; \
38 std::ofstream DebugFile; \
39 DebugFile.open(DBGFILE, std::ofstream::app); \
40 DebugFile << x << std::endl; \
45 #define ODEBUG4FULL(x)
55 using namespace Eigen;
67 CONSTRUCT_SIGNAL_IN(pos_motor_measure,
dynamicgraph::Vector),
68 CONSTRUCT_SIGNAL_IN(pos_joint_measure,
dynamicgraph::Vector),
73 CONSTRUCT_SIGNAL_OUT(tau,
dynamicgraph::Vector, m_pos_desSIN),
75 m_solver(m_model, m_cost, DISABLE_FULLDDP, DISABLE_QPBOX),
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")));
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);
107 DDPSolver<double, 5, 1>::stateVec_t xinit, xDes;
110 xinit << pos_joint_measure(0), dx_measure(0), temp_measure(0), tau_measure(0),
114 xDes << pos_des, 0.0, 25.0, tau_des, 25.0;
121 DDPSolver<double, 5, 1> m_solver(model, cost, 0, 0);
123 m_solver.FirstInitSolver(xinit, xDes, m_T, m_dt, m_iterMax, m_stopCrit);
127 m_solver.solveTrajectory();
131 DDPSolver<double, 5, 1>::traj lastTraj;
132 lastTraj = m_solver.getLastSolvedTrajectory();
133 ODEBUG5(
"getLastSolvedTrajectory");
135 DDPSolver<double, 5, 1>::commandVecTab_t uList;
136 uList = lastTraj.uList;
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;
149 const double &stopCriteria) {