sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
joint-torque-controller.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2015, Andrea Del Prete, LAAS-CNRS
3  *
4  */
5 
6 #include <dynamic-graph/factory.h>
7 
8 #include <Eigen/Dense>
9 #include <sot/core/debug.hh>
10 #include <sot/torque_control/commands-helper.hh>
11 #include <sot/torque_control/joint-torque-controller.hh>
12 
13 namespace dynamicgraph {
14 namespace sot {
15 namespace torque_control {
16 
17 #define MODEL_INPUT_SIGNALS \
18  m_motorParameterKt_pSIN << m_motorParameterKt_nSIN \
19  << m_motorParameterKf_pSIN \
20  << m_motorParameterKf_nSIN \
21  << m_motorParameterKv_pSIN \
22  << m_motorParameterKv_nSIN \
23  << m_motorParameterKa_pSIN \
24  << m_motorParameterKa_nSIN << m_polySignDqSIN
25 
26 #define ESTIMATOR_INPUT_SIGNALS \
27  m_jointsPositionsSIN << m_jointsVelocitiesSIN << m_jointsAccelerationsSIN \
28  << m_jointsTorquesSIN << m_jointsTorquesDerivativeSIN
29 
30 #define TORQUE_INTEGRAL_INPUT_SIGNALS \
31  m_KiTorqueSIN << m_torque_integral_saturationSIN
32 #define TORQUE_CONTROL_INPUT_SIGNALS \
33  m_jointsTorquesDesiredSIN << m_KpTorqueSIN << m_KdTorqueSIN \
34  << m_coulomb_friction_compensation_percentageSIN
35 #define VEL_CONTROL_INPUT_SIGNALS m_dq_desSIN << m_KdVelSIN << m_KiVelSIN
36 
37 #define ALL_INPUT_SIGNALS \
38  ESTIMATOR_INPUT_SIGNALS << TORQUE_INTEGRAL_INPUT_SIGNALS \
39  << TORQUE_CONTROL_INPUT_SIGNALS \
40  << VEL_CONTROL_INPUT_SIGNALS << MODEL_INPUT_SIGNALS
41 #define ALL_OUTPUT_SIGNALS \
42  m_uSOUT << m_torque_error_integralSOUT << m_smoothSignDqSOUT
43 
44 namespace dynamicgraph = ::dynamicgraph;
45 using namespace dynamicgraph;
46 using namespace dynamicgraph::command;
47 using namespace std;
48 using namespace Eigen;
49 
52 typedef JointTorqueController EntityClassName;
53 
54 /* --- DG FACTORY ------------------------------------------------------- */
55 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(JointTorqueController,
56  "JointTorqueController");
57 
58 /* --- CONSTRUCTION ----------------------------------------------------- */
59 /* --- CONSTRUCTION ----------------------------------------------------- */
60 /* --- CONSTRUCTION ----------------------------------------------------- */
61 JointTorqueController::JointTorqueController(const std::string& name)
62  : Entity(name),
63  CONSTRUCT_SIGNAL_IN(jointsPositions, dynamicgraph::Vector),
64  CONSTRUCT_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector),
65  CONSTRUCT_SIGNAL_IN(jointsAccelerations, dynamicgraph::Vector),
66  CONSTRUCT_SIGNAL_IN(jointsTorques, dynamicgraph::Vector),
67  CONSTRUCT_SIGNAL_IN(jointsTorquesDerivative, dynamicgraph::Vector),
68  CONSTRUCT_SIGNAL_IN(jointsTorquesDesired, dynamicgraph::Vector),
69  CONSTRUCT_SIGNAL_IN(dq_des, dynamicgraph::Vector),
70  CONSTRUCT_SIGNAL_IN(KpTorque,
71  dynamicgraph::Vector) // proportional gain for torque
72  // feedback controller
73  ,
74  CONSTRUCT_SIGNAL_IN(
75  KiTorque,
76  dynamicgraph::Vector) // integral gain for torque feedback controller
77  ,
78  CONSTRUCT_SIGNAL_IN(KdTorque,
79  dynamicgraph::Vector) // derivative gain for torque
80  // feedback controller
81  ,
82  CONSTRUCT_SIGNAL_IN(
83  KdVel, dynamicgraph::Vector) // derivative gain for velocity feedback
84  ,
85  CONSTRUCT_SIGNAL_IN(
86  KiVel, dynamicgraph::Vector) // integral gain for velocity feedback
87  ,
88  CONSTRUCT_SIGNAL_IN(torque_integral_saturation, dynamicgraph::Vector),
89  CONSTRUCT_SIGNAL_IN(coulomb_friction_compensation_percentage,
90  dynamicgraph::Vector),
91  CONSTRUCT_SIGNAL_IN(motorParameterKt_p, dynamicgraph::Vector),
92  CONSTRUCT_SIGNAL_IN(motorParameterKt_n, dynamicgraph::Vector),
93  CONSTRUCT_SIGNAL_IN(motorParameterKf_p, dynamicgraph::Vector),
94  CONSTRUCT_SIGNAL_IN(motorParameterKf_n, dynamicgraph::Vector),
95  CONSTRUCT_SIGNAL_IN(motorParameterKv_p, dynamicgraph::Vector),
96  CONSTRUCT_SIGNAL_IN(motorParameterKv_n, dynamicgraph::Vector),
97  CONSTRUCT_SIGNAL_IN(motorParameterKa_p, dynamicgraph::Vector),
98  CONSTRUCT_SIGNAL_IN(motorParameterKa_n, dynamicgraph::Vector),
99  CONSTRUCT_SIGNAL_IN(polySignDq, dynamicgraph::Vector),
100  CONSTRUCT_SIGNAL_OUT(
101  u, dynamicgraph::Vector,
102  ESTIMATOR_INPUT_SIGNALS
103  << TORQUE_CONTROL_INPUT_SIGNALS << VEL_CONTROL_INPUT_SIGNALS
104  << MODEL_INPUT_SIGNALS << m_torque_error_integralSOUT),
105  CONSTRUCT_SIGNAL_OUT(smoothSignDq, dynamicgraph::Vector,
106  m_jointsVelocitiesSIN),
107  CONSTRUCT_SIGNAL_OUT(torque_error_integral, dynamicgraph::Vector,
108  m_jointsTorquesSIN
109  << m_jointsTorquesDesiredSIN
110  << TORQUE_INTEGRAL_INPUT_SIGNALS) {
111  Entity::signalRegistration(ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS);
112 
113  /* Commands. */
114  addCommand("getTimestep",
115  makeDirectGetter(*this, &m_dt,
116  docDirectGetter("Control timestep", "double")));
117 
118  addCommand("init",
119  makeCommandVoid2(*this, &JointTorqueController::init,
120  docCommandVoid2("Initialize the controller.",
121  "Control timestep [s].",
122  "Robot reference (string)")));
123  addCommand("reset_integral",
124  makeCommandVoid0(*this, &JointTorqueController::reset_integral,
125  docCommandVoid0("Reset the integral error.")));
126 }
127 
128 /* --- COMMANDS ---------------------------------------------------------- */
129 /* --- COMMANDS ---------------------------------------------------------- */
130 /* --- COMMANDS ---------------------------------------------------------- */
131 void JointTorqueController::init(const double& timestep,
132  const std::string& robot_ref) {
133  assert(timestep > 0.0 && "Timestep should be > 0");
134  if (!m_jointsVelocitiesSIN.isPlugged())
135  return SEND_MSG("Init failed: signal jointsVelocities is not plugged",
136  MSG_TYPE_ERROR);
137  if (!m_jointsTorquesSIN.isPlugged())
138  return SEND_MSG("Init failed: signal m_jointsTorquesSIN is not plugged",
139  MSG_TYPE_ERROR);
140  if (!m_jointsTorquesDesiredSIN.isPlugged())
141  return SEND_MSG(
142  "Init failed: signal m_jointsTorquesDesiredSIN is not plugged",
143  MSG_TYPE_ERROR);
144  if (!m_KpTorqueSIN.isPlugged())
145  return SEND_MSG("Init failed: signal m_KpTorqueSIN is not plugged",
146  MSG_TYPE_ERROR);
147  if (!m_KiTorqueSIN.isPlugged())
148  return SEND_MSG("Init failed: signal m_KiTorqueSIN is not plugged",
149  MSG_TYPE_ERROR);
150 
151  /* Retrieve m_robot_util informations */
152  std::string localName(robot_ref);
153  if (isNameInRobotUtil(localName)) {
154  m_robot_util = getRobotUtil(localName);
155  } else {
156  SEND_MSG("You should have an entity controller manager initialized before",
157  MSG_TYPE_ERROR);
158  return;
159  }
160 
161  m_dt = timestep;
162  m_tau_star.setZero(m_robot_util->m_nbJoints);
163  m_current_des.setZero(m_robot_util->m_nbJoints);
164  m_tauErrIntegral.setZero(m_robot_util->m_nbJoints);
165  // m_dqDesIntegral.setZero(m_robot_util->m_nbJoints);
166  m_dqErrIntegral.setZero(m_robot_util->m_nbJoints);
167 }
168 
169 void JointTorqueController::reset_integral() {
170  m_tauErrIntegral.setZero();
171  m_dqErrIntegral.setZero();
172 }
173 
174 /* --- SIGNALS ---------------------------------------------------------- */
175 /* --- SIGNALS ---------------------------------------------------------- */
176 /* --- SIGNALS ---------------------------------------------------------- */
177 
178 DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector) {
179  // const Eigen::VectorXd& q = m_jointsPositionsSIN(iter);
180  const Eigen::VectorXd& dq = m_jointsVelocitiesSIN(iter);
181  const Eigen::VectorXd& ddq = m_jointsAccelerationsSIN(iter);
182  const Eigen::VectorXd& tau = m_jointsTorquesSIN(iter);
183  const Eigen::VectorXd& dtau = m_jointsTorquesDerivativeSIN(iter);
184  const Eigen::VectorXd& tau_d = m_jointsTorquesDesiredSIN(iter);
185  // const Eigen::VectorXd& dtau_d =
186  // m_jointsTorquesDesiredDerivativeSIN(iter);
187  const Eigen::VectorXd& dq_des = m_dq_desSIN(iter);
188  const Eigen::VectorXd& kp = m_KpTorqueSIN(iter);
189  const Eigen::VectorXd& kd = m_KdTorqueSIN(iter);
190  const Eigen::VectorXd& kd_vel = m_KdVelSIN(iter);
191  const Eigen::VectorXd& ki_vel = m_KiVelSIN(iter);
192  const Eigen::VectorXd& tauErrInt = m_torque_error_integralSOUT(iter);
193  const Eigen::VectorXd& colFricCompPerc =
194  m_coulomb_friction_compensation_percentageSIN(iter);
195  const Eigen::VectorXd& motorParameterKt_p = m_motorParameterKt_pSIN(iter);
196  const Eigen::VectorXd& motorParameterKt_n = m_motorParameterKt_nSIN(iter);
197  const Eigen::VectorXd& motorParameterKf_p = m_motorParameterKf_pSIN(iter);
198  const Eigen::VectorXd& motorParameterKf_n = m_motorParameterKf_nSIN(iter);
199  const Eigen::VectorXd& motorParameterKv_p = m_motorParameterKv_pSIN(iter);
200  const Eigen::VectorXd& motorParameterKv_n = m_motorParameterKv_nSIN(iter);
201  const Eigen::VectorXd& motorParameterKa_p = m_motorParameterKa_pSIN(iter);
202  const Eigen::VectorXd& motorParameterKa_n = m_motorParameterKa_nSIN(iter);
203  const Eigen::VectorXd& polySignDq = m_polySignDqSIN(iter);
204  // const Eigen::VectorXd& dq_thr = m_dq_thresholdSIN(iter);
205 
206  m_tau_star =
207  tau_d + kp.cwiseProduct(tau_d - tau) + tauErrInt - kd.cwiseProduct(dtau);
208 
209  int offset = 0;
210  if (dq.size() == (int)(m_robot_util->m_nbJoints + 6)) offset = 6;
211 
212  m_dqErrIntegral += m_dt * ki_vel.cwiseProduct(dq_des - dq);
213  const Eigen::VectorXd& err_int_sat = m_torque_integral_saturationSIN(iter);
214  // saturate
215  bool saturating = false;
216  for (int i = 0; i < (int)m_robot_util->m_nbJoints; i++) {
217  if (m_dqErrIntegral(i) > err_int_sat(i)) {
218  saturating = true;
219  m_dqErrIntegral(i) = err_int_sat(i);
220  } else if (m_dqErrIntegral(i) < -err_int_sat(i)) {
221  saturating = true;
222  m_dqErrIntegral(i) = -err_int_sat(i);
223  }
224  }
225  if (saturating)
226  SEND_INFO_STREAM_MSG("Saturate dqErr integral: " +
227  toString(m_dqErrIntegral.head<12>()));
228 
229  for (int i = 0; i < (int)m_robot_util->m_nbJoints; i++) {
230  m_current_des(i) = motorModel.getCurrent(
231  m_tau_star(i),
232  dq(i + offset) + kd_vel(i) * (dq_des(i) - dq(i + offset)) +
233  m_dqErrIntegral(i), // ki_vel(i)*(m_dqDesIntegral(i)-q(i)),
234  ddq(i + offset), motorParameterKt_p(i), motorParameterKt_n(i),
235  motorParameterKf_p(i) * colFricCompPerc(i),
236  motorParameterKf_n(i) * colFricCompPerc(i), motorParameterKv_p(i),
237  motorParameterKv_n(i), motorParameterKa_p(i), motorParameterKa_n(i),
238  static_cast<unsigned int>(polySignDq(i)));
239  }
240 
241  s = m_current_des;
242  return s;
243 }
244 
245 DEFINE_SIGNAL_OUT_FUNCTION(torque_error_integral, dynamicgraph::Vector) {
246  const Eigen::VectorXd& tau = m_jointsTorquesSIN(iter);
247  const Eigen::VectorXd& tau_d = m_jointsTorquesDesiredSIN(iter);
248  const Eigen::VectorXd& err_int_sat = m_torque_integral_saturationSIN(iter);
249  const Eigen::VectorXd& ki = m_KiTorqueSIN(iter);
250 
251  // compute torque error integral and saturate
252  m_tauErrIntegral += m_dt * ki.cwiseProduct(tau_d - tau);
253  for (int i = 0; i < (int)m_robot_util->m_nbJoints; i++) {
254  if (m_tauErrIntegral(i) > err_int_sat(i))
255  m_tauErrIntegral(i) = err_int_sat(i);
256  else if (m_tauErrIntegral(i) < -err_int_sat(i))
257  m_tauErrIntegral(i) = -err_int_sat(i);
258  }
259 
260  s = m_tauErrIntegral;
261  return s;
262 }
263 
264 DEFINE_SIGNAL_OUT_FUNCTION(smoothSignDq, dynamicgraph::Vector) {
265  const Eigen::VectorXd& dq = m_jointsVelocitiesSIN(iter);
266  const Eigen::VectorXd& polySignDq = m_polySignDqSIN(iter);
267  if (s.size() != (int)m_robot_util->m_nbJoints)
268  s.resize(m_robot_util->m_nbJoints);
269 
270  for (int i = 0; i < (int)m_robot_util->m_nbJoints; i++)
271  s(i) = motorModel.smoothSign(dq[i], 0.1,
272  static_cast<unsigned int>(polySignDq[i]));
273  // TODO Use Eigen binaryexpr
274  return s;
275 }
276 
277 void JointTorqueController::display(std::ostream& os) const {
278  os << "JointTorqueController " << getName() << ":\n";
279  try {
280  getProfiler().report_all(3, os);
281  } catch (ExceptionSignal e) {
282  }
283 }
284 
285 } // namespace torque_control
286 } // namespace sot
287 } // namespace dynamicgraph
ALL_OUTPUT_SIGNALS
#define ALL_OUTPUT_SIGNALS
Definition: joint-torque-controller.cpp:41
dynamicgraph::sot::torque_control::JointTorqueController::m_dt
double m_dt
Definition: joint-torque-controller.hh:127
dynamicgraph::sot::torque_control::JointTorqueController::init
void init(const double &timestep, const std::string &robotRef)
Definition: joint-torque-controller.cpp:131
MODEL_INPUT_SIGNALS
#define MODEL_INPUT_SIGNALS
Definition: joint-torque-controller.cpp:17
ALL_INPUT_SIGNALS
#define ALL_INPUT_SIGNALS
Definition: joint-torque-controller.cpp:37
dynamicgraph
to read text file
Definition: treeview.dox:22
TORQUE_INTEGRAL_INPUT_SIGNALS
#define TORQUE_INTEGRAL_INPUT_SIGNALS
Definition: joint-torque-controller.cpp:30
TORQUE_CONTROL_INPUT_SIGNALS
#define TORQUE_CONTROL_INPUT_SIGNALS
Definition: joint-torque-controller.cpp:32
dynamicgraph::sot::torque_control::JointTorqueController::reset_integral
void reset_integral()
Definition: joint-torque-controller.cpp:169
commands-helper.hh
joint-torque-controller.hh
dynamicgraph::sot::torque_control::JointTorqueController::JointTorqueController
EIGEN_MAKE_ALIGNED_OPERATOR_NEW JointTorqueController(const std::string &name)
Definition: joint-torque-controller.cpp:61
dynamicgraph::sot::torque_control::JointTorqueController::m_tau_star
Eigen::VectorXd m_tau_star
timestep of the controller
Definition: joint-torque-controller.hh:128
torque_control
Definition: __init__.py:1
dynamicgraph::sot::torque_control::JointTorqueController::m_robot_util
RobotUtilShrPtr m_robot_util
integral of the velocity error
Definition: joint-torque-controller.hh:136
dynamicgraph::sot::torque_control::JointTorqueController::m_current_des
Eigen::VectorXd m_current_des
Definition: joint-torque-controller.hh:129
dynamicgraph::sot::torque_control::JointTorqueController::m_tauErrIntegral
Eigen::VectorXd m_tauErrIntegral
Definition: joint-torque-controller.hh:130
ESTIMATOR_INPUT_SIGNALS
#define ESTIMATOR_INPUT_SIGNALS
Definition: joint-torque-controller.cpp:26
dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
dynamicgraph::sot::torque_control::EntityClassName
AdmittanceController EntityClassName
Definition: admittance-controller.cpp:51
dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
Definition: admittance-controller.cpp:193
dynamicgraph::sot::torque_control::JointTorqueController::m_dqErrIntegral
Eigen::VectorXd m_dqErrIntegral
integral of the current error
Definition: joint-torque-controller.hh:134
VEL_CONTROL_INPUT_SIGNALS
#define VEL_CONTROL_INPUT_SIGNALS
Definition: joint-torque-controller.cpp:35
dynamicgraph::sot::torque_control::JointTorqueController::display
virtual void display(std::ostream &os) const
Definition: joint-torque-controller.cpp:277