control-pd.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2010,
3  * François Bleibel,
4  * Olivier Stasse,
5  *
6  * CNRS/AIST
7  *
8  */
9 
10 #ifndef __SOT_Control_PD_HH__
11 #define __SOT_Control_PD_HH__
12 
13 /* --------------------------------------------------------------------- */
14 /* --- INCLUDE --------------------------------------------------------- */
15 /* --------------------------------------------------------------------- */
16 
17 /* Matrix */
18 #include <dynamic-graph/linear-algebra.h>
19 namespace dg = dynamicgraph;
20 
21 /* SOT */
22 #include <dynamic-graph/entity.h>
23 #include <dynamic-graph/signal-ptr.h>
24 #include <dynamic-graph/signal-time-dependent.h>
25 
26 /* --------------------------------------------------------------------- */
27 /* --- API ------------------------------------------------------------- */
28 /* --------------------------------------------------------------------- */
29 
30 #if defined(WIN32)
31 #if defined(control_pd_EXPORTS)
32 #define ControlPD_EXPORT __declspec(dllexport)
33 #else
34 #define ControlPD_EXPORT __declspec(dllimport)
35 #endif
36 #else
37 #define ControlPD_EXPORT
38 #endif
39 
40 namespace dynamicgraph {
41 namespace sot {
42 
43 /* --------------------------------------------------------------------- */
44 /* --- CLASS ----------------------------------------------------------- */
45 /* --------------------------------------------------------------------- */
46 
47 class ControlPD_EXPORT ControlPD : public Entity {
48 
49 public: /* --- CONSTRUCTOR ---- */
50  ControlPD(const std::string &name);
51 
52 public: /* --- INIT --- */
53  void init(const double &step);
54 
55 public: /* --- CONSTANTS --- */
56  /* Default values. */
57  static const double TIME_STEP_DEFAULT; // = 0.001
58 
59 public: /* --- ENTITY INHERITANCE --- */
60  static const std::string CLASS_NAME;
61  virtual void display(std::ostream &os) const;
62  virtual const std::string &getClassName(void) const { return CLASS_NAME; }
63 
64 protected:
65  /* Parameters of the torque-control function:
66  * tau = kp * (qd-q) + kd* (dqd-dq) */
67  double TimeStep;
68 
69 public: /* --- SIGNALS --- */
70  SignalPtr<dg::Vector, int> KpSIN;
71  SignalPtr<dg::Vector, int> KdSIN;
72  SignalPtr<dg::Vector, int> positionSIN;
73  SignalPtr<dg::Vector, int> desiredpositionSIN;
74  SignalPtr<dg::Vector, int> velocitySIN;
75  SignalPtr<dg::Vector, int> desiredvelocitySIN;
76  SignalTimeDependent<dg::Vector, int> controlSOUT;
77  SignalTimeDependent<dg::Vector, int> positionErrorSOUT;
78  SignalTimeDependent<dg::Vector, int> velocityErrorSOUT;
79 
80 protected:
81  dg::Vector &computeControl(dg::Vector &tau, int t);
82  dg::Vector position_error_;
83  dg::Vector velocity_error_;
84  dg::Vector &getPositionError(dg::Vector &position_error, int t);
85  dg::Vector &getVelocityError(dg::Vector &velocity_error, int t);
86 };
87 
88 } // namespace sot
89 } // namespace dynamicgraph
90 
91 #endif // #ifndef __SOT_Control_PD_HH__
dynamicgraph::sot::ControlPD::positionErrorSOUT
SignalTimeDependent< dg::Vector, int > positionErrorSOUT
Definition: control-pd.hh:77
dynamicgraph
Definition: abstract-sot-external-interface.hh:17
dynamicgraph::sot::ControlPD::velocityErrorSOUT
SignalTimeDependent< dg::Vector, int > velocityErrorSOUT
Definition: control-pd.hh:78
dynamicgraph::sot::ControlPD::getClassName
virtual const std::string & getClassName(void) const
Definition: control-pd.hh:62
dynamicgraph::sot::ControlPD::velocitySIN
SignalPtr< dg::Vector, int > velocitySIN
Definition: control-pd.hh:74
dynamicgraph::sot::ControlPD::KpSIN
SignalPtr< dg::Vector, int > KpSIN
Definition: control-pd.hh:70
dynamicgraph::sot::ControlPD::TimeStep
double TimeStep
Definition: control-pd.hh:67
dynamicgraph::sot::ControlPD::controlSOUT
SignalTimeDependent< dg::Vector, int > controlSOUT
Definition: control-pd.hh:76
dynamicgraph::sot::ControlPD::KdSIN
SignalPtr< dg::Vector, int > KdSIN
Definition: control-pd.hh:71
dynamicgraph::sot::ControlPD
Definition: control-pd.hh:47
dynamicgraph::sot::ControlPD::velocity_error_
dg::Vector velocity_error_
Definition: control-pd.hh:83
dynamicgraph::sot::ControlPD::position_error_
dg::Vector position_error_
Definition: control-pd.hh:82
dynamicgraph::sot::ControlPD::TIME_STEP_DEFAULT
static const double TIME_STEP_DEFAULT
Definition: control-pd.hh:57
ControlPD_EXPORT
#define ControlPD_EXPORT
Definition: control-pd.hh:37
dynamicgraph::sot::ControlPD::desiredpositionSIN
SignalPtr< dg::Vector, int > desiredpositionSIN
Definition: control-pd.hh:73
dynamicgraph::sot::ControlPD::positionSIN
SignalPtr< dg::Vector, int > positionSIN
Definition: control-pd.hh:72
dynamicgraph::sot::ControlPD::CLASS_NAME
static const std::string CLASS_NAME
Definition: control-pd.hh:60
dynamicgraph::sot::ControlPD::desiredvelocitySIN
SignalPtr< dg::Vector, int > desiredvelocitySIN
Definition: control-pd.hh:75