ddp-actuator-solver.hh 2.91 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

#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

18

19
#include <tsid/utils/stop-watch.hpp>
20
21
22
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/robot-utils.hh>
23
#include <sot/torque_control/utils/causal-filter.hh>
24

25
26
27
28
29
30
31
32
33
34
35
36
#include <ddp-actuator-solver/ddpsolver.hh>
#include <ddp-actuator-solver/ddpsolver.hh>

#include <ddp-actuator-solver/examples/dctemp.hh>
#include <ddp-actuator-solver/examples/costtemp.hh>

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		 \
37
      << m_tau_measureSIN << m_temp_measureSIN << m_tau_desSIN
38
39
40
41
42
43
44
45
46
47
48
49
50
51

#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);
52
53
          DECLARE_SIGNAL_IN(tau_des,            dynamicgraph::Vector);
          DECLARE_SIGNAL_IN(temp_measure,       dynamicgraph::Vector);
54
55
56
57
58
	  DECLARE_SIGNAL_OUT(tau,               dynamicgraph::Vector);

	protected:
	  double m_dt;
	  double m_ambiant_temperature;
59
	  DDPSolver<double,5,1>::stateVec_t m_xinit,m_xDes,m_x,m_zeroState;
60
61
62
63
64
65
66
67
68
69
70
71
72
73
	  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
	    
	    /** Constructor */
	    DdpActuatorSolver(const std::string &name);
	  virtual void display(std::ostream &os) const;

74
	protected:
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
	  /** Initialize the DDP.
	   * @param timestep Control period (in seconds).
	   * @param T  Size of the preview window (in nb of timestep).
	   * @param nbItMax Maximum number of iterations.
	   * @param stopCriteria The value of the stopping criteria.
	   */
	  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