position-controller.hh 3.13 KB
Newer Older
andreadelprete's avatar
andreadelprete committed
1
2
3
4
5
6
7
8
9
10
11
12
/*
 * Copyright 2015, Andrea Del Prete, LAAS-CNRS
 *
 */

#ifndef __sot_torque_control_position_controller_H__
#define __sot_torque_control_position_controller_H__

/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

Guilhem Saurel's avatar
Guilhem Saurel committed
13
14
15
#if defined(WIN32)
#if defined(position_controller_EXPORTS)
#define SOTPOSITIONCONTROLLER_EXPORT __declspec(dllexport)
andreadelprete's avatar
andreadelprete committed
16
#else
Guilhem Saurel's avatar
Guilhem Saurel committed
17
18
19
20
#define SOTPOSITIONCONTROLLER_EXPORT __declspec(dllimport)
#endif
#else
#define SOTPOSITIONCONTROLLER_EXPORT
andreadelprete's avatar
andreadelprete committed
21
22
23
24
25
26
27
28
29
#endif

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#include <map>
#include "boost/assign.hpp"

30
31
32
33
34
35
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/robot-utils.hh>

#include <sot/torque_control/utils/vector-conversions.hh>

andreadelprete's avatar
andreadelprete committed
36
namespace dynamicgraph {
Guilhem Saurel's avatar
Guilhem Saurel committed
37
38
namespace sot {
namespace torque_control {
andreadelprete's avatar
andreadelprete committed
39

Guilhem Saurel's avatar
Guilhem Saurel committed
40
41
42
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
andreadelprete's avatar
andreadelprete committed
43

Guilhem Saurel's avatar
Guilhem Saurel committed
44
45
46
class SOTPOSITIONCONTROLLER_EXPORT PositionController : public ::dynamicgraph::Entity {
  typedef PositionController EntityClassName;
  DYNAMIC_GRAPH_ENTITY_DECL();
andreadelprete's avatar
andreadelprete committed
47

Guilhem Saurel's avatar
Guilhem Saurel committed
48
49
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
andreadelprete's avatar
andreadelprete committed
50

Guilhem Saurel's avatar
Guilhem Saurel committed
51
52
  /* --- CONSTRUCTOR ---- */
  PositionController(const std::string& name);
53

Guilhem Saurel's avatar
Guilhem Saurel committed
54
  void init(const double& dt, const std::string& robotRef);
andreadelprete's avatar
andreadelprete committed
55

Guilhem Saurel's avatar
Guilhem Saurel committed
56
  void resetIntegral();
andreadelprete's avatar
andreadelprete committed
57

Guilhem Saurel's avatar
Guilhem Saurel committed
58
59
60
61
62
63
64
65
  /* --- SIGNALS --- */
  DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(qRef, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(dqRef, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);  /// joint proportional gains
  DECLARE_SIGNAL_IN(Kd, dynamicgraph::Vector);  /// joint derivative gains
  DECLARE_SIGNAL_IN(Ki, dynamicgraph::Vector);  /// joint integral gains
andreadelprete's avatar
andreadelprete committed
66

Guilhem Saurel's avatar
Guilhem Saurel committed
67
68
69
  DECLARE_SIGNAL_OUT(pwmDes, dynamicgraph::Vector);  /// Kp*e_q + Kd*de_q + Ki*int(e_q)
  // DEBUG SIGNALS
  DECLARE_SIGNAL_OUT(qError, dynamicgraph::Vector);  /// qRef-q
Guilhem Saurel's avatar
Guilhem Saurel committed
70

Guilhem Saurel's avatar
Guilhem Saurel committed
71
72
73
  /* --- COMMANDS --- */
  /* --- ENTITY INHERITANCE --- */
  virtual void display(std::ostream& os) const;
andreadelprete's avatar
andreadelprete committed
74

Guilhem Saurel's avatar
Guilhem Saurel committed
75
76
77
  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* file = "", int line = 0) {
    Entity::sendMsg("[PositionController-" + name + "] " + msg, t, file, line);
  }
andreadelprete's avatar
andreadelprete committed
78

Guilhem Saurel's avatar
Guilhem Saurel committed
79
80
81
82
83
 protected:
  RobotUtilShrPtr m_robot_util;  /// Robot Util
  Eigen::VectorXd m_pwmDes;
  bool m_initSucceeded;  /// true if the entity has been successfully initialized
  double m_dt;           /// control loop time period
andreadelprete's avatar
andreadelprete committed
84

Guilhem Saurel's avatar
Guilhem Saurel committed
85
86
  /// Integral of the joint tracking errors
  Eigen::VectorXd m_e_integral;
Guilhem Saurel's avatar
Guilhem Saurel committed
87

Guilhem Saurel's avatar
Guilhem Saurel committed
88
  Eigen::VectorXd m_q, m_dq;
andreadelprete's avatar
andreadelprete committed
89

Guilhem Saurel's avatar
Guilhem Saurel committed
90
};  // class PositionController
andreadelprete's avatar
andreadelprete committed
91

Guilhem Saurel's avatar
Guilhem Saurel committed
92
93
94
}  // namespace torque_control
}  // namespace sot
}  // namespace dynamicgraph
andreadelprete's avatar
andreadelprete committed
95

Guilhem Saurel's avatar
Guilhem Saurel committed
96
#endif  // #ifndef __sot_torque_control_position_controller_H__