sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
current-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_current_controller_H__
7 #define __sot_torque_control_current_controller_H__
8 
9 /* --------------------------------------------------------------------- */
10 /* --- API ------------------------------------------------------------- */
11 /* --------------------------------------------------------------------- */
12 
13 #if defined(WIN32)
14 #if defined(__sot_torque_control_current_controller_H__)
15 #define SOTCURRENTCONTROLLER_EXPORT __declspec(dllexport)
16 #else
17 #define SOTCURRENTCONTROLLER_EXPORT __declspec(dllimport)
18 #endif
19 #else
20 #define SOTCURRENTCONTROLLER_EXPORT
21 #endif
22 
23 /* --------------------------------------------------------------------- */
24 /* --- INCLUDE --------------------------------------------------------- */
25 /* --------------------------------------------------------------------- */
26 
27 #include <pinocchio/fwd.hpp>
28 
29 // include pinocchio first
30 
31 #include <dynamic-graph/signal-helper.h>
32 
33 #include <map>
34 #include <pinocchio/multibody/model.hpp>
35 #include <pinocchio/parsers/urdf.hpp>
36 #include <sot/core/matrix-geometry.hh>
37 #include <sot/core/robot-utils.hh>
39 #include <tsid/robots/robot-wrapper.hpp>
40 
41 #include "boost/assign.hpp"
42 
43 namespace dynamicgraph {
44 namespace sot {
45 namespace torque_control {
46 
47 /* --------------------------------------------------------------------- */
48 /* --- CLASS ----------------------------------------------------------- */
49 /* --------------------------------------------------------------------- */
50 
52  : public ::dynamicgraph::Entity {
54  DYNAMIC_GRAPH_ENTITY_DECL();
55 
56  public:
57  /* --- CONSTRUCTOR ---- */
58  CurrentController(const std::string& name);
59 
65  void init(const double& dt, const std::string& robotRef,
66  const unsigned int& currentOffsetIters);
67 
68  /* --- SIGNALS --- */
69  DECLARE_SIGNAL_IN(i_des, dynamicgraph::Vector);
70  DECLARE_SIGNAL_IN(i_measured, dynamicgraph::Vector);
71  DECLARE_SIGNAL_IN(i_sens_gains,
72  dynamicgraph::Vector);
73  DECLARE_SIGNAL_IN(
74  kp_current, dynamicgraph::Vector);
75  DECLARE_SIGNAL_IN(
76  ki_current, dynamicgraph::Vector);
77 
78  DECLARE_SIGNAL_IN(
79  i_max, dynamicgraph::Vector);
80  DECLARE_SIGNAL_IN(
82  u_max,
83  dynamicgraph::Vector);
84  DECLARE_SIGNAL_IN(u_saturation,
86  dynamicgraph::Vector);
87 
89  DECLARE_SIGNAL_IN(
90  in_out_gain,
91  dynamicgraph::Vector);
92  DECLARE_SIGNAL_IN(
93  dq, dynamicgraph::Vector);
94  DECLARE_SIGNAL_IN(
96  bemf_factor,
97  dynamicgraph::Vector);
98  DECLARE_SIGNAL_IN(
101  percentage_bemf_compensation,
102  dynamicgraph::Vector);
103  DECLARE_SIGNAL_IN(
105  dead_zone_offsets,
106  dynamicgraph::Vector);
107  DECLARE_SIGNAL_IN(
108  percentage_dead_zone_compensation,
109  dynamicgraph::Vector);
110  DECLARE_SIGNAL_IN(
113  i_max_dead_zone_compensation,
114  dynamicgraph::Vector);
115  DECLARE_SIGNAL_IN(i_sensor_offsets_low_level,
117  dynamicgraph::Vector);
118  DECLARE_SIGNAL_IN(
120  i_sensor_offsets_real_in,
121  dynamicgraph::Vector);
122 
123  DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
124  DECLARE_SIGNAL_OUT(
125  u_safe,
126  dynamicgraph::Vector);
127  DECLARE_SIGNAL_OUT(i_real,
128  dynamicgraph::Vector);
129  DECLARE_SIGNAL_OUT(
131  i_low_level,
132  dynamicgraph::Vector);
133  DECLARE_SIGNAL_OUT(
134  i_sensor_offsets_real_out,
135  dynamicgraph::Vector);
136  DECLARE_SIGNAL_OUT(dead_zone_compensation,
137  dynamicgraph::Vector);
138  DECLARE_SIGNAL_OUT(i_errors,
140  dynamicgraph::Vector);
141  DECLARE_SIGNAL_OUT(
142  i_errors_ll_wo_bemf,
143  dynamicgraph::Vector);
144 
145  /* --- COMMANDS --- */
146 
147  void reset_integral();
148 
149  /* --- ENTITY INHERITANCE --- */
150  virtual void display(std::ostream& os) const;
151 
152  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO,
153  const char* = "", int = 0) {
154  logger_.stream(t) << ("[CurrentController-" + name + "] " + msg) << '\n';
155  }
156 
157  protected:
158  RobotUtilShrPtr m_robot_util;
159  bool
162  double m_dt;
164  int m_iter;
165  double m_sleep_time;
166 
168  unsigned int m_currentOffsetIters;
169  dynamicgraph::Vector m_i_offsets_real;
170  dynamicgraph::Vector m_i_err_integr;
171 
172  dynamicgraph::Vector m_dz_coeff;
173 
174  dynamicgraph::Vector m_avg_i_err_pos;
175  dynamicgraph::Vector m_avg_i_err_neg;
176 
177 }; // class CurrentController
178 
179 } // namespace torque_control
180 } // namespace sot
181 } // namespace dynamicgraph
182 
183 #endif // #ifndef __sot_torque_control_current_controller_H__
dynamicgraph::sot::torque_control::CurrentController::sendMsg
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
Definition: current-controller.hh:152
dynamicgraph::sot::torque_control::CurrentController::m_dz_coeff
dynamicgraph::Vector m_dz_coeff
Definition: current-controller.hh:172
dynamicgraph::sot::torque_control::CurrentController::m_initSucceeded
bool m_initSucceeded
Definition: current-controller.hh:160
dynamicgraph::sot::torque_control::CurrentController::m_currentOffsetIters
unsigned int m_currentOffsetIters
Definition: current-controller.hh:168
dynamicgraph
to read text file
Definition: treeview.dox:22
dynamicgraph::sot::torque_control::CurrentController::m_i_offsets_real
dynamicgraph::Vector m_i_offsets_real
Definition: current-controller.hh:169
vector-conversions.hh
dynamicgraph::sot::torque_control::CurrentController::m_robot_util
RobotUtilShrPtr m_robot_util
Definition: current-controller.hh:158
torque_control
Definition: __init__.py:1
SOTCURRENTCONTROLLER_EXPORT
#define SOTCURRENTCONTROLLER_EXPORT
Definition: current-controller.hh:20
dynamicgraph::sot::torque_control::CurrentController::m_avg_i_err_neg
dynamicgraph::Vector m_avg_i_err_neg
Definition: current-controller.hh:175
dynamicgraph::sot::torque_control::CurrentController::m_is_first_iter
bool m_is_first_iter
control loop time period
Definition: current-controller.hh:163
dynamicgraph::sot::torque_control::CurrentController::m_emergency_stop_triggered
bool m_emergency_stop_triggered
true if the entity has been successfully initialized
Definition: current-controller.hh:161
dynamicgraph::sot::torque_control::CurrentController::m_sleep_time
double m_sleep_time
Definition: current-controller.hh:165
dynamicgraph::sot::torque_control::CurrentController::m_dt
double m_dt
Definition: current-controller.hh:162
dynamicgraph::sot::torque_control::CurrentController::m_avg_i_err_pos
dynamicgraph::Vector m_avg_i_err_pos
Definition: current-controller.hh:174
dynamicgraph::sot::torque_control::EntityClassName
AdmittanceController EntityClassName
Definition: admittance-controller.cpp:51
dynamicgraph::sot::torque_control::CurrentController
Definition: current-controller.hh:51
dynamicgraph::sot::torque_control::CurrentController::m_iter
int m_iter
true at the first iteration, false otherwise
Definition: current-controller.hh:164
dynamicgraph::sot::torque_control::CurrentController::m_i_err_integr
dynamicgraph::Vector m_i_err_integr
Definition: current-controller.hh:170