position-controller.hh 3.15 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
#endif

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

#include <map>
28
29
30
31

#include <pinocchio/fwd.hpp>

#include <boost/assign.hpp>
andreadelprete's avatar
andreadelprete committed
32

33
34
35
36
37
38
#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
39
namespace dynamicgraph {
Guilhem Saurel's avatar
Guilhem Saurel committed
40
41
namespace sot {
namespace torque_control {
andreadelprete's avatar
andreadelprete committed
42

Guilhem Saurel's avatar
Guilhem Saurel committed
43
44
45
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
andreadelprete's avatar
andreadelprete committed
46

Guilhem Saurel's avatar
Guilhem Saurel committed
47
48
49
class SOTPOSITIONCONTROLLER_EXPORT PositionController : public ::dynamicgraph::Entity {
  typedef PositionController EntityClassName;
  DYNAMIC_GRAPH_ENTITY_DECL();
andreadelprete's avatar
andreadelprete committed
50

Guilhem Saurel's avatar
Guilhem Saurel committed
51
52
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
andreadelprete's avatar
andreadelprete committed
53

Guilhem Saurel's avatar
Guilhem Saurel committed
54
55
  /* --- CONSTRUCTOR ---- */
  PositionController(const std::string& name);
56

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

Guilhem Saurel's avatar
Guilhem Saurel committed
59
  void resetIntegral();
andreadelprete's avatar
andreadelprete committed
60

Guilhem Saurel's avatar
Guilhem Saurel committed
61
62
63
64
65
66
67
68
  /* --- 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
69

Guilhem Saurel's avatar
Guilhem Saurel committed
70
71
72
  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
73

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

Guilhem Saurel's avatar
Guilhem Saurel committed
78
79
  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
    logger_.stream(t) << ("[PositionController-" + name + "] " + msg) << '\n';
Guilhem Saurel's avatar
Guilhem Saurel committed
80
  }
andreadelprete's avatar
andreadelprete committed
81

Guilhem Saurel's avatar
Guilhem Saurel committed
82
83
84
85
86
 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
87

Guilhem Saurel's avatar
Guilhem Saurel committed
88
89
  /// Integral of the joint tracking errors
  Eigen::VectorXd m_e_integral;
Guilhem Saurel's avatar
Guilhem Saurel committed
90

Guilhem Saurel's avatar
Guilhem Saurel committed
91
  Eigen::VectorXd m_q, m_dq;
andreadelprete's avatar
andreadelprete committed
92

Guilhem Saurel's avatar
Guilhem Saurel committed
93
};  // class PositionController
andreadelprete's avatar
andreadelprete committed
94

Guilhem Saurel's avatar
Guilhem Saurel committed
95
96
97
}  // namespace torque_control
}  // namespace sot
}  // namespace dynamicgraph
andreadelprete's avatar
andreadelprete committed
98

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