8 #include <dynamic-graph/factory.h>
9 #include <sot/core/debug.hh>
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
22 #define ESTIMATOR_INPUT_SIGNALS \
23 m_jointsPositionsSIN << m_jointsVelocitiesSIN << m_jointsAccelerationsSIN << m_jointsTorquesSIN \
24 << m_jointsTorquesDerivativeSIN
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
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
40 using namespace Eigen;
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),
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),
85 CONSTRUCT_SIGNAL_OUT(smoothSignDq,
dynamicgraph::Vector, m_jointsVelocitiesSIN),
86 CONSTRUCT_SIGNAL_OUT(torque_error_integral,
dynamicgraph::Vector,
91 addCommand(
"getTimestep", makeDirectGetter(*
this, &
m_dt, docDirectGetter(
"Control timestep",
"double")));
94 docCommandVoid2(
"Initialize the controller.",
"Control timestep [s].",
95 "Robot reference (string)")));
97 docCommandVoid0(
"Reset the integral error.")));
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);
115 std::string localName(robot_ref);
116 if (isNameInRobotUtil(localName)) {
119 SEND_MSG(
"You should have an entity controller manager initialized before", MSG_TYPE_ERROR);
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);
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);
166 m_tau_star = tau_d + kp.cwiseProduct(tau_d - tau) + tauErrInt - kd.cwiseProduct(dtau);
169 if (dq.size() == (
int)(m_robot_util->m_nbJoints + 6)) offset = 6;
171 m_dqErrIntegral += m_dt * ki_vel.cwiseProduct(dq_des - dq);
172 const Eigen::VectorXd& err_int_sat = m_torque_integral_saturationSIN(iter);
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)) {
178 m_dqErrIntegral(i) = err_int_sat(i);
179 }
else if (m_dqErrIntegral(i) < -err_int_sat(i)) {
181 m_dqErrIntegral(i) = -err_int_sat(i);
184 if (saturating) SEND_INFO_STREAM_MSG(
"Saturate dqErr integral: " + toString(m_dqErrIntegral.head<12>()));
186 for (
int i = 0; i < (int)m_robot_util->m_nbJoints; i++) {
187 m_current_des(i) = motorModel.getCurrent(
189 dq(i + offset) + kd_vel(i) * (dq_des(i) - dq(i + offset)) +
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)));
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);
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);
215 s = m_tauErrIntegral;
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);
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]));
231 os <<
"JointTorqueController " << getName() <<
":\n";
233 getProfiler().report_all(3, os);
234 }
catch (ExceptionSignal e) {