sot-torque-control  1.6.1
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
position-controller.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2015, Andrea Del Prete, LAAS-CNRS
3  *
4  */
5 
6 #ifndef __sot_torque_control_position_controller_H__
7 #define __sot_torque_control_position_controller_H__
8 
9 /* --------------------------------------------------------------------- */
10 /* --- API ------------------------------------------------------------- */
11 /* --------------------------------------------------------------------- */
12 
13 #if defined(WIN32)
14 #if defined(position_controller_EXPORTS)
15 #define SOTPOSITIONCONTROLLER_EXPORT __declspec(dllexport)
16 #else
17 #define SOTPOSITIONCONTROLLER_EXPORT __declspec(dllimport)
18 #endif
19 #else
20 #define SOTPOSITIONCONTROLLER_EXPORT
21 #endif
22 
23 /* --------------------------------------------------------------------- */
24 /* --- INCLUDE --------------------------------------------------------- */
25 /* --------------------------------------------------------------------- */
26 
27 #include <map>
28 
29 #include <pinocchio/fwd.hpp>
30 
31 #include <boost/assign.hpp>
32 
33 #include <dynamic-graph/signal-helper.h>
34 #include <sot/core/matrix-geometry.hh>
35 #include <sot/core/robot-utils.hh>
36 
38 
39 namespace dynamicgraph {
40 namespace sot {
41 namespace torque_control {
42 
43 /* --------------------------------------------------------------------- */
44 /* --- CLASS ----------------------------------------------------------- */
45 /* --------------------------------------------------------------------- */
46 
47 class SOTPOSITIONCONTROLLER_EXPORT PositionController : public ::dynamicgraph::Entity {
49  DYNAMIC_GRAPH_ENTITY_DECL();
50 
51  public:
52  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
53 
54  /* --- CONSTRUCTOR ---- */
55  PositionController(const std::string& name);
56 
57  void init(const double& dt, const std::string& robotRef);
58 
59  void resetIntegral();
60 
61  /* --- SIGNALS --- */
62  DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector);
63  DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector);
64  DECLARE_SIGNAL_IN(qRef, dynamicgraph::Vector);
65  DECLARE_SIGNAL_IN(dqRef, dynamicgraph::Vector);
66  DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
67  DECLARE_SIGNAL_IN(Kd, dynamicgraph::Vector);
68  DECLARE_SIGNAL_IN(Ki, dynamicgraph::Vector);
69 
70  DECLARE_SIGNAL_OUT(pwmDes, dynamicgraph::Vector);
71  // DEBUG SIGNALS
72  DECLARE_SIGNAL_OUT(qError, dynamicgraph::Vector);
73 
74  /* --- COMMANDS --- */
75  /* --- ENTITY INHERITANCE --- */
76  virtual void display(std::ostream& os) const;
77 
78  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
79  logger_.stream(t) << ("[PositionController-" + name + "] " + msg) << '\n';
80  }
81 
82  protected:
83  RobotUtilShrPtr m_robot_util;
84  Eigen::VectorXd m_pwmDes;
86  double m_dt;
87 
89  Eigen::VectorXd m_e_integral;
90 
91  Eigen::VectorXd m_q, m_dq;
92 
93 }; // class PositionController
94 
95 } // namespace torque_control
96 } // namespace sot
97 } // namespace dynamicgraph
98 
99 #endif // #ifndef __sot_torque_control_position_controller_H__
SOTPOSITIONCONTROLLER_EXPORT
#define SOTPOSITIONCONTROLLER_EXPORT
Definition: position-controller.hh:20
dynamicgraph
to read text file
Definition: treeview.dox:22
dynamicgraph::sot::torque_control::PositionController::sendMsg
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
Definition: position-controller.hh:78
vector-conversions.hh
dynamicgraph::sot::torque_control::PositionController::m_e_integral
Eigen::VectorXd m_e_integral
control loop time period
Definition: position-controller.hh:89
torque_control
Definition: __init__.py:1
dynamicgraph::sot::torque_control::PositionController::m_q
Eigen::VectorXd m_q
Definition: position-controller.hh:91
dynamicgraph::sot::torque_control::EntityClassName
AdmittanceController EntityClassName
Definition: admittance-controller.cpp:44
dynamicgraph::sot::torque_control::PositionController::m_initSucceeded
bool m_initSucceeded
Definition: position-controller.hh:85
dynamicgraph::sot::torque_control::PositionController::m_robot_util
RobotUtilShrPtr m_robot_util
Definition: position-controller.hh:83
dynamicgraph::sot::torque_control::PositionController::m_pwmDes
Eigen::VectorXd m_pwmDes
Robot Util.
Definition: position-controller.hh:84
dynamicgraph::sot::torque_control::PositionController
Definition: position-controller.hh:47
dynamicgraph::sot::torque_control::PositionController::m_dt
double m_dt
true if the entity has been successfully initialized
Definition: position-controller.hh:86