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