6 #ifndef __sot_torque_control_current_controller_H__
7 #define __sot_torque_control_current_controller_H__
14 #if defined(__sot_torque_control_current_controller_H__)
15 #define SOTCURRENTCONTROLLER_EXPORT __declspec(dllexport)
17 #define SOTCURRENTCONTROLLER_EXPORT __declspec(dllimport)
20 #define SOTCURRENTCONTROLLER_EXPORT
28 #include "boost/assign.hpp"
30 #include <pinocchio/multibody/model.hpp>
31 #include <pinocchio/parsers/urdf.hpp>
33 #include <tsid/robots/robot-wrapper.hpp>
34 #include <dynamic-graph/signal-helper.h>
35 #include <sot/core/matrix-geometry.hh>
36 #include <sot/core/robot-utils.hh>
49 DYNAMIC_GRAPH_ENTITY_DECL();
59 void init(
const double& dt,
const std::string& robotRef,
const unsigned int& currentOffsetIters);
70 dynamicgraph::Vector);
75 dq, dynamicgraph::Vector);
79 dynamicgraph::Vector);
83 dynamicgraph::Vector);
86 i_max_dead_zone_compensation,
87 dynamicgraph::Vector);
89 dynamicgraph::Vector);
98 dynamicgraph::Vector);
104 void reset_integral();
107 virtual void display(std::ostream& os)
const;
109 void sendMsg(
const std::string& msg, MsgType t = MSG_TYPE_INFO,
const char* =
"",
int = 0) {
110 logger_.stream(t) << (
"[CurrentController-" + name +
"] " + msg) <<
'\n';
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
DECLARE_SIGNAL_OUT(i_errors, dynamicgraph::Vector)
dead-zone compensation current applied by the controller
bool m_is_first_iter
control loop time period
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
dynamicgraph::Vector m_dz_coeff
DECLARE_SIGNAL_IN(i_max, dynamicgraph::Vector)
proportional current feedback gain
DECLARE_SIGNAL_OUT(u_safe, dynamicgraph::Vector)
dynamicgraph::Vector m_i_offsets_real
RobotUtilShrPtr m_robot_util
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector)
real offset of the current sensors
DECLARE_SIGNAL_OUT(i_low_level, dynamicgraph::Vector)
current measurements after gain and offset compensation
dynamicgraph::Vector m_i_err_integr
DECLARE_SIGNAL_IN(i_sensor_offsets_low_level, dynamicgraph::Vector)
value of current tracking error at which deadzone is completely compensated
DECLARE_SIGNAL_IN(percentage_bemf_compensation, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(u_max, dynamicgraph::Vector)
max current allowed before stopping the controller (in Ampers)
DECLARE_SIGNAL_OUT(i_real, dynamicgraph::Vector)
same as u when everything is fine, 0 otherwise
dynamicgraph::Vector m_avg_i_err_neg
DECLARE_SIGNAL_IN(i_des, dynamicgraph::Vector)
dynamicgraph::Vector m_avg_i_err_pos
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)
Joint velocities; used to compensate for BEMF effect on low level current loop.
DECLARE_SIGNAL_IN(kp_current, dynamicgraph::Vector)
gains of current sensors
unsigned int m_currentOffsetIters
time to sleep at every iteration (to slow down simulation)
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)
max desired current allowed before stopping the controller (in Ampers)
bool m_emergency_stop_triggered
true if the entity has been successfully initialized
DECLARE_SIGNAL_IN(in_out_gain, dynamicgraph::Vector)
values at which to saturate the control (in bits)
DECLARE_SIGNAL_IN(i_sensor_offsets_real_in, dynamicgraph::Vector)
offset of the current sensors seen by the low level
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
AdmittanceController EntityClassName