sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
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
43namespace dynamicgraph {
44namespace sot {
45namespace 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);
74 kp_current, dynamicgraph::Vector);
76 ki_current, dynamicgraph::Vector);
77
79 i_max, dynamicgraph::Vector);
82 u_max,
83 dynamicgraph::Vector);
85 DECLARE_SIGNAL_IN(u_saturation,
86 dynamicgraph::Vector);
88
90 in_out_gain,
91 dynamicgraph::Vector);
93 dq, dynamicgraph::Vector);
96 bemf_factor,
97 dynamicgraph::Vector);
101 percentage_bemf_compensation,
102 dynamicgraph::Vector);
105 dead_zone_offsets,
106 dynamicgraph::Vector);
108 percentage_dead_zone_compensation,
109 dynamicgraph::Vector);
113 i_max_dead_zone_compensation,
114 dynamicgraph::Vector);
116 DECLARE_SIGNAL_IN(i_sensor_offsets_low_level,
117 dynamicgraph::Vector);
120 i_sensor_offsets_real_in,
121 dynamicgraph::Vector);
122
123 DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
125 u_safe,
126 dynamicgraph::Vector);
128 dynamicgraph::Vector);
131 i_low_level,
132 dynamicgraph::Vector);
134 i_sensor_offsets_real_out,
135 dynamicgraph::Vector);
136 DECLARE_SIGNAL_OUT(dead_zone_compensation,
137 dynamicgraph::Vector);
140 dynamicgraph::Vector);
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;
167
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__
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
DECLARE_SIGNAL_OUT(i_errors, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(i_max_dead_zone_compensation, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(percentage_dead_zone_compensation, dynamicgraph::Vector)
current control dead zone offsets
DECLARE_SIGNAL_OUT(i_errors_ll_wo_bemf, dynamicgraph::Vector)
current tracking error
DECLARE_SIGNAL_IN(i_max, dynamicgraph::Vector)
proportional current feedback gain
DECLARE_SIGNAL_OUT(u_safe, dynamicgraph::Vector)
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector)
real offset of the current sensors
DECLARE_SIGNAL_OUT(i_low_level, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(i_sensor_offsets_low_level, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(percentage_bemf_compensation, dynamicgraph::Vector)
DECLARE_SIGNAL_OUT(i_real, dynamicgraph::Vector)
same as u when everything is fine, 0 otherwise
int m_iter
true at the first iteration, false otherwise
DECLARE_SIGNAL_IN(ki_current, dynamicgraph::Vector)
proportional current feedback gain
DECLARE_SIGNAL_IN(bemf_factor, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(kp_current, dynamicgraph::Vector)
gains of current sensors
DECLARE_SIGNAL_OUT(i_sensor_offsets_real_out, dynamicgraph::Vector)
current measurements as seen by low-level ctrl
DECLARE_SIGNAL_OUT(dead_zone_compensation, dynamicgraph::Vector)
real offset of the current sensors
DECLARE_SIGNAL_IN(u_saturation, dynamicgraph::Vector)
bool m_emergency_stop_triggered
true if the entity has been successfully initialized
DECLARE_SIGNAL_IN(in_out_gain, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(i_sensor_offsets_real_in, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(dead_zone_offsets, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(i_sens_gains, dynamicgraph::Vector)
motor currents
DECLARE_SIGNAL_IN(i_measured, dynamicgraph::Vector)
desired motor currents
DECLARE_SIGNAL_IN(dq, dynamicgraph::Vector)
gain from input to output control values
#define SOTCURRENTCONTROLLER_EXPORT
to read text file
Definition treeview.dox:22