Commit 3a707a0f authored by Paul Dandignac's avatar Paul Dandignac Committed by Noëlie Ramuzat
Browse files

Additions of debug functions and some addition for the solver

parent f9c329a7
......@@ -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:
......
......@@ -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;
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment