position-controller.hh 3.5 KB
Newer Older
andreadelprete's avatar
andreadelprete committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
/*
 * Copyright 2015, Andrea Del Prete, LAAS-CNRS
 *
 */

#ifndef __sot_torque_control_position_controller_H__
#define __sot_torque_control_position_controller_H__

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

#if defined (WIN32)
#  if defined (position_controller_EXPORTS)
#    define SOTPOSITIONCONTROLLER_EXPORT __declspec(dllexport)
#  else
#    define SOTPOSITIONCONTROLLER_EXPORT __declspec(dllimport)
#  endif
#else
#  define SOTPOSITIONCONTROLLER_EXPORT
#endif


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

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

31
32
33
34
35
36
#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
37
38
39
40
41
42
43
44
45
46
47
48
49
namespace dynamicgraph {
  namespace sot {
    namespace torque_control {

      /* --------------------------------------------------------------------- */
      /* --- CLASS ----------------------------------------------------------- */
      /* --------------------------------------------------------------------- */

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

      public:
andreadelprete's avatar
andreadelprete committed
52
53
54
55
56
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        /* --- CONSTRUCTOR ---- */
        PositionController( const std::string & name );

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

        void resetIntegral();

        /* --- SIGNALS --- */
62
63
64
65
66
67
68
69
70
        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

        DECLARE_SIGNAL_OUT(pwmDes,      dynamicgraph::Vector);  /// Kp*e_q + Kd*de_q + Ki*int(e_q)
andreadelprete's avatar
andreadelprete committed
71
        // DEBUG SIGNALS
72
        DECLARE_SIGNAL_OUT(qError,      dynamicgraph::Vector);  /// qRef-q
andreadelprete's avatar
andreadelprete committed
73
74
75
76
77
78
79
80


        /* --- COMMANDS --- */
        /* --- ENTITY INHERITANCE --- */
        virtual void display( std::ostream& os ) const;

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

andreadelprete's avatar
andreadelprete committed
84
      protected:
85
	RobotUtilShrPtr       m_robot_util;        /// Robot Util
andreadelprete's avatar
andreadelprete committed
86
87
88
89
90
91
92
93
94
95
        Eigen::VectorXd   m_pwmDes;
        bool              m_initSucceeded;    /// true if the entity has been successfully initialized
        double            m_dt;               /// control loop time period

        /// Integral of the joint tracking errors
        Eigen::VectorXd   m_e_integral;

        Eigen::VectorXd m_q, m_dq;

      }; // class PositionController
Guilhem Saurel's avatar
Guilhem Saurel committed
96

andreadelprete's avatar
andreadelprete committed
97
98
99
100
101
102
103
    }    // namespace torque_control
  }      // namespace sot
}        // namespace dynamicgraph



#endif // #ifndef __sot_torque_control_position_controller_H__