sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
base-estimator.cpp
Go to the documentation of this file.
1/*
2 * Copyright 2017, Thomas Flayols, LAAS-CNRS
3 *
4 */
5
6#include <dynamic-graph/factory.h>
7
8#include <sot/core/debug.hh>
9#include <sot/core/stop-watch.hh>
12
13#include "pinocchio/algorithm/frames.hpp"
14
15namespace dynamicgraph {
16namespace sot {
17namespace torque_control {
18namespace dg = ::dynamicgraph;
19using namespace dg;
20using namespace dg::command;
21using namespace std;
22using namespace pinocchio;
23using boost::math::normal; // typedef provides default type is double.
24
25void se3Interp(const pinocchio::SE3& s1, const pinocchio::SE3& s2,
26 const double alpha, pinocchio::SE3& s12) {
27 const Eigen::Vector3d t_(s1.translation() * alpha +
28 s2.translation() * (1 - alpha));
29
30 const Eigen::Vector3d w(pinocchio::log3(s1.rotation()) * alpha +
31 pinocchio::log3(s2.rotation()) * (1 - alpha));
32
33 s12 = pinocchio::SE3(pinocchio::exp3(w), t_);
34}
35void rpyToMatrix(double roll, double pitch, double yaw, Eigen::Matrix3d& R) {
36 Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
37 Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
38 Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
39 Eigen::Quaternion<double> q = yawAngle * pitchAngle * rollAngle;
40 R = q.matrix();
41}
42
43void rpyToMatrix(const Eigen::Vector3d& rpy, Eigen::Matrix3d& R) {
44 return rpyToMatrix(rpy[0], rpy[1], rpy[2], R);
45}
46
47void matrixToRpy(const Eigen::Matrix3d& M, Eigen::Vector3d& rpy) {
48 double m = sqrt(M(2, 1) * M(2, 1) + M(2, 2) * M(2, 2));
49 rpy(1) = atan2(-M(2, 0), m);
50 if (fabs(fabs(rpy(1)) - 0.5 * M_PI) < 0.001) {
51 rpy(0) = 0.0;
52 rpy(2) = -atan2(M(0, 1), M(1, 1));
53 } else {
54 rpy(2) = atan2(M(1, 0), M(0, 0)); // alpha
55 rpy(0) = atan2(M(2, 1), M(2, 2)); // gamma
56 }
57}
58
59void quanternionMult(const Eigen::Vector4d& q1, const Eigen::Vector4d& q2,
60 Eigen::Vector4d& q12) {
61 q12(0) = q2(0) * q1(0) - q2(1) * q1(1) - q2(2) * q1(2) - q2(3) * q1(3);
62 q12(1) = q2(0) * q1(1) + q2(1) * q1(0) - q2(2) * q1(3) + q2(3) * q1(2);
63 q12(2) = q2(0) * q1(2) + q2(1) * q1(3) + q2(2) * q1(0) - q2(3) * q1(1);
64 q12(3) = q2(0) * q1(3) - q2(1) * q1(2) + q2(2) * q1(1) + q2(3) * q1(0);
65}
66
67void pointRotationByQuaternion(const Eigen::Vector3d& point,
68 const Eigen::Vector4d& quat,
69 Eigen::Vector3d& rotatedPoint) {
70 const Eigen::Vector4d p4(0.0, point(0), point(1), point(2));
71 const Eigen::Vector4d quat_conj(quat(0), -quat(1), -quat(2), -quat(3));
72 Eigen::Vector4d q_tmp1, q_tmp2;
73 quanternionMult(quat, p4, q_tmp1);
74 quanternionMult(q_tmp1, quat_conj, q_tmp2);
75 rotatedPoint(0) = q_tmp2(1);
76 rotatedPoint(1) = q_tmp2(2);
77 rotatedPoint(2) = q_tmp2(3);
78}
79
80#define PROFILE_BASE_POSITION_ESTIMATION "base-est position estimation"
81#define PROFILE_BASE_VELOCITY_ESTIMATION "base-est velocity estimation"
82#define PROFILE_BASE_KINEMATICS_COMPUTATION "base-est kinematics computation"
83
84#define INPUT_SIGNALS \
85 m_joint_positionsSIN << m_joint_velocitiesSIN << m_imu_quaternionSIN \
86 << m_forceLLEGSIN << m_forceRLEGSIN << m_dforceLLEGSIN \
87 << m_dforceRLEGSIN << m_w_lf_inSIN << m_w_rf_inSIN \
88 << m_K_fb_feet_posesSIN << m_lf_ref_xyzquatSIN \
89 << m_rf_ref_xyzquatSIN << m_accelerometerSIN \
90 << m_gyroscopeSIN
91#define OUTPUT_SIGNALS \
92 m_qSOUT << m_vSOUT << m_v_kinSOUT << m_v_flexSOUT << m_v_imuSOUT \
93 << m_v_gyrSOUT << m_lf_xyzquatSOUT << m_rf_xyzquatSOUT << m_a_acSOUT \
94 << m_v_acSOUT << m_q_lfSOUT << m_q_rfSOUT << m_q_imuSOUT \
95 << m_w_lfSOUT << m_w_rfSOUT << m_w_lf_filteredSOUT \
96 << m_w_rf_filteredSOUT
97
100typedef BaseEstimator EntityClassName;
101
102/* --- DG FACTORY ---------------------------------------------------- */
104
105/* ------------------------------------------------------------------- */
106/* --- CONSTRUCTION -------------------------------------------------- */
107/* ------------------------------------------------------------------- */
108BaseEstimator::BaseEstimator(const std::string& name)
109 : Entity(name),
110 CONSTRUCT_SIGNAL_IN(joint_positions, dynamicgraph::Vector),
111 CONSTRUCT_SIGNAL_IN(joint_velocities, dynamicgraph::Vector),
112 CONSTRUCT_SIGNAL_IN(imu_quaternion, dynamicgraph::Vector),
113 CONSTRUCT_SIGNAL_IN(forceLLEG, dynamicgraph::Vector),
114 CONSTRUCT_SIGNAL_IN(forceRLEG, dynamicgraph::Vector),
115 CONSTRUCT_SIGNAL_IN(dforceLLEG, dynamicgraph::Vector),
116 CONSTRUCT_SIGNAL_IN(dforceRLEG, dynamicgraph::Vector),
117 CONSTRUCT_SIGNAL_IN(w_lf_in, double),
118 CONSTRUCT_SIGNAL_IN(w_rf_in, double),
119 CONSTRUCT_SIGNAL_IN(K_fb_feet_poses, double),
120 CONSTRUCT_SIGNAL_IN(lf_ref_xyzquat, dynamicgraph::Vector),
121 CONSTRUCT_SIGNAL_IN(rf_ref_xyzquat, dynamicgraph::Vector),
122 CONSTRUCT_SIGNAL_IN(accelerometer, dynamicgraph::Vector),
123 CONSTRUCT_SIGNAL_IN(gyroscope, dynamicgraph::Vector),
124 CONSTRUCT_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector,
125 m_joint_positionsSIN << m_joint_velocitiesSIN),
126 CONSTRUCT_SIGNAL_OUT(q, dynamicgraph::Vector,
127 m_kinematics_computationsSINNER
128 << m_joint_positionsSIN << m_imu_quaternionSIN
129 << m_forceLLEGSIN << m_forceRLEGSIN
130 << m_w_lf_inSIN << m_w_rf_inSIN
131 << m_K_fb_feet_posesSIN << m_w_lf_filteredSOUT
132 << m_w_rf_filteredSOUT << m_lf_ref_xyzquatSIN
133 << m_rf_ref_xyzquatSIN),
134 CONSTRUCT_SIGNAL_OUT(v, dynamicgraph::Vector,
135 m_kinematics_computationsSINNER
136 << m_accelerometerSIN << m_gyroscopeSIN
137 << m_qSOUT << m_dforceLLEGSIN
138 << m_dforceRLEGSIN),
139 CONSTRUCT_SIGNAL_OUT(v_kin, dynamicgraph::Vector, m_vSOUT),
140 CONSTRUCT_SIGNAL_OUT(v_flex, dynamicgraph::Vector, m_vSOUT),
141 CONSTRUCT_SIGNAL_OUT(v_imu, dynamicgraph::Vector, m_vSOUT),
142 CONSTRUCT_SIGNAL_OUT(v_gyr, dynamicgraph::Vector, m_vSOUT),
143 CONSTRUCT_SIGNAL_OUT(lf_xyzquat, dynamicgraph::Vector, m_qSOUT),
144 CONSTRUCT_SIGNAL_OUT(rf_xyzquat, dynamicgraph::Vector, m_qSOUT),
145 CONSTRUCT_SIGNAL_OUT(a_ac, dynamicgraph::Vector, m_vSOUT),
146 CONSTRUCT_SIGNAL_OUT(v_ac, dynamicgraph::Vector, m_vSOUT),
147 CONSTRUCT_SIGNAL_OUT(q_lf, dynamicgraph::Vector, m_qSOUT),
148 CONSTRUCT_SIGNAL_OUT(q_rf, dynamicgraph::Vector, m_qSOUT),
149 CONSTRUCT_SIGNAL_OUT(q_imu, dynamicgraph::Vector,
150 m_qSOUT << m_imu_quaternionSIN),
151 CONSTRUCT_SIGNAL_OUT(w_lf, double, m_forceLLEGSIN),
152 CONSTRUCT_SIGNAL_OUT(w_rf, double, m_forceRLEGSIN),
153 CONSTRUCT_SIGNAL_OUT(w_lf_filtered, double, m_w_lfSOUT),
154 CONSTRUCT_SIGNAL_OUT(w_rf_filtered, double, m_w_rfSOUT),
155 m_initSucceeded(false),
156 m_reset_foot_pos(true),
157 m_w_imu(0.0),
158 m_zmp_std_dev_rf(1.0),
159 m_zmp_std_dev_lf(1.0),
160 m_fz_std_dev_rf(1.0),
161 m_fz_std_dev_lf(1.0),
162 m_zmp_margin_lf(0.0),
163 m_zmp_margin_rf(0.0) {
164 Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);
165
166 m_K_rf << 4034, 23770, 239018, 707, 502, 936;
167 m_K_lf << 4034, 23770, 239018, 707, 502, 936;
168 m_left_foot_sizes << 0.130, -0.100, 0.075, -0.056;
169 m_right_foot_sizes << 0.130, -0.100, 0.056, -0.075;
170
171 /* Commands. */
172 addCommand("init",
173 makeCommandVoid2(
174 *this, &BaseEstimator::init,
175 docCommandVoid2("Initialize the entity.", "time step (double)",
176 "URDF file path (string)")));
177
178 addCommand("set_fz_stable_windows_size",
179 makeCommandVoid1(
181 docCommandVoid1(
182 "Set the windows size used to detect that feet is stable",
183 "int")));
184 addCommand("set_alpha_w_filter",
185 makeCommandVoid1(
187 docCommandVoid1("Set the filter parameter to filter weights",
188 "double")));
189 addCommand(
190 "set_alpha_DC_acc",
191 makeCommandVoid1(*this, &BaseEstimator::set_alpha_DC_acc,
192 docCommandVoid1("Set the filter parameter for removing "
193 "DC from accelerometer data",
194 "double")));
195 addCommand("set_alpha_DC_vel",
196 makeCommandVoid1(
198 docCommandVoid1("Set the filter parameter for removing DC "
199 "from velocity integrated from acceleration",
200 "double")));
201 addCommand(
202 "reset_foot_positions",
203 makeCommandVoid0(*this, &BaseEstimator::reset_foot_positions,
204 docCommandVoid0("Reset the position of the feet.")));
205 addCommand(
206 "get_imu_weight",
207 makeDirectGetter(
208 *this, &m_w_imu,
209 docDirectGetter("Weight of imu-based orientation in sensor fusion",
210 "double")));
211 addCommand("set_imu_weight",
212 makeCommandVoid1(
214 docCommandVoid1(
215 "Set the weight of imu-based orientation in sensor fusion",
216 "double")));
217 addCommand(
218 "set_zmp_std_dev_right_foot",
219 makeCommandVoid1(*this, &BaseEstimator::set_zmp_std_dev_right_foot,
220 docCommandVoid1("Set the standard deviation of the ZMP "
221 "measurement errors for the right foot",
222 "double")));
223 addCommand(
224 "set_zmp_std_dev_left_foot",
225 makeCommandVoid1(*this, &BaseEstimator::set_zmp_std_dev_left_foot,
226 docCommandVoid1("Set the standard deviation of the ZMP "
227 "measurement errors for the left foot",
228 "double")));
229 addCommand("set_normal_force_std_dev_right_foot",
230 makeCommandVoid1(
232 docCommandVoid1("Set the standard deviation of the normal "
233 "force measurement errors for the right foot",
234 "double")));
235 addCommand("set_normal_force_std_dev_left_foot",
236 makeCommandVoid1(
238 docCommandVoid1("Set the standard deviation of the normal "
239 "force measurement errors for the left foot",
240 "double")));
241 addCommand("set_stiffness_right_foot",
242 makeCommandVoid1(
244 docCommandVoid1(
245 "Set the 6d stiffness of the spring at the right foot",
246 "vector")));
247 addCommand(
248 "set_stiffness_left_foot",
249 makeCommandVoid1(
251 docCommandVoid1("Set the 6d stiffness of the spring at the left foot",
252 "vector")));
253 addCommand(
254 "set_right_foot_sizes",
255 makeCommandVoid1(
257 docCommandVoid1(
258 "Set the size of the right foot (pos x, neg x, pos y, neg y)",
259 "4d vector")));
260 addCommand(
261 "set_left_foot_sizes",
262 makeCommandVoid1(
264 docCommandVoid1(
265 "Set the size of the left foot (pos x, neg x, pos y, neg y)",
266 "4d vector")));
267 addCommand(
268 "set_zmp_margin_right_foot",
269 makeCommandVoid1(
271 docCommandVoid1("Set the ZMP margin for the right foot", "double")));
272 addCommand(
273 "set_zmp_margin_left_foot",
274 makeCommandVoid1(
276 docCommandVoid1("Set the ZMP margin for the left foot", "double")));
277 addCommand(
278 "set_normal_force_margin_right_foot",
279 makeCommandVoid1(
281 docCommandVoid1("Set the normal force margin for the right foot",
282 "double")));
283 addCommand(
284 "set_normal_force_margin_left_foot",
285 makeCommandVoid1(
287 docCommandVoid1("Set the normal force margin for the left foot",
288 "double")));
289}
290
291void BaseEstimator::init(const double& dt, const std::string& robotRef) {
292 m_dt = dt;
293 try {
294 /* Retrieve m_robot_util informations */
295 std::string localName(robotRef);
296 if (isNameInRobotUtil(localName)) {
297 m_robot_util = getRobotUtil(localName);
298 std::cerr << "m_robot_util:" << m_robot_util << std::endl;
299 } else {
300 SEND_MSG(
301 "You should have an entity controller manager initialized before",
302 MSG_TYPE_ERROR);
303 return;
304 }
305
306 pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename,
307 pinocchio::JointModelFreeFlyer(), m_model);
308
309 assert(
310 m_model.existFrame(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
311 assert(
312 m_model.existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
313 assert(
314 m_model.existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
316 m_model.getFrameId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
318 m_model.getFrameId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name);
319 m_IMU_body_id = m_model.getFrameId(m_robot_util->m_imu_joint_name);
320
321 m_q_pin.setZero(m_model.nq);
322 m_q_pin[6] = 1.; // for quaternion
323 m_q_sot.setZero(m_robot_util->m_nbJoints + 6);
324 m_v_pin.setZero(m_robot_util->m_nbJoints + 6);
325 m_v_sot.setZero(m_robot_util->m_nbJoints + 6);
326 m_v_kin.setZero(m_robot_util->m_nbJoints + 6);
327 m_v_flex.setZero(m_robot_util->m_nbJoints + 6);
328 m_v_imu.setZero(m_robot_util->m_nbJoints + 6);
329 m_v_gyr.setZero(m_robot_util->m_nbJoints + 6);
331 SE3(Matrix3::Identity(),
332 -Eigen::Map<const Vector3>(
333 &m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ(0)));
334
335 m_last_vel.setZero();
336 m_last_DCvel.setZero();
338 .setZero(); // this is to replace by acceleration at 1st iteration
339
340 m_alpha_DC_acc = 0.9995;
341 m_alpha_DC_vel = 0.9995;
342 m_alpha_w_filter = 1.0;
348 m_isFirstIterFlag = true;
349 } catch (const std::exception& e) {
350 std::cout << e.what();
351 SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename,
352 MSG_TYPE_ERROR);
353 return;
354 }
355 m_data = new pinocchio::Data(m_model);
356 m_initSucceeded = true;
357}
358
360 if (ws < 0.0)
361 return SEND_MSG("windows_size should be a positive integer",
362 MSG_TYPE_ERROR);
364}
365
367 if (a < 0.0 || a > 1.0)
368 return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR);
370}
371
372void BaseEstimator::set_alpha_DC_acc(const double& a) {
373 if (a < 0.0 || a > 1.0)
374 return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR);
375 m_alpha_DC_acc = a;
376}
377
378void BaseEstimator::set_alpha_DC_vel(const double& a) {
379 if (a < 0.0 || a > 1.0)
380 return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR);
381 m_alpha_DC_vel = a;
382}
383
385
386void BaseEstimator::set_imu_weight(const double& w) {
387 if (w < 0.0)
388 return SEND_MSG("Imu weight must be nonnegative", MSG_TYPE_ERROR);
389 m_w_imu = w;
390}
391
392void BaseEstimator::set_stiffness_right_foot(const dynamicgraph::Vector& k) {
393 if (k.size() != 6)
394 return SEND_MSG(
395 "Stiffness vector should have size 6, not " + toString(k.size()),
396 MSG_TYPE_ERROR);
397 m_K_rf = k;
398}
399
400void BaseEstimator::set_stiffness_left_foot(const dynamicgraph::Vector& k) {
401 if (k.size() != 6)
402 return SEND_MSG(
403 "Stiffness vector should have size 6, not " + toString(k.size()),
404 MSG_TYPE_ERROR);
405 m_K_lf = k;
406}
407
408void BaseEstimator::set_zmp_std_dev_right_foot(const double& std_dev) {
409 if (std_dev <= 0.0)
410 return SEND_MSG("Standard deviation must be a positive number",
411 MSG_TYPE_ERROR);
412 m_zmp_std_dev_rf = std_dev;
413}
414
415void BaseEstimator::set_zmp_std_dev_left_foot(const double& std_dev) {
416 if (std_dev <= 0.0)
417 return SEND_MSG("Standard deviation must be a positive number",
418 MSG_TYPE_ERROR);
419 m_zmp_std_dev_lf = std_dev;
420}
421
423 if (std_dev <= 0.0)
424 return SEND_MSG("Standard deviation must be a positive number",
425 MSG_TYPE_ERROR);
426 m_fz_std_dev_rf = std_dev;
427}
428
430 if (std_dev <= 0.0)
431 return SEND_MSG("Standard deviation must be a positive number",
432 MSG_TYPE_ERROR);
433 m_fz_std_dev_lf = std_dev;
434}
435
436void BaseEstimator::set_right_foot_sizes(const dynamicgraph::Vector& s) {
437 if (s.size() != 4)
438 return SEND_MSG(
439 "Foot size vector should have size 4, not " + toString(s.size()),
440 MSG_TYPE_ERROR);
442}
443
444void BaseEstimator::set_left_foot_sizes(const dynamicgraph::Vector& s) {
445 if (s.size() != 4)
446 return SEND_MSG(
447 "Foot size vector should have size 4, not " + toString(s.size()),
448 MSG_TYPE_ERROR);
450}
451
453 m_zmp_margin_rf = margin;
454}
455
456void BaseEstimator::set_zmp_margin_left_foot(const double& margin) {
457 m_zmp_margin_lf = margin;
458}
459
461 m_fz_margin_rf = margin;
462}
463
465 m_fz_margin_lf = margin;
466}
467
468void BaseEstimator::compute_zmp(const Vector6& w, Vector2& zmp) {
469 pinocchio::Force f(w);
470 f = m_sole_M_ftSens.act(f);
471 if (f.linear()[2] == 0.0) return;
472 zmp[0] = -f.angular()[1] / f.linear()[2];
473 zmp[1] = f.angular()[0] / f.linear()[2];
474}
475
476double BaseEstimator::compute_zmp_weight(const Vector2& zmp,
477 const Vector4& foot_sizes,
478 double std_dev, double margin) {
479 double fs0 = foot_sizes[0] - margin;
480 double fs1 = foot_sizes[1] + margin;
481 double fs2 = foot_sizes[2] - margin;
482 double fs3 = foot_sizes[3] + margin;
483
484 if (zmp[0] > fs0 || zmp[0] < fs1 || zmp[1] > fs2 || zmp[1] < fs3) return 0;
485
486 double cdx = ((cdf(m_normal, (fs0 - zmp[0]) / std_dev) -
487 cdf(m_normal, (fs1 - zmp[0]) / std_dev)) -
488 0.5) *
489 2.0;
490 double cdy = ((cdf(m_normal, (fs2 - zmp[1]) / std_dev) -
491 cdf(m_normal, (fs3 - zmp[1]) / std_dev)) -
492 0.5) *
493 2.0;
494 return cdx * cdy;
495}
496
497double BaseEstimator::compute_force_weight(double fz, double std_dev,
498 double margin) {
499 if (fz < margin) return 0.0;
500 return (cdf(m_normal, (fz - margin) / std_dev) - 0.5) * 2.0;
501}
502
504 const Vector6& ftrf) {
505 // compute the base position wrt each foot
506 SE3 dummy, dummy1, lfMff, rfMff;
507 m_oMrfs = SE3::Identity();
508 m_oMlfs = SE3::Identity();
510 static_cast<int>(m_right_foot_id), rfMff, dummy,
511 dummy1); // rfMff is obtain reading oMff becaused oMrfs
512 // is here set to Identity
513 kinematics_estimation(ftlf, m_K_lf, m_oMlfs, static_cast<int>(m_left_foot_id),
514 lfMff, dummy, dummy1);
515
516 // distance from ankle to ground
517 const Vector3& ankle_2_sole_xyz =
518 m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ;
519 const SE3 groundMfoot(Matrix3::Identity(), -1.0 * ankle_2_sole_xyz);
520 lfMff = groundMfoot * lfMff;
521 rfMff = groundMfoot * rfMff;
522
523 // set the world frame in between the feet
524 const Vector3 w(0.5 * (pinocchio::log3(lfMff.rotation()) +
525 pinocchio::log3(rfMff.rotation())));
526 SE3 oMff = SE3(pinocchio::exp3(w),
527 0.5 * (lfMff.translation() + rfMff.translation()));
528 // add a constant offset to make the world frame match the one in OpenHRP
529 oMff.translation()(0) += 9.562e-03;
530
531 m_oMlfs = oMff * lfMff.inverse() * groundMfoot;
532 m_oMrfs = oMff * rfMff.inverse() * groundMfoot;
533
534 m_oMlfs_xyzquat.head<3>() = m_oMlfs.translation();
535 Eigen::Quaternion<double> quat_lf(m_oMlfs.rotation());
536 m_oMlfs_xyzquat(3) = quat_lf.w();
537 m_oMlfs_xyzquat(4) = quat_lf.x();
538 m_oMlfs_xyzquat(5) = quat_lf.y();
539 m_oMlfs_xyzquat(6) = quat_lf.z();
540
541 m_oMrfs_xyzquat.head<3>() = m_oMrfs.translation();
542 Eigen::Quaternion<double> quat_rf(m_oMrfs.rotation());
543 m_oMrfs_xyzquat(3) = quat_rf.w();
544 m_oMrfs_xyzquat(4) = quat_rf.x();
545 m_oMrfs_xyzquat(5) = quat_rf.y();
546 m_oMrfs_xyzquat(6) = quat_rf.z();
547
548 // save this poses to use it if no ref is provided
551
552 sendMsg("Reference pos of left foot:\n" + toString(m_oMlfs), MSG_TYPE_INFO);
553 sendMsg("Reference pos of right foot:\n" + toString(m_oMrfs), MSG_TYPE_INFO);
554
555 // kinematics_estimation(ftrf, m_K_rf, m_oMrfs, m_right_foot_id,
556 // m_oMff_rf, dummy); kinematics_estimation(ftlf, m_K_lf, m_oMlfs,
557 // m_left_foot_id, m_oMff_lf, dummy); sendMsg("Base estimation left
558 // foot:\n"+toString(m_oMff_lf), MSG_TYPE_DEBUG); sendMsg("Base
559 // estimation right foot:\n"+toString(m_oMff_rf), MSG_TYPE_DEBUG);
560 // sendMsg("Difference base estimation left-right
561 // foot:\n"+toString(m_oMff_rf.inverse()*m_oMff_lf), MSG_TYPE_DEBUG);
562
563 m_reset_foot_pos = false;
564}
565
566void BaseEstimator::kinematics_estimation(const Vector6& ft, const Vector6& K,
567 const SE3& oMfs, const int foot_id,
568 SE3& oMff, SE3& oMfa, SE3& fsMff) {
569 Vector3 xyz;
570 xyz << -ft[0] / K(0), -ft[1] / K(1), -ft[2] / K(2);
571 Matrix3 R;
572 rpyToMatrix(-ft[3] / K(3), -ft[4] / K(4), -ft[5] / K(5), R);
573 const SE3 fsMfa(R, xyz); // foot sole to foot ankle
574 oMfa = oMfs * fsMfa; // world to foot ankle
575 const SE3 faMff(m_data->oMf[foot_id].inverse()); // foot ankle to free flyer
576 //~ sendMsg("faMff (foot_id="+toString(foot_id)+"):\n" + toString(faMff),
577 // MSG_TYPE_INFO); ~ sendMsg("fsMfa (foot_id="+toString(foot_id)+"):\n" +
578 // toString(fsMfa), MSG_TYPE_INFO);
579 oMff = oMfa * faMff; // world to free flyer
580 fsMff = fsMfa * faMff; // foot sole to free flyer
581}
582
583/* ------------------------------------------------------------------- */
584/* --- SIGNALS ------------------------------------------------------- */
585/* ------------------------------------------------------------------- */
586
587DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector) {
588 if (!m_initSucceeded) {
589 SEND_WARNING_STREAM_MSG(
590 "Cannot compute signal kinematics_computations before initialization!");
591 return s;
592 }
593
594 const Eigen::VectorXd& qj = m_joint_positionsSIN(iter); // n+6
595 const Eigen::VectorXd& dq = m_joint_velocitiesSIN(iter);
596 assert(qj.size() ==
597 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
598 "Unexpected size of signal joint_positions");
599 assert(dq.size() ==
600 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
601 "Unexpected size of signal joint_velocities");
602
603 /* convert sot to pinocchio joint order */
604 m_robot_util->joints_sot_to_urdf(qj, m_q_pin.tail(m_robot_util->m_nbJoints));
605 m_robot_util->joints_sot_to_urdf(dq, m_v_pin.tail(m_robot_util->m_nbJoints));
606
607 getProfiler().start(PROFILE_BASE_KINEMATICS_COMPUTATION);
608
609 /* Compute kinematics assuming world is at free-flyer frame */
610 m_q_pin.head<6>().setZero();
611 m_q_pin(6) = 1.0;
612 m_v_pin.head<6>().setZero();
613 pinocchio::forwardKinematics(m_model, *m_data, m_q_pin, m_v_pin);
614 pinocchio::framesForwardKinematics(m_model, *m_data);
615
616 getProfiler().stop(PROFILE_BASE_KINEMATICS_COMPUTATION);
617
618 return s;
619}
620
621DEFINE_SIGNAL_OUT_FUNCTION(q, dynamicgraph::Vector) {
622 if (!m_initSucceeded) {
623 SEND_WARNING_STREAM_MSG("Cannot compute signal q before initialization!");
624 return s;
625 }
626 if (s.size() !=
627 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6))
628 s.resize(m_robot_util->m_nbJoints + 6);
629
630 const Eigen::VectorXd& qj = m_joint_positionsSIN(iter); // n+6
631 const Eigen::Vector4d& quatIMU_vec = m_imu_quaternionSIN(iter);
632 const Vector6& ftrf = m_forceRLEGSIN(iter);
633 const Vector6& ftlf = m_forceLLEGSIN(iter);
634
635 // if the weights are not specified by the user through the input signals
636 // w_lf, w_rf then compute them if one feet is not stable, force weight to 0.0
637 double wL, wR;
638 if (m_w_lf_inSIN.isPlugged())
639 wL = m_w_lf_inSIN(iter);
640 else {
641 wL = m_w_lf_filteredSOUT(iter);
642 if (m_left_foot_is_stable == false) wL = 0.0;
643 }
644 if (m_w_rf_inSIN.isPlugged())
645 wR = m_w_rf_inSIN(iter);
646 else {
647 wR = m_w_rf_filteredSOUT(iter);
648 if (m_right_foot_is_stable == false) wR = 0.0;
649 }
650
651 assert(qj.size() ==
652 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
653 "Unexpected size of signal joint_positions");
654
655 // if both weights are zero set them to a small positive value to avoid
656 // division by zero
657 if (wR == 0.0 && wL == 0.0) {
658 SEND_WARNING_STREAM_MSG(
659 "The robot is flying!" +
660 ("- forceRLEG: " + toString(ftrf.transpose())) +
661 "- forceLLEG: " + toString(ftlf.transpose()) +
662 "- m_right_foot_is_stable: " + toString(m_right_foot_is_stable) +
663 "- m_left_foot_is_stable: " + toString(m_left_foot_is_stable));
664 wR = 1e-3;
665 wL = 1e-3;
666 }
667
668 m_kinematics_computationsSINNER(iter);
669
670 if (m_reset_foot_pos) reset_foot_positions_impl(ftlf, ftrf);
671
672 getProfiler().start(PROFILE_BASE_POSITION_ESTIMATION);
673 {
674 SE3 oMlfa, oMrfa, lfsMff, rfsMff;
675 kinematics_estimation(ftrf, m_K_rf, m_oMrfs,
676 static_cast<int>(m_right_foot_id), m_oMff_rf, oMrfa,
677 rfsMff);
678 kinematics_estimation(ftlf, m_K_lf, m_oMlfs,
679 static_cast<int>(m_left_foot_id), m_oMff_lf, oMlfa,
680 lfsMff);
681
682 // get rpy
683 const SE3 ffMchest(
684 m_data->oMf[m_IMU_body_id]); // transform between freeflyer and body
685 // attached to IMU sensor
686 const SE3 chestMff(ffMchest.inverse()); // transform between body attached
687 // to IMU sensor and freeflyer
688
689 Vector3 rpy_chest, rpy_chest_lf, rpy_chest_rf,
690 rpy_chest_imu; // orientation of the body which imu is attached to.
691 // (fusion, from left kine, from right kine, from imu)
692
693 matrixToRpy((m_oMff_lf * ffMchest).rotation(), rpy_chest_lf);
694 matrixToRpy((m_oMff_rf * ffMchest).rotation(), rpy_chest_rf);
695 Eigen::Quaternion<double> quatIMU(quatIMU_vec[0], quatIMU_vec[1],
696 quatIMU_vec[2], quatIMU_vec[3]);
697 matrixToRpy(quatIMU.toRotationMatrix(), rpy_chest_imu);
698
699 // average (we do not take into account the IMU yaw)
700 double wSum = wL + wR + m_w_imu;
701 rpy_chest(0) = (rpy_chest_lf[0] * wL + rpy_chest_rf[0] * wR +
702 rpy_chest_imu[0] * m_w_imu) /
703 wSum;
704 rpy_chest(1) = (rpy_chest_lf[1] * wL + rpy_chest_rf[1] * wR +
705 rpy_chest_imu[1] * m_w_imu) /
706 wSum;
707 rpy_chest(2) = (rpy_chest_lf[2] * wL + rpy_chest_rf[2] * wR) / (wL + wR);
708
709 rpyToMatrix(rpy_chest, m_oRchest);
710 m_oRff = m_oRchest * chestMff.rotation();
711
712 // translation to get foot at the right position
713 // evaluate Mrl Mlf for q with the good orientation and zero translation for
714 // freeflyer
715 const Vector3 pos_lf_ff =
716 m_oRff * m_data->oMf[m_left_foot_id].translation();
717 const Vector3 pos_rf_ff =
718 m_oRff * m_data->oMf[m_right_foot_id].translation();
719 // get average translation
720 m_q_pin.head<3>() = ((oMlfa.translation() - pos_lf_ff) * wL +
721 (oMrfa.translation() - pos_rf_ff) * wR) /
722 (wL + wR);
723
724 m_q_sot.tail(m_robot_util->m_nbJoints) = qj;
725 base_se3_to_sot(m_q_pin.head<3>(), m_oRff, m_q_sot.head<6>());
726
727 s = m_q_sot;
728
729 // store estimation of the base pose in SE3 format
730 const SE3 oMff_est(m_oRff, m_q_pin.head<3>());
731
732 // feedback on feet poses
733 if (m_K_fb_feet_posesSIN.isPlugged()) {
734 const double K_fb = m_K_fb_feet_posesSIN(iter);
735 if (K_fb > 0.0 && m_w_imu > 0.0) {
736 assert(m_w_imu > 0.0 &&
737 "Update of the feet 6d poses should not be done if wIMU = 0.0");
738 assert(K_fb < 1.0 &&
739 "Feedback gain on foot correction should be less than 1.0 "
740 "(K_fb_feet_poses>1.0)");
741 // feet positions in the world according to current base estimation
742 const SE3 oMlfs_est(oMff_est * (lfsMff.inverse()));
743 const SE3 oMrfs_est(oMff_est * (rfsMff.inverse()));
744 // error in current foot position
745 SE3 leftDrift = m_oMlfs.inverse() * oMlfs_est;
746 SE3 rightDrift = m_oMrfs.inverse() * oMrfs_est;
747
749 SE3 leftDrift_delta;
750 SE3 rightDrift_delta;
751 se3Interp(leftDrift, SE3::Identity(), K_fb * wR, leftDrift_delta);
752 se3Interp(rightDrift, SE3::Identity(), K_fb * wL, rightDrift_delta);
753 // if a feet is not stable on the ground (aka flying), fully update his
754 // position
755 if (m_right_foot_is_stable == false) rightDrift_delta = rightDrift;
756 if (m_left_foot_is_stable == false) leftDrift_delta = leftDrift;
757 if (m_right_foot_is_stable == false && m_left_foot_is_stable == false) {
758 // robot is jumping, do not update any feet position
759 rightDrift_delta = SE3::Identity();
760 leftDrift_delta = SE3::Identity();
761 }
762 m_oMlfs = m_oMlfs * leftDrift_delta;
763 m_oMrfs = m_oMrfs * rightDrift_delta;
764 // dedrift (x, y, z, yaw) using feet pose references
765 SE3 oMlfs_ref, oMrfs_ref;
766 if (m_lf_ref_xyzquatSIN.isPlugged() and
767 m_rf_ref_xyzquatSIN.isPlugged()) {
769 const Vector7& lf_ref_xyzquat_vec = m_lf_ref_xyzquatSIN(iter);
770 const Vector7& rf_ref_xyzquat_vec = m_rf_ref_xyzquatSIN(iter);
771 const Eigen::Quaterniond ql(
772 m_lf_ref_xyzquatSIN(iter)(3), m_lf_ref_xyzquatSIN(iter)(4),
773 m_lf_ref_xyzquatSIN(iter)(5), m_lf_ref_xyzquatSIN(iter)(6));
774 const Eigen::Quaterniond qr(
775 m_rf_ref_xyzquatSIN(iter)(3), m_rf_ref_xyzquatSIN(iter)(4),
776 m_rf_ref_xyzquatSIN(iter)(5), m_rf_ref_xyzquatSIN(iter)(6));
777 oMlfs_ref = SE3(ql.toRotationMatrix(), lf_ref_xyzquat_vec.head<3>());
778 oMrfs_ref = SE3(qr.toRotationMatrix(), rf_ref_xyzquat_vec.head<3>());
779 } else {
780 oMlfs_ref = m_oMlfs_default_ref;
781 oMrfs_ref = m_oMrfs_default_ref;
782 }
785 const Vector3 translation_feet_drift =
786 0.5 * ((oMlfs_ref.translation() - m_oMlfs.translation()) +
787 (oMrfs_ref.translation() - m_oMrfs.translation()));
789 const Vector3 V_ref = oMrfs_ref.translation() - oMlfs_ref.translation();
790 const Vector3 V_est = m_oMrfs.translation() - m_oMlfs.translation();
793 const double yaw_drift =
794 (atan2(V_ref(1), V_ref(0)) - atan2(V_est(1), V_est(0)));
795 //~ printf("yaw_drift=%lf\r\n",yaw_drift);
797 const Vector3 rpy_feet_drift(0., 0., yaw_drift);
798 Matrix3 rotation_feet_drift;
799 rpyToMatrix(rpy_feet_drift, rotation_feet_drift);
800 const SE3 drift_to_ref(rotation_feet_drift, translation_feet_drift);
801 m_oMlfs = m_oMlfs * drift_to_ref;
802 m_oMrfs = m_oMrfs * drift_to_ref;
803 }
804 }
805 // convert to xyz+quaternion format //Rq: this convertions could be done in
806 // outupt signals function?
807 m_oMlfs_xyzquat.head<3>() = m_oMlfs.translation();
808 Eigen::Quaternion<double> quat_lf(m_oMlfs.rotation());
809 m_oMlfs_xyzquat(3) = quat_lf.w();
810 m_oMlfs_xyzquat(4) = quat_lf.x();
811 m_oMlfs_xyzquat(5) = quat_lf.y();
812 m_oMlfs_xyzquat(6) = quat_lf.z();
813
814 m_oMrfs_xyzquat.head<3>() = m_oMrfs.translation();
815 Eigen::Quaternion<double> quat_rf(m_oMrfs.rotation());
816 m_oMrfs_xyzquat(3) = quat_rf.w();
817 m_oMrfs_xyzquat(4) = quat_rf.x();
818 m_oMrfs_xyzquat(5) = quat_rf.y();
819 m_oMrfs_xyzquat(6) = quat_rf.z();
820 }
821 getProfiler().stop(PROFILE_BASE_POSITION_ESTIMATION);
822 return s;
823}
824
825DEFINE_SIGNAL_OUT_FUNCTION(lf_xyzquat, dynamicgraph::Vector) {
826 if (!m_initSucceeded) {
827 SEND_WARNING_STREAM_MSG(
828 "Cannot compute signal lf_xyzquat before initialization! iter" +
829 toString(iter));
830 return s;
831 }
832 if (s.size() != 7) s.resize(7);
833 s = m_oMlfs_xyzquat;
834 return s;
835}
836
837DEFINE_SIGNAL_OUT_FUNCTION(rf_xyzquat, dynamicgraph::Vector) {
838 if (!m_initSucceeded) {
839 SEND_WARNING_STREAM_MSG(
840 "Cannot compute signal rf_xyzquat before initialization! iter" +
841 toString(iter));
842 return s;
843 }
844 if (s.size() != 7) s.resize(7);
845 s = m_oMrfs_xyzquat;
846 return s;
847}
848
849DEFINE_SIGNAL_OUT_FUNCTION(q_lf, dynamicgraph::Vector) {
850 if (!m_initSucceeded) {
851 SEND_WARNING_STREAM_MSG(
852 "Cannot compute signal q_lf before initialization!");
853 return s;
854 }
855 if (s.size() !=
856 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6))
857 s.resize(m_robot_util->m_nbJoints + 6);
858
859 const Eigen::VectorXd& q = m_qSOUT(iter);
860 s.tail(m_robot_util->m_nbJoints) = q.tail(m_robot_util->m_nbJoints);
861 base_se3_to_sot(m_oMff_lf.translation(), m_oMff_lf.rotation(), s.head<6>());
862
863 return s;
864}
865
866DEFINE_SIGNAL_OUT_FUNCTION(q_rf, dynamicgraph::Vector) {
867 if (!m_initSucceeded) {
868 SEND_WARNING_STREAM_MSG(
869 "Cannot compute signal q_rf before initialization!");
870 return s;
871 }
872 if (s.size() !=
873 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6))
874 s.resize(m_robot_util->m_nbJoints + 6);
875
876 const Eigen::VectorXd& q = m_qSOUT(iter);
877 s.tail(m_robot_util->m_nbJoints) = q.tail(m_robot_util->m_nbJoints);
878 base_se3_to_sot(m_oMff_rf.translation(), m_oMff_rf.rotation(), s.head<6>());
879
880 return s;
881}
882
883DEFINE_SIGNAL_OUT_FUNCTION(q_imu, dynamicgraph::Vector) {
884 if (!m_initSucceeded) {
885 SEND_WARNING_STREAM_MSG(
886 "Cannot compute signal q_imu before initialization!");
887 return s;
888 }
889 if (s.size() !=
890 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6))
891 s.resize(m_robot_util->m_nbJoints + 6);
892
893 const Eigen::VectorXd& q = m_qSOUT(iter);
894 s.tail(m_robot_util->m_nbJoints) = q.tail(m_robot_util->m_nbJoints);
895
896 const Eigen::Vector4d& quatIMU_vec = m_imu_quaternionSIN(iter);
897 Eigen::Quaternion<double> quatIMU(quatIMU_vec);
898 base_se3_to_sot(q.head<3>(), quatIMU.toRotationMatrix(), s.head<6>());
899
900 return s;
901}
902
904 if (!m_initSucceeded) {
905 SEND_WARNING_STREAM_MSG(
906 "Cannot compute signal w_lf before initialization!");
907 return s;
908 }
909
910 const Vector6& wrench = m_forceLLEGSIN(iter);
911 Vector2 zmp;
912 zmp.setZero();
913 compute_zmp(wrench, zmp);
914 double w_zmp = compute_zmp_weight(zmp, m_left_foot_sizes, m_zmp_std_dev_lf,
915 m_zmp_margin_lf);
916 double w_fz =
917 compute_force_weight(wrench(2), m_fz_std_dev_lf, m_fz_margin_lf);
918 // check that foot is sensing a force greater than the margin treshold for
919 // more than 'm_fz_stable_windows_size' samples
920 if (wrench(2) > m_fz_margin_lf)
921 m_lf_fz_stable_cpt++;
922 else
923 m_lf_fz_stable_cpt = 0;
924
925 if (m_lf_fz_stable_cpt >= m_fz_stable_windows_size) {
926 m_lf_fz_stable_cpt = m_fz_stable_windows_size;
927 m_left_foot_is_stable = true;
928 } else {
929 m_left_foot_is_stable = false;
930 }
931 s = w_zmp * w_fz;
932 return s;
933}
934
936 if (!m_initSucceeded) {
937 SEND_WARNING_STREAM_MSG(
938 "Cannot compute signal w_rf before initialization!");
939 return s;
940 }
941
942 const Vector6& wrench = m_forceRLEGSIN(iter);
943 Vector2 zmp;
944 zmp.setZero();
945 compute_zmp(wrench, zmp);
946 double w_zmp = compute_zmp_weight(zmp, m_right_foot_sizes, m_zmp_std_dev_rf,
947 m_zmp_margin_rf);
948 double w_fz =
949 compute_force_weight(wrench(2), m_fz_std_dev_rf, m_fz_margin_rf);
950 // check that foot is sensing a force greater than the margin treshold for
951 // more than 'm_fz_stable_windows_size' samples
952 if (wrench(2) > m_fz_margin_rf)
953 m_rf_fz_stable_cpt++;
954 else
955 m_rf_fz_stable_cpt = 0;
956
957 if (m_rf_fz_stable_cpt >= m_fz_stable_windows_size) {
958 m_rf_fz_stable_cpt = m_fz_stable_windows_size;
959 m_right_foot_is_stable = true;
960 } else {
961 m_right_foot_is_stable = false;
962 }
963 s = w_zmp * w_fz;
964 return s;
965}
966
967DEFINE_SIGNAL_OUT_FUNCTION(w_rf_filtered, double) {
968 if (!m_initSucceeded) {
969 SEND_WARNING_STREAM_MSG(
970 "Cannot compute signal w_rf_filtered before initialization!");
971 return s;
972 }
973 double w_rf = m_w_rfSOUT(iter);
974 m_w_rf_filtered =
975 m_alpha_w_filter * w_rf +
976 (1 - m_alpha_w_filter) * m_w_rf_filtered; // low pass filter
977 s = m_w_rf_filtered;
978 return s;
979}
980
981DEFINE_SIGNAL_OUT_FUNCTION(w_lf_filtered, double) {
982 if (!m_initSucceeded) {
983 SEND_WARNING_STREAM_MSG(
984 "Cannot compute signal w_lf_filtered before initialization!");
985 return s;
986 }
987 double w_lf = m_w_lfSOUT(iter);
988 m_w_lf_filtered =
989 m_alpha_w_filter * w_lf +
990 (1 - m_alpha_w_filter) * m_w_lf_filtered; // low pass filter
991 s = m_w_lf_filtered;
992 return s;
993}
994
995DEFINE_SIGNAL_OUT_FUNCTION(v, dynamicgraph::Vector) {
996 if (!m_initSucceeded) {
997 SEND_WARNING_STREAM_MSG("Cannot compute signal v before initialization!");
998 return s;
999 }
1000 if (s.size() !=
1001 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6))
1002 s.resize(m_robot_util->m_nbJoints + 6);
1003
1004 m_kinematics_computationsSINNER(iter);
1005 m_qSOUT(iter);
1006
1007 getProfiler().start(PROFILE_BASE_VELOCITY_ESTIMATION);
1008 {
1009 const Eigen::VectorXd& dq = m_joint_velocitiesSIN(iter);
1010 const Eigen::Vector3d& acc_imu = m_accelerometerSIN(iter);
1011 const Eigen::Vector3d& gyr_imu = m_gyroscopeSIN(iter);
1012 const Vector6& dftrf = m_dforceRLEGSIN(iter);
1013 const Vector6& dftlf = m_dforceLLEGSIN(iter);
1014 assert(dq.size() ==
1015 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
1016 "Unexpected size of signal joint_velocities");
1017
1018 // if the weights are not specified by the user through the input signals
1019 // w_lf, w_rf then compute them if one feet is not stable, force weight to
1020 // 0.0
1021 double wL, wR;
1022 if (m_w_lf_inSIN.isPlugged())
1023 wL = m_w_lf_inSIN(iter);
1024 else {
1025 wL = m_w_lf_filteredSOUT(iter);
1026 if (m_left_foot_is_stable == false) wL = 0.0;
1027 }
1028 if (m_w_rf_inSIN.isPlugged())
1029 wR = m_w_rf_inSIN(iter);
1030 else {
1031 wR = m_w_rf_filteredSOUT(iter);
1032 if (m_right_foot_is_stable == false) wR = 0.0;
1033 }
1034 // if both weights are zero set them to a small positive value to avoid
1035 // division by zero
1036 if (wR == 0.0 && wL == 0.0) {
1037 wR = 1e-3;
1038 wL = 1e-3;
1039 }
1040
1041 /* Compute foot velocities */
1042 const Frame& f_lf = m_model.frames[m_left_foot_id];
1043 const Motion v_lf_local = m_data->v[f_lf.parent];
1044 const SE3 ffMlf = m_data->oMi[f_lf.parent];
1045 Vector6 v_kin_l =
1046 -ffMlf.act(v_lf_local).toVector(); // this is the velocity of the base
1047 // in the frame of the base.
1048 v_kin_l.head<3>() = m_oRff * v_kin_l.head<3>();
1049 v_kin_l.segment<3>(3) = m_oRff * v_kin_l.segment<3>(3);
1050
1051 const Frame& f_rf = m_model.frames[m_right_foot_id];
1052 const Motion v_rf_local = m_data->v[f_rf.parent];
1053 const SE3 ffMrf = m_data->oMi[f_rf.parent];
1054 Vector6 v_kin_r =
1055 -ffMrf.act(v_rf_local).toVector(); // this is the velocity of the base
1056 // in the frame of the base.
1057 v_kin_r.head<3>() = m_oRff * v_kin_r.head<3>();
1058 v_kin_r.segment<3>(3) = m_oRff * v_kin_r.segment<3>(3);
1059
1060 m_v_kin.head<6>() = (wR * v_kin_r + wL * v_kin_l) / (wL + wR);
1061
1062 /* Compute velocity induced by the flexibility */
1063 Vector6 v_flex_l;
1064 Vector6 v_flex_r;
1065 v_flex_l << -dftlf[0] / m_K_lf(0), -dftlf[1] / m_K_lf(1),
1066 -dftlf[2] / m_K_lf(2), -dftlf[3] / m_K_lf(3), -dftlf[4] / m_K_lf(4),
1067 -dftlf[5] / m_K_lf(5);
1068 v_flex_r << -dftrf[0] / m_K_rf(0), -dftrf[1] / m_K_rf(1),
1069 -dftrf[2] / m_K_rf(2), -dftrf[3] / m_K_rf(3), -dftrf[4] / m_K_rf(4),
1070 -dftrf[5] / m_K_rf(5);
1071 const Eigen::Matrix<double, 6, 6> lfAff = ffMlf.inverse().toActionMatrix();
1072 const Eigen::Matrix<double, 6, 6> rfAff = ffMrf.inverse().toActionMatrix();
1073 Eigen::Matrix<double, 12, 6> A;
1074 A << lfAff, rfAff;
1075 Eigen::Matrix<double, 12, 1> b;
1076 b << v_flex_l, v_flex_r;
1077 //~ m_v_flex.head<6>() = A.jacobiSvd(Eigen::ComputeThinU |
1078 // Eigen::ComputeThinV).solve(b);
1079 m_v_flex.head<6>() = (A.transpose() * A).ldlt().solve(A.transpose() * b);
1080 m_v_flex.head<3>() = m_oRff * m_v_flex.head<3>();
1081
1082 /* Get an estimate of linear velocities from gyroscope only*/
1083 // we make the asumtion than we are 'turning around the foot' with pure
1084 // angular velocity in the ankle measured by the gyro
1085 const Matrix3 ffRimu = (m_data->oMf[m_IMU_body_id]).rotation();
1086 const Matrix3 lfRimu = ffMlf.rotation().transpose() * ffRimu;
1087 const Matrix3 rfRimu = ffMrf.rotation().transpose() * ffRimu;
1088
1089 const SE3 chestMimu(
1090 Matrix3::Identity(),
1091 Vector3(-0.13, 0.0,
1092 0.118));
1094 const SE3 ffMchest(m_data->oMf[m_IMU_body_id]);
1095 const SE3 imuMff = (ffMchest * chestMimu).inverse();
1096 // gVw_a = gVo_g + gHa.act(aVb_a)-gVb_g //angular velocity in the ankle
1097 // from gyro and d_enc
1098 const Frame& f_imu = m_model.frames[m_IMU_body_id];
1099 Vector3 gVo_a_l = Vector3(gyr_imu(0), gyr_imu(1), gyr_imu(2)) +
1100 (imuMff * ffMlf).act(v_lf_local).angular() -
1101 m_data->v[f_imu.parent].angular();
1102 Vector3 gVo_a_r = Vector3(gyr_imu(0), gyr_imu(1), gyr_imu(2)) +
1103 (imuMff * ffMrf).act(v_rf_local).angular() -
1104 m_data->v[f_imu.parent].angular();
1105 Motion v_gyr_ankle_l(Vector3(0., 0., 0.), lfRimu * gVo_a_l);
1106 Motion v_gyr_ankle_r(Vector3(0., 0., 0.), rfRimu * gVo_a_r);
1107 Vector6 v_gyr_l = -ffMlf.inverse().act(v_gyr_ankle_l).toVector();
1108 Vector6 v_gyr_r = -ffMrf.inverse().act(v_gyr_ankle_r).toVector();
1109 m_v_gyr.head<6>() = (wL * v_gyr_l + wR * v_gyr_r) / (wL + wR);
1110
1111 /* Get an estimate of linear velocities from filtered accelerometer
1112 * integration */
1113
1114 /* rotate acceleration to express it in the world frame */
1115 //~ pointRotationByQuaternion(acc_imu,quatIMU_vec,acc_world); //this is
1116 // false because yaw from imuquat is drifting
1117 const Vector3 acc_world = m_oRchest * acc_imu;
1118
1119 /* now, the acceleration is expressed in the world frame,
1120 * so it can be written as the sum of the proper acceleration and the
1121 * constant gravity vector. We could remove this assuming a [0,0,9.81]
1122 * but we prefer to filter this signal with low frequency high pass
1123 * filter since it is robust to gravity norm error, and we know that
1124 * globaly the robot can not accelerate continuously. */
1125
1127 if (m_isFirstIterFlag) {
1128 m_last_DCacc = acc_world; // Copy the first acceleration data
1129 m_isFirstIterFlag = false;
1130 sendMsg("iter:" + toString(iter) + "\n", MSG_TYPE_INFO);
1131 }
1132 const Vector3 DCacc =
1133 acc_world * (1 - m_alpha_DC_acc) + m_last_DCacc * (m_alpha_DC_acc);
1134 //~ sendMsg("acc_world :"+toString(acc_world)+"\n",
1135 // MSG_TYPE_INFO);
1136 m_last_DCacc = DCacc;
1137 const Vector3 ACacc = acc_world - DCacc;
1138 m_v_ac = ACacc;
1139 /* Then this acceleration is integrated. */
1140 const Vector3 vel = m_last_vel + ACacc * m_dt;
1141 m_last_vel = vel;
1142 /* To stabilise the integrated velocity, we add the
1143 * asumption that globaly, velocity is zero. So we remove DC again */
1144 const Vector3 DCvel =
1145 vel * (1 - m_alpha_DC_vel) + m_last_DCvel * (m_alpha_DC_vel);
1146 m_last_DCvel = DCvel;
1147 const Vector3 ACvel = vel - DCvel;
1148 m_v_ac = ACvel;
1149
1150 /* Express back velocity in the imu frame to get a full 6d velocity with the
1151 * gyro*/
1152 const Vector3 imuVimu = m_oRchest.transpose() * ACvel;
1153 /* Here we could remove dc from gyrometer to remove bias*/
1154 const Motion imuWimu(imuVimu, gyr_imu);
1155 // const Frame & f_imu = m_model.frames[m_IMU_body_id];
1156 // const SE3 ffMchest(m_data->oMf[m_IMU_body_id]);
1157 // const SE3 chestMimu(Matrix3::Identity(), +1.0*Vector3(-0.13, 0.0,
1158 // 0.118)); ///TODO Read this transform from setable parameter /// TODO
1159 // chesk the sign of the translation
1160 const SE3 ffMimu = ffMchest * chestMimu;
1161 const Motion v = ffMimu.act(imuWimu); //- ffWchest;
1162 m_v_imu.head<6>() = v.toVector();
1163 m_v_imu.head<3>() = m_oRff * m_v_imu.head<3>();
1164
1165 //~ m_v_sot.head<6>() = m_v_kin.head<6>();
1166 //~ m_v_sot.head<6>() = m_v_flex.head<6>() + m_v_kin.head<6>();
1167 m_v_sot.head<6>() = m_v_gyr.head<6>() + m_v_kin.head<6>();
1168 // m_v_sot.head<6>() = m_v_gyr.head<6>();
1169 //~ m_v_sot.head<6>() = m_v_imu.head<6>();
1170
1171 m_v_sot.tail(m_robot_util->m_nbJoints) = dq;
1172 m_v_kin.tail(m_robot_util->m_nbJoints) = dq;
1173 m_v_flex.tail(m_robot_util->m_nbJoints) = dq;
1174 m_v_gyr.tail(m_robot_util->m_nbJoints) = dq;
1175 m_v_imu.tail(m_robot_util->m_nbJoints) = dq;
1176 s = m_v_sot;
1177 }
1178 getProfiler().stop(PROFILE_BASE_VELOCITY_ESTIMATION);
1179 return s;
1180}
1181
1182DEFINE_SIGNAL_OUT_FUNCTION(v_kin, dynamicgraph::Vector) {
1183 if (!m_initSucceeded) {
1184 SEND_WARNING_STREAM_MSG(
1185 "Cannot compute signal v_kin before initialization!");
1186 return s;
1187 }
1188 m_vSOUT(iter);
1189 s = m_v_kin;
1190 return s;
1191}
1192
1193DEFINE_SIGNAL_OUT_FUNCTION(v_flex, dynamicgraph::Vector) {
1194 if (!m_initSucceeded) {
1195 SEND_WARNING_STREAM_MSG(
1196 "Cannot compute signal v_flex before initialization!");
1197 return s;
1198 }
1199 m_vSOUT(iter);
1200 s = m_v_flex + m_v_kin;
1201 return s;
1202}
1203
1204DEFINE_SIGNAL_OUT_FUNCTION(v_imu, dynamicgraph::Vector) {
1205 if (!m_initSucceeded) {
1206 SEND_WARNING_STREAM_MSG(
1207 "Cannot compute signal v_imu before initialization!");
1208 return s;
1209 }
1210 m_vSOUT(iter);
1211 s = m_v_imu;
1212 return s;
1213}
1214
1215DEFINE_SIGNAL_OUT_FUNCTION(v_gyr, dynamicgraph::Vector) {
1216 if (!m_initSucceeded) {
1217 SEND_WARNING_STREAM_MSG(
1218 "Cannot compute signal v_gyr before initialization!");
1219 return s;
1220 }
1221 m_vSOUT(iter);
1222 s = m_v_gyr;
1223 return s;
1224}
1225
1226DEFINE_SIGNAL_OUT_FUNCTION(v_ac, dynamicgraph::Vector) {
1227 if (!m_initSucceeded) {
1228 SEND_WARNING_STREAM_MSG(
1229 "Cannot compute signal v_ac before initialization!");
1230 return s;
1231 }
1232 m_vSOUT(iter);
1233 s = m_v_ac;
1234 return s;
1235}
1236
1237DEFINE_SIGNAL_OUT_FUNCTION(a_ac, dynamicgraph::Vector) {
1238 if (!m_initSucceeded) {
1239 SEND_WARNING_STREAM_MSG(
1240 "Cannot compute signal a_ac before initialization!");
1241 return s;
1242 }
1243 m_vSOUT(iter);
1244 s = m_a_ac;
1245 return s;
1246}
1247
1248/* --- COMMANDS ---------------------------------------------------------- */
1249
1250/* ------------------------------------------------------------------- */
1251/* --- ENTITY -------------------------------------------------------- */
1252/* ------------------------------------------------------------------- */
1253
1254void BaseEstimator::display(std::ostream& os) const {
1255 os << "BaseEstimator " << getName();
1256 try {
1257 getProfiler().report_all(3, os);
1258 } catch (ExceptionSignal e) {
1259 }
1260}
1261} // namespace torque_control
1262} // namespace sot
1263} // namespace dynamicgraph
#define INPUT_SIGNALS
#define OUTPUT_SIGNALS
#define PROFILE_BASE_POSITION_ESTIMATION
#define PROFILE_BASE_VELOCITY_ESTIMATION
#define PROFILE_BASE_KINEMATICS_COMPUTATION
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
double m_fz_margin_lf
margin used for computing zmp weight
SE3 m_oMlfs
world-to-base transformation obtained through right foot
void reset_foot_positions_impl(const Vector6 &ftlf, const Vector6 &ftrf)
Eigen::VectorXd m_v_pin
robot configuration according to SoT convention
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BaseEstimator(const std::string &name)
void set_stiffness_left_foot(const dynamicgraph::Vector &k)
Eigen::VectorXd m_v_kin
6d stiffness of left foot spring
void kinematics_estimation(const Vector6 &ft, const Vector6 &K, const SE3 &oMfs, const int foot_id, SE3 &oMff, SE3 &oMfa, SE3 &fsMff)
RobotUtilShrPtr m_robot_util
sampling time step
Vector3 m_last_vel
base orientation in the world
pinocchio::Data * m_data
Pinocchio robot model.
double m_zmp_margin_rf
margin used for computing zmp weight
double m_zmp_std_dev_rf
weight of IMU for sensor fusion
double m_dt
true after the command resetFootPositions is called
double m_fz_margin_rf
margin used for computing normal force weight
void set_stiffness_right_foot(const dynamicgraph::Vector &k)
virtual void display(std::ostream &os) const
filtered weight of the estimation coming from the right foot
void set_normal_force_margin_right_foot(const double &margin)
double compute_zmp_weight(const Vector2 &zmp, const Vector4 &foot_sizes, double std_dev, double margin)
void set_zmp_std_dev_right_foot(const double &std_dev)
void compute_zmp(const Vector6 &w, Vector2 &zmp)
void set_zmp_std_dev_left_foot(const double &std_dev)
void set_normal_force_std_dev_right_foot(const double &std_dev)
void init(const double &dt, const std::string &urdfFile)
double compute_force_weight(double fz, double std_dev, double margin)
Eigen::VectorXd m_v_sot
robot velocities according to pinocchio convention
Eigen::VectorXd m_q_sot
robot configuration according to pinocchio convention
Vector6 m_K_rf
margin used for computing normal force weight
bool m_reset_foot_pos
true if the entity has been successfully initialized
Vector7 m_oMlfs_xyzquat
transformation from world to right foot sole
SE3 m_oMrfs
transformation from world to left foot sole
pinocchio::FrameIndex m_right_foot_id
foot sole to F/T sensor transformation
void set_right_foot_sizes(const dynamicgraph::Vector &s)
void set_normal_force_std_dev_left_foot(const double &std_dev)
Vector6 m_K_lf
6d stiffness of right foot spring
void set_left_foot_sizes(const dynamicgraph::Vector &s)
void set_normal_force_margin_left_foot(const double &margin)
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
void pointRotationByQuaternion(const Eigen::Vector3d &point, const Eigen::Vector4d &quat, Eigen::Vector3d &rotatedPoint)
void quanternionMult(const Eigen::Vector4d &q1, const Eigen::Vector4d &q2, Eigen::Vector4d &q12)
Eigen::Matrix< double, 3, 1 > Vector3
DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
Eigen::Matrix< double, 6, 1 > Vector6
void se3Interp(const pinocchio::SE3 &s1, const pinocchio::SE3 &s2, const double alpha, pinocchio::SE3 &s12)
void rpyToMatrix(double r, double p, double y, Eigen::Matrix3d &R)
void matrixToRpy(const Eigen::Matrix3d &M, Eigen::Vector3d &rpy)
to read text file
Definition: treeview.dox:22