Commit 64205b75 authored by NoelieRamuzat's avatar NoelieRamuzat Committed by Noëlie RAMUZAT
Browse files

[Pyrene_Actuator] Change output signal, take torque_ctrl entry directly

Return the torque_ctrl command except on the joint controlled by the ddp (here right elbow -> n°25)
parent 28bdde81
...@@ -59,6 +59,7 @@ class SOTDDPPYRENEACTUATORSOLVER_EXPORT DdpPyreneActuatorSolver ...@@ -59,6 +59,7 @@ class SOTDDPPYRENEACTUATORSOLVER_EXPORT DdpPyreneActuatorSolver
DECLARE_SIGNAL_IN(pos_des, dynamicgraph::Vector); DECLARE_SIGNAL_IN(pos_des, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(pos_joint_measure, dynamicgraph::Vector); DECLARE_SIGNAL_IN(pos_joint_measure, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dx_joint_measure, dynamicgraph::Vector); DECLARE_SIGNAL_IN(dx_joint_measure, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(tau_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(tau, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(tau, dynamicgraph::Vector);
/* --- ENTITY INHERITANCE --- */ /* --- ENTITY INHERITANCE --- */
......
...@@ -19,6 +19,7 @@ ...@@ -19,6 +19,7 @@
#include <sot/core/debug.hh> #include <sot/core/debug.hh>
#include <sot/torque_control/commands-helper.hh> #include <sot/torque_control/commands-helper.hh>
#include <sot/torque_control/ddp_pyrene_actuator_solver.hh> #include <sot/torque_control/ddp_pyrene_actuator_solver.hh>
#include <math.h>
#if DEBUG #if DEBUG
#define ODEBUG(x) std::cout << x << std::endl #define ODEBUG(x) std::cout << x << std::endl
...@@ -59,7 +60,7 @@ using namespace dynamicgraph; ...@@ -59,7 +60,7 @@ using namespace dynamicgraph;
using namespace dynamicgraph::command; using namespace dynamicgraph::command;
using namespace Eigen; using namespace Eigen;
#define ALL_INPUT_SIGNALS m_pos_desSIN << m_pos_joint_measureSIN << m_dx_joint_measureSIN #define ALL_INPUT_SIGNALS m_pos_desSIN << m_pos_joint_measureSIN << m_dx_joint_measureSIN << m_tau_desSIN
#define ALL_OUTPUT_SIGNALS m_tauSOUT #define ALL_OUTPUT_SIGNALS m_tauSOUT
...@@ -76,6 +77,7 @@ DdpPyreneActuatorSolver(const std::string &name) ...@@ -76,6 +77,7 @@ DdpPyreneActuatorSolver(const std::string &name)
CONSTRUCT_SIGNAL_IN (pos_des, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN (pos_des, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_IN (pos_joint_measure, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN (pos_joint_measure, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_IN (dx_joint_measure, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN (dx_joint_measure, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_IN (tau_des, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_OUT(tau, dynamicgraph::Vector, ALL_INPUT_SIGNALS), CONSTRUCT_SIGNAL_OUT(tau, dynamicgraph::Vector, ALL_INPUT_SIGNALS),
m_dt(1e-3), m_dt(1e-3),
m_T(50), m_T(50),
...@@ -135,6 +137,9 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau, dynamicgraph::Vector) ...@@ -135,6 +137,9 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau, dynamicgraph::Vector)
/// Measured joint speed /// Measured joint speed
const dynamicgraph::Vector & const dynamicgraph::Vector &
dx_joint_measure = m_dx_joint_measureSIN(iter); dx_joint_measure = m_dx_joint_measureSIN(iter);
/// Desired torque
const dynamicgraph::Vector &
tau_des = m_tau_desSIN(iter);
DDPSolver<double, 2, 1>::stateVec_t xinit, xDes; DDPSolver<double, 2, 1>::stateVec_t xinit, xDes;
...@@ -159,10 +164,11 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau, dynamicgraph::Vector) ...@@ -159,10 +164,11 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau, dynamicgraph::Vector)
DDPSolver<double, 2, 1>::commandVecTab_t uList; DDPSolver<double, 2, 1>::commandVecTab_t uList;
uList = lastTraj.uList; uList = lastTraj.uList;
//s = uList[0]; s = tau_des;
s.resize(32); if (!isnan(uList[m_T-1](0,0)))
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[m_T-1](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; {
//s.setZero(32); s[25] = -uList[m_T-1](0,0);
}
ODEBUG5(s); ODEBUG5(s);
return 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