ddp-actuator-solver.hh

Go to the documentation of this file.

 
#ifndef _SOT_DDP_ACTUATOR_SOLVER_H
#define _SOT_DDP_ACTUATOR_SOLVER_H
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
#if defined(WIN32)
#if defined(sot_ddp_actuator_EXPORTS)
#define SOTDDPACTUATORSOLVER_EXPORT __declspec(dllexport)
#else
#define SOTDDPACTUATORSOLVER_EXPORT __declspec(dllimport)
#endif
#else
#define SOTDDPACTUATORSOLVER_EXPORT
#endif
 
#include <dynamic-graph/signal-helper.h>
 
#include <ddp-actuator-solver/ddpsolver.hh>
#include <ddp-actuator-solver/temperature_control/costtemp.hh>
#include <ddp-actuator-solver/temperature_control/dctemp.hh>
#include <sot/core/causal-filter.hh>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/robot-utils.hh>
#include <tsid/utils/stop-watch.hpp>
 
namespace dynamicgraph {
namespace sot {
namespace torque_control {
 
#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_desSIN
 
#define ALL_OUTPUT_SIGNALS m_tauSOUT
 
class SOTDDPACTUATORSOLVER_EXPORT DdpActuatorSolver
    : public ::dynamicgraph::Entity {
  DYNAMIC_GRAPH_ENTITY_DECL();
 
 public: /* --- SIGNALS --- */
  DECLARE_SIGNAL_IN(pos_des, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(pos_motor_measure, dynamicgraph::Vector);
  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(tau_des, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(temp_measure, dynamicgraph::Vector);
  DECLARE_SIGNAL_OUT(tau, dynamicgraph::Vector);
 
 protected:
  double m_dt;
  double m_ambiant_temperature;
  DDPSolver<double, 5, 1>::stateVec_t m_xinit, m_xDes, m_x, m_zeroState;
  DDPSolver<double, 5, 1>::commandVec_t m_u;
  DCTemp m_model;
  CostTemp m_cost;
  DDPSolver<double, 5, 1> m_solver;
  unsigned int m_T;
  double m_stopCrit;
  unsigned int m_iterMax;
 
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  DdpActuatorSolver(const std::string &name);
  virtual void display(std::ostream &os) const;
 
 protected:
  void param_init(const double &timestep, const int &T, const int &nbItMax,
                  const double &stopCriteria);
};
}  // namespace torque_control
}  // namespace sot
}  // namespace dynamicgraph
#endif  // _SOT_DDP_ACTUATOR_SOLVER_H