Additions of debug functions and some addition for the solver

include/sot/torque_control/ddp-actuator-solver.hh

@@ -34,7 +34,7 @@ namespace dynamicgraph {
 
 #define ALL_INPUT_SIGNALS m_pos_desSIN << m_pos_motor_measureSIN \
       << m_pos_joint_measureSIN << m_dx_measureSIN		 \
-      << m_tau_measureSIN << m_temp_measureSIN 
+      << m_tau_measureSIN << m_temp_measureSIN << m_tau_desSIN
 
 #define ALL_OUTPUT_SIGNALS  m_tauSOUT
       
@@ -49,7 +49,8 @@ namespace dynamicgraph {
 	  DECLARE_SIGNAL_IN(pos_joint_measure,  dynamicgraph::Vector);
 	  DECLARE_SIGNAL_IN(dx_measure,         dynamicgraph::Vector);
 	  DECLARE_SIGNAL_IN(tau_measure,        dynamicgraph::Vector);
-	  DECLARE_SIGNAL_IN(temp_measure,        dynamicgraph::Vector);	  
+          DECLARE_SIGNAL_IN(tau_des,            dynamicgraph::Vector);
+          DECLARE_SIGNAL_IN(temp_measure,       dynamicgraph::Vector);
 	  DECLARE_SIGNAL_OUT(tau,               dynamicgraph::Vector);
 
 	protected:

src/ddp-actuator-solver.cpp

@@ -20,6 +20,9 @@
 #include <sot/torque_control/commands-helper.hh>
 #include <sot/torque_control/ddp-actuator-solver.hh>
 
+#include <ddp-actuator-solver/examples/dctemp.hh>
+#include <ddp-actuator-solver/examples/costtemp.hh>
+
 #if DEBUG
 #define ODEBUG(x) std::cout << x << std::endl
 #else
@@ -74,6 +77,7 @@ namespace dynamicgraph
 	  CONSTRUCT_SIGNAL_IN (pos_joint_measure, dynamicgraph::Vector),
 	  CONSTRUCT_SIGNAL_IN (dx_measure,        dynamicgraph::Vector),
 	  CONSTRUCT_SIGNAL_IN (tau_measure,       dynamicgraph::Vector),
+          CONSTRUCT_SIGNAL_IN (tau_des,           dynamicgraph::Vector),
 	  CONSTRUCT_SIGNAL_IN (temp_measure,      dynamicgraph::Vector),
 	  CONSTRUCT_SIGNAL_OUT(tau,               dynamicgraph::Vector, m_pos_desSIN),
 	  m_T(3000),
@@ -119,6 +123,9 @@ namespace dynamicgraph
 	/// Measured torque
 	const dynamicgraph::Vector &
           tau_measure = m_tau_measureSIN(iter);
+        /// Desired torque
+        const dynamicgraph::Vector &
+          tau_des = m_tau_desSIN(iter);
 
 	DDPSolver<double,5,1>::stateVec_t xinit,xDes;
 
@@ -127,29 +134,40 @@ namespace dynamicgraph
 	  dx_measure(0),
 	  temp_measure(0),
 	  tau_measure(0),
-	  m_ambiant_temperature;
+          //m_ambiant_temperature;
+          25.0;
+
+        xDes << m_pos_desSIN(0), 0.0, 25.0, m_tau_desSIN(0), 25.0;
+        ODEBUG5(xinit);
+        ODEBUG5("");
+        ODEBUG5(xDes);
 
-	xDes << m_pos_desSIN(0), 0.0, 0.0, 0.0, 0.0;
+        DCTemp model;
+        CostTemp cost;
+        DDPSolver<double,5,1> m_solver(model,cost,0,0);
 
-	m_solver.initSolver(xinit,xDes);
+        m_solver.FirstInitSolver(xinit,xDes,m_T,m_dt,m_iterMax, m_stopCrit);
+        ODEBUG5("FirstInitSolver");
 
-        ODEBUG5("FirstInitSolver")
 	/// --- Solve the DDP ---
-	m_solver.solveTrajectory();
-        ODEBUG5("Trajectory solved")
+        m_solver.solveTrajectory();
+        ODEBUG5("Trajectory solved");
 
-	/// --- Get the command ---
-	DDPSolver<double,5,1>::traj lastTraj;
-	lastTraj = m_solver.getLastSolvedTrajectory();
-        ODEBUG5("getLastSolvedTrajectory")
+        /// --- Get the command ---
+        DDPSolver<double,5,1>::traj lastTraj;
+        lastTraj = m_solver.getLastSolvedTrajectory();
+        ODEBUG5("getLastSolvedTrajectory");
 
-	DDPSolver<double,5,1>::commandVecTab_t uList;
-	uList = lastTraj.uList;
-        ODEBUG5("uList")
+        DDPSolver<double,5,1>::commandVecTab_t uList;
+        uList = lastTraj.uList;
+        ODEBUG5("uList");
 
 
-        s = uList[0];
-        ODEBUG5(s)
+        //s = uList[0];
+        s.resize(32);
+        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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+        //s.setZero(32);
+        ODEBUG5(s);
         return s;
       }