6 #include <tsid/utils/stop-watch.hpp>
7 #include <dynamic-graph/factory.h>
8 #include <sot/core/debug.hh>
15 namespace dg = ::dynamicgraph;
17 using namespace dg::command;
19 using namespace Eigen;
21 using namespace tsid::math;
22 using namespace tsid::tasks;
23 using namespace dg::sot;
25 #define PROFILE_DQ_DES_COMPUTATION "Admittance control computation"
27 #define REF_FORCE_SIGNALS m_fRightFootRefSIN << m_fLeftFootRefSIN
29 #define FORCE_SIGNALS m_fRightFootSIN << m_fLeftFootSIN << m_fRightFootFilteredSIN << m_fLeftFootFilteredSIN
31 #define GAIN_SIGNALS \
32 m_kp_forceSIN << m_ki_forceSIN << m_kp_velSIN << m_ki_velSIN << m_force_integral_saturationSIN \
33 << m_force_integral_deadzoneSIN
34 #define STATE_SIGNALS m_encodersSIN << m_jointsVelocitiesSIN
36 #define INPUT_SIGNALS \
37 STATE_SIGNALS << REF_FORCE_SIGNALS << FORCE_SIGNALS << GAIN_SIGNALS << m_controlledJointsSIN << m_dampingSIN
39 #define DES_VEL_SIGNALS m_vDesRightFootSOUT << m_vDesLeftFootSOUT //<< m_fRightHandErrorSOUT << m_fLeftHandErrorSOUT
40 #define OUTPUT_SIGNALS m_uSOUT << m_dqDesSOUT << DES_VEL_SIGNALS
46 typedef Eigen::Matrix<double, 3, 1>
Vector3;
47 typedef Eigen::Matrix<double, 6, 1>
Vector6;
57 CONSTRUCT_SIGNAL_IN(jointsVelocities,
dynamicgraph::Vector),
62 CONSTRUCT_SIGNAL_IN(force_integral_saturation,
dynamicgraph::Vector),
63 CONSTRUCT_SIGNAL_IN(force_integral_deadzone,
dynamicgraph::Vector),
64 CONSTRUCT_SIGNAL_IN(fRightFootRef,
dynamicgraph::Vector),
68 CONSTRUCT_SIGNAL_IN(fRightFootFiltered,
dynamicgraph::Vector),
69 CONSTRUCT_SIGNAL_IN(fLeftFootFiltered,
dynamicgraph::Vector),
70 CONSTRUCT_SIGNAL_IN(controlledJoints,
dynamicgraph::Vector),
79 CONSTRUCT_SIGNAL_OUT(vDesRightFoot,
dynamicgraph::Vector,
80 m_fRightFootSIN << m_fRightFootFilteredSIN << m_fRightFootRefSIN <<
GAIN_SIGNALS),
82 m_fLeftFootSIN << m_fLeftFootFilteredSIN << m_fLeftFootRefSIN <<
GAIN_SIGNALS)
89 m_initSucceeded(false),
90 m_useJacobianTranspose(true) {
97 addCommand(
"getUseJacobianTranspose",
100 docDirectGetter(
"If true it uses the Jacobian transpose, otherwise the pseudoinverse",
"bool")));
101 addCommand(
"setUseJacobianTranspose",
104 docDirectSetter(
"If true it uses the Jacobian transpose, otherwise the pseudoinverse",
"bool")));
106 docCommandVoid2(
"Initialize the entity.",
"Time period in seconds (double)",
107 "Robot name (string)")));
111 if (dt <= 0.0)
return SEND_MSG(
"Timestep must be positive", MSG_TYPE_ERROR);
112 if (!m_encodersSIN.isPlugged())
return SEND_MSG(
"Init failed: signal encoders is not plugged", MSG_TYPE_ERROR);
113 if (!m_jointsVelocitiesSIN.isPlugged())
114 return SEND_MSG(
"Init failed: signal jointsVelocities is not plugged", MSG_TYPE_ERROR);
115 if (!m_controlledJointsSIN.isPlugged())
116 return SEND_MSG(
"Init failed: signal controlledJoints is not plugged", MSG_TYPE_ERROR);
122 std::string localName(robotRef);
123 if (isNameInRobotUtil(localName))
126 return SEND_MSG(
"You should have an entity controller manager initialized before", MSG_TYPE_ERROR);
129 vector<string> package_dirs;
130 m_robot =
new robots::RobotWrapper(
m_robot_util->m_urdf_filename, package_dirs, pinocchio::JointModelFreeFlyer());
150 }
catch (
const std::exception& e) {
151 std::cout << e.what();
152 return SEND_MSG(
"Init failed: Could load URDF :" +
m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
161 if (!m_initSucceeded) {
162 SEND_WARNING_STREAM_MSG(
"Cannot compute signal u before initialization!");
166 const Vector& dqDes = m_dqDesSOUT(iter);
167 const Vector& q = m_encodersSIN(iter);
168 const Vector& dq = m_jointsVelocitiesSIN(iter);
169 const Vector& kp = m_kp_velSIN(iter);
170 const Vector& ki = m_ki_velSIN(iter);
178 m_dq_fd = (q - m_qPrev) / m_dt;
181 m_dqErrIntegral += m_dt * ki.cwiseProduct(dqDes - m_dq_fd);
182 s = kp.cwiseProduct(dqDes - dq) + m_dqErrIntegral;
188 if (!m_initSucceeded) {
189 SEND_WARNING_STREAM_MSG(
"Cannot compute signal dqDes before initialization!");
195 const dg::sot::Vector6d v_des_LF = m_vDesLeftFootSOUT(iter);
196 const dg::sot::Vector6d v_des_RF = m_vDesRightFootSOUT(iter);
197 const Vector& q_sot = m_encodersSIN(iter);
202 assert(q_sot.size() == m_nj &&
"Unexpected size of signal encoder");
205 m_robot_util->joints_sot_to_urdf(q_sot, m_q_urdf.tail(m_nj));
206 m_robot->computeAllTerms(*m_data, m_q_urdf, m_v_urdf);
207 m_robot->frameJacobianLocal(*m_data, m_frame_id_rf, m_J_RF);
208 m_robot->frameJacobianLocal(*m_data, m_frame_id_lf, m_J_LF);
217 if (m_useJacobianTranspose) {
218 m_dq_des_urdf = m_J_RF.rightCols(m_nj).transpose() * v_des_RF;
219 m_dq_des_urdf += m_J_LF.rightCols(m_nj).transpose() * v_des_LF;
221 m_J_RF_QR.compute(m_J_RF.rightCols(m_nj));
222 m_J_LF_QR.compute(m_J_LF.rightCols(m_nj));
224 m_dq_des_urdf = m_J_RF_QR.solve(v_des_RF);
225 m_dq_des_urdf += m_J_LF_QR.solve(v_des_LF);
228 if (s.size() != m_nj) s.resize(m_nj);
230 m_robot_util->joints_urdf_to_sot(m_dq_des_urdf, s);
238 if (!m_initSucceeded) {
239 SEND_MSG(
"Cannot compute signal vDesRightFoot before initialization!", MSG_TYPE_WARNING_STREAM);
242 const Vector6& f = m_fRightFootSIN(iter);
243 const Vector6& f_filt = m_fRightFootFilteredSIN(iter);
244 const Vector6& fRef = m_fRightFootRefSIN(iter);
245 const Vector6d& kp = m_kp_forceSIN(iter);
246 const Vector6d& ki = m_ki_forceSIN(iter);
247 const Vector6d& f_sat = m_force_integral_saturationSIN(iter);
248 const Vector6d& dz = m_force_integral_deadzoneSIN(iter);
250 dg::sot::Vector6d err = fRef - f;
251 dg::sot::Vector6d err_filt = fRef - f_filt;
252 dg::sot::Vector6d v_des = -kp.cwiseProduct(err_filt);
254 for (
int i = 0; i < 6; i++) {
256 m_v_RF_int(i) -= m_dt * ki(i) * (err(i) - dz(i));
257 else if (err(i) < -dz(i))
258 m_v_RF_int(i) -= m_dt * ki(i) * (err(i) + dz(i));
262 bool saturating =
false;
263 for (
int i = 0; i < 6; i++) {
264 if (m_v_RF_int(i) > f_sat(i)) {
266 m_v_RF_int(i) = f_sat(i);
267 }
else if (m_v_RF_int(i) < -f_sat(i)) {
269 m_v_RF_int(i) = -f_sat(i);
272 if (saturating) SEND_INFO_STREAM_MSG(
"Saturate m_v_RF_int integral: " + toString(m_v_RF_int.transpose()));
273 s = v_des + m_v_RF_int;
278 if (!m_initSucceeded) {
279 SEND_MSG(
"Cannot compute signal vDesLeftFoot before initialization!", MSG_TYPE_WARNING_STREAM);
282 const Vector6& f = m_fLeftFootSIN(iter);
283 const Vector6& f_filt = m_fLeftFootFilteredSIN(iter);
284 const Vector6& fRef = m_fLeftFootRefSIN(iter);
285 const Vector6d& kp = m_kp_forceSIN(iter);
286 const Vector6d& ki = m_ki_forceSIN(iter);
287 const Vector6d& f_sat = m_force_integral_saturationSIN(iter);
288 const Vector6d& dz = m_force_integral_deadzoneSIN(iter);
290 dg::sot::Vector6d err = fRef - f;
291 dg::sot::Vector6d err_filt = fRef - f_filt;
292 dg::sot::Vector6d v_des = -kp.cwiseProduct(err_filt);
294 for (
int i = 0; i < 6; i++) {
296 m_v_LF_int(i) -= m_dt * ki(i) * (err(i) - dz(i));
297 else if (err(i) < -dz(i))
298 m_v_LF_int(i) -= m_dt * ki(i) * (err(i) + dz(i));
302 bool saturating =
false;
303 for (
int i = 0; i < 6; i++) {
304 if (m_v_LF_int(i) > f_sat(i)) {
306 m_v_LF_int(i) = f_sat(i);
307 }
else if (m_v_LF_int(i) < -f_sat(i)) {
309 m_v_LF_int(i) = -f_sat(i);
312 if (saturating) SEND_INFO_STREAM_MSG(
"Saturate m_v_LF_int integral: " + toString(m_v_LF_int.transpose()));
313 s = v_des + m_v_LF_int;
367 os <<
"AdmittanceController " << getName();
369 getProfiler().report_all(3, os);
370 }
catch (ExceptionSignal e) {
376 eigen_assert(A.computeU() && A.computeV() &&
377 "svdSolveWithDamping() requires both unitaries U and V to be computed (thin unitaries suffice).");
378 assert(A.rows() == b.size());
381 VectorXd tmp(A.cols());
382 int nzsv =
static_cast<int>(A.nonzeroSingularValues());
383 tmp.noalias() = A.matrixU().leftCols(nzsv).adjoint() * b;
385 double sv, d2 = damping * damping;
386 for (
int i = 0; i < nzsv; i++) {
387 sv = A.singularValues()(i);
388 tmp(i) *= sv / (sv * sv + d2);
391 VectorXd res = A.matrixV().leftCols(nzsv) * tmp;