Correct terminology regarding pressure and foot force ratios

What the controller refers to are foot force differences (in admittance
control) and ratios (in wrench distribution). Pressure is defined as normal
contact force per unit area, so that's a different thing (and it doesn't have
the unit of a force).

CHANGELOG.md

@@ -6,6 +6,7 @@ All notable changes to this project will be documented in this file.
 
 ### Fixed
 
+- Terminology regarding pressure and foot force ratios
 - ZMP frame initialization
 
 ## [v1.5] - 2019/12/10

doc/stabilizer.dox

@@ -31,14 +31,13 @@ As an example, until version 1.3 of the controller we introduced a modeling erro
 <tr><td><b>Gain</b></td><td><b>Effect of increasing</b></td></tr>
 <tr><td>Foot: <i>CoPx</i></td><td>Improves CoP control but increases sagittal position compliance</td></tr>
 <tr><td>Foot: <i>CoPy</i></td><td>Improves CoP control but increases lateral position compliance</td></tr>
-<tr><td>Legs: <i>DFz</i></td><td>Improves foot pressure tracking in double support, can yield vertical 
-oscillations</td></tr>
+<tr><td>Legs: <i>DFz</i></td><td>Improves foot force difference tracking in double support, can yield vertical oscillations</td></tr>
 <tr><td>Legs: <i>DFz damping</i></td><td>Damps potential oscillations in double support</td></tr>
 <tr><td>CoM: <i>Ax</i></td><td>Improves ZMP control but increases sagittal position compliance</td></tr>
 <tr><td>CoM: <i>Ay</i></td><td>Improves ZMP control but increases lateral position compliance</td></tr>
 </table>
 
-You can debug them with the ``Tracking DCM X/Y`` and ``Tracking DCM-ZMP X/Y`` plots, except for the foot *DFz* admittance which is better checked in ``Foot pressure both``.
+You can debug them with the ``Tracking DCM X/Y`` and ``Tracking DCM-ZMP X/Y`` plots, except for the foot *DFz* admittance which is better checked in ``Foot force both``.
 
 \section tests Standard tests
 
@@ -59,7 +58,7 @@ All logs will be stored in ``/tmp`` until you reboot your machine.
 
 \subsection standing Standing
 
-Standing tests evaluate the performance of disturbance rejection when you push the robot at the trunk. They are useful to debug 
+Standing tests evaluate the performance of disturbance rejection when you push the robot at the trunk.
 
 - Put the robot on the ground in standing posture
 - Push the robot sagitally or laterally until the measured ZMP at ground level is halfway (or all the way) between its rest position and the boundary of the support area

etc/LIPMWalking.conf.cmake

@@ -15,7 +15,7 @@
     {
       "net_wrench": 10000.0,
       "ankle_torque": 100.0,
-      "pressure": 1.0
+      "force_ratio": 1.0
     },
     "tasks":
     {

etc/mc_log_ui/custom_plot.json

@@ -2,10 +2,9 @@
     ["3D CoM", "t", ["pendulum_com_x", "controlRobot_com_x", "realRobot_com_x", "pendulum_com_y", "controlRobot_com_y", "realRobot_com_y", "pendulum_com_z", "controlRobot_com_z", "realRobot_com_z"], [], [], []],
     ["3D DCM", "t", ["pendulum_dcm_x", "realRobot_dcm_x", "pendulum_dcm_y", "realRobot_dcm_y"], [], [], []],
     ["3D ZMP", "t", ["pendulum_zmp_x", "stabilizer_zmp_x", "realRobot_zmp_x", "pendulum_zmp_y", "stabilizer_zmp_y", "realRobot_zmp_y"], [], [], []],
-    ["Floating-base observers Y", "t", ["observers_flex_posW_ty", "observers_kin_posW_ty", "pIn_y"], [], [], []],
-    ["Foot pressure both", "t", ["cop_jvrc1_LeftFootCenter_measured_wrench_fz", "cop_jvrc1_LeftFootCenter_target_wrench_fz", "cop_jvrc1_RightFootCenter_measured_wrench_fz", "cop_jvrc1_RightFootCenter_target_wrench_fz"], [], [], []],
-    ["Foot pressure left", "t", ["cop_jvrc1_LeftFootCenter_measured_wrench_fz", "cop_jvrc1_LeftFootCenter_target_wrench_fz"], [], [], []],
-    ["Foot pressure right", "t", ["cop_jvrc1_RightFootCenter_measured_wrench_fz", "cop_jvrc1_RightFootCenter_target_wrench_fz"], [], [], []],
+    ["Foot force both", "t", ["cop_jvrc1_LeftFootCenter_measured_wrench_fz", "cop_jvrc1_LeftFootCenter_target_wrench_fz", "cop_jvrc1_RightFootCenter_measured_wrench_fz", "cop_jvrc1_RightFootCenter_target_wrench_fz"], [], [], []],
+    ["Foot force left", "t", ["cop_jvrc1_LeftFootCenter_measured_wrench_fz", "cop_jvrc1_LeftFootCenter_target_wrench_fz"], [], [], []],
+    ["Foot force right", "t", ["cop_jvrc1_RightFootCenter_measured_wrench_fz", "cop_jvrc1_RightFootCenter_target_wrench_fz"], [], [], []],
     ["Measured CoM-ZMP X", "t", ["realRobot_zmp_x", "support_xmin", "support_xmax", "realRobot_com_x"], [], [], []],
     ["Measured CoM-ZMP Y", "t", ["realRobot_zmp_y", "support_ymin", "support_ymax", "realRobot_com_y"], [], [], []],
     ["Measured DCM-ZMP X", "t", ["support_xmin", "support_xmax", "realRobot_dcm_x", "realRobot_zmp_x"], [], [], []],
@@ -14,8 +13,6 @@
     ["Reference CoM-ZMP Y", "t", ["pendulum_zmp_y", "support_ymin", "support_ymax", "pendulum_com_y"], [], [], []],
     ["Reference DCM-ZMP X", "t", ["support_xmin", "support_xmax", "pendulum_dcm_x", "pendulum_zmp_x"], [], [], []],
     ["Reference DCM-ZMP Y", "t", ["support_ymin", "support_ymax", "pendulum_dcm_y", "pendulum_zmp_y"], [], [], []],
-    ["Swing foot Left", "cop_jvrc1_LeftFootCenter_target_pose_tx", ["cop_jvrc1_LeftFootCenter_surface_pose_tz", "cop_jvrc1_LeftFootCenter_target_pose_tz", "realRobot_LeftFootCenter_tz"], [], [], []],
-    ["Swing foot Right", "cop_jvrc1_RightFootCenter_target_pose_tx", ["cop_jvrc1_RightFootCenter_surface_pose_tz", "cop_jvrc1_RightFootCenter_target_pose_tz", "realRobot_RightFootCenter_tz"], [], [], []],
     ["Swing foot X", "t", ["cop_jvrc1_RightFootCenter_surface_pose_tx", "cop_jvrc1_LeftFootCenter_surface_pose_tx", "cop_jvrc1_RightFootCenter_target_pose_tx", "cop_jvrc1_LeftFootCenter_target_pose_tx", "realRobot_LeftFootCenter_tx", "realRobot_RightFootCenter_tx"], [], [], []],
     ["Swing foot Y", "t", ["cop_jvrc1_RightFootCenter_surface_pose_ty", "cop_jvrc1_LeftFootCenter_surface_pose_ty", "cop_jvrc1_RightFootCenter_target_pose_ty", "cop_jvrc1_LeftFootCenter_target_pose_ty", "realRobot_LeftFootCenter_ty", "realRobot_RightFootCenter_ty"], [], [], []],
     ["Swing foot Z", "t", ["cop_jvrc1_RightFootCenter_surface_pose_tz", "cop_jvrc1_LeftFootCenter_surface_pose_tz", "cop_jvrc1_RightFootCenter_target_pose_tz", "cop_jvrc1_LeftFootCenter_target_pose_tz", "realRobot_LeftFootCenter_tz", "realRobot_RightFootCenter_tz"], [], [], []],

include/lipm_walking/Stabilizer.h

@@ -71,7 +71,7 @@ namespace lipm_walking
     static constexpr double MAX_FDC_RY_VEL = 0.2; /**< Maximum y-axis angular velocity in [rad] / [s] for foot damping control. */
     static constexpr double MAX_FDC_RZ_VEL = 0.2; /**< Maximum z-axis angular velocity in [rad] / [s] for foot damping control. */
     static constexpr double MAX_ZMPCC_COM_OFFSET = 0.05; /**< Maximum CoM offset due to admittance control in [m] */
-    static constexpr double MIN_DS_PRESSURE = 15.; /**< Minimum normal contact force in DSP, used to avoid low-pressure targets when close to contact switches. */
+    static constexpr double MIN_DSP_FZ = 15.; /**< Minimum normal contact force in [N] for DSP, used to avoid low-force targets when close to contact switches. */
 
     /** Initialize stabilizer.
      *
@@ -161,9 +161,9 @@ namespace lipm_walking
      *
      * \param footTask One of leftFootTask or rightFootTask.
      *
-     * This function has no effect when the measured pressure is already higher
-     * than the target. Otherwise, it will set a positive admittance along the
-     * z-axis of the contact frame.
+     * This function has no effect when the measured normal force is already
+     * higher than the target. Otherwise, it will set a positive admittance
+     * along the z-axis of the contact frame.
      *
      */
     void seekTouchdown(std::shared_ptr<mc_tasks::force::CoPTask> footTask);
@@ -222,7 +222,7 @@ namespace lipm_walking
      *
      * \param wrench Net contact wrench in the inertial frame.
      *
-     * \param leftFootRatio Desired pressure distribution ratio for left foot.
+     * \param leftFootRatio Desired force distribution ratio for left foot.
      *
      */
     void updateState(const Eigen::Vector3d & com, const Eigen::Vector3d & comd, const sva::ForceVecd & wrench, double leftFootRatio);
@@ -284,17 +284,17 @@ namespace lipm_walking
       void configure(const mc_rtc::Configuration & config)
       {
         double ankleTorqueWeight = config("ankle_torque");
+        double forceRatioWeight = config("force_ratio");
         double netWrenchWeight = config("net_wrench");
-        double pressureWeight = config("pressure");
         ankleTorqueSqrt = std::sqrt(ankleTorqueWeight);
+        forceRatioSqrt = std::sqrt(forceRatioWeight);
         netWrenchSqrt = std::sqrt(netWrenchWeight);
-        pressureSqrt = std::sqrt(pressureWeight);
       }
 
     public:
       double ankleTorqueSqrt;
+      double forceRatioSqrt;
       double netWrenchSqrt;
-      double pressureSqrt;
     };
 
     /** Check that all gains are within boundaries.

src/Controller.cpp

@@ -608,9 +608,9 @@ namespace lipm_walking
   {
     static bool isInTheAir = false;
     constexpr double CONTACT_THRESHOLD = 30.; // [N]
-    double leftFootPressure = realRobot().forceSensor("LeftFootForceSensor").force().z();
-    double rightFootPressure = realRobot().forceSensor("RightFootForceSensor").force().z();
-    if (leftFootPressure < CONTACT_THRESHOLD && rightFootPressure < CONTACT_THRESHOLD)
+    double leftFootForce = realRobot().forceSensor("LeftFootForceSensor").force().z();
+    double rightFootForce = realRobot().forceSensor("RightFootForceSensor").force().z();
+    if (leftFootForce < CONTACT_THRESHOLD && rightFootForce < CONTACT_THRESHOLD)
     {
       if (!isInTheAir)
       {

src/NetWrenchObserver.cpp

@@ -51,7 +51,7 @@ namespace lipm_walking
     for (std::string sensorName : sensorNames_)
     {
       const auto & sensor = robot.forceSensor(sensorName);
-      if (sensor.force().z() > 1.) // pressure is more than 1 [N]
+      if (sensor.force().z() > 1.) // normal force is more than 1 [N]
       {
         netWrench_ += sensor.worldWrench(robot);
       }

src/Stabilizer.cpp

@@ -53,7 +53,7 @@ namespace lipm_walking
   constexpr double Stabilizer::MAX_FDC_RY_VEL;
   constexpr double Stabilizer::MAX_FDC_RZ_VEL;
   constexpr double Stabilizer::MAX_ZMPCC_COM_OFFSET;
-  constexpr double Stabilizer::MIN_DS_PRESSURE;
+  constexpr double Stabilizer::MIN_DSP_FZ;
 
   namespace
   {
@@ -113,8 +113,8 @@ namespace lipm_walking
     logger.addLogEntry("stabilizer_dcmIntegrator_timeConstant", [this]() { return dcmIntegrator_.timeConstant(); });
     logger.addLogEntry("stabilizer_dfz_damping", [this]() { return dfzDamping_; });
     logger.addLogEntry("stabilizer_fdqp_weights_ankleTorque", [this]() { return std::pow(fdqpWeights_.ankleTorqueSqrt, 2); });
+    logger.addLogEntry("stabilizer_fdqp_weights_forceRatio", [this]() { return std::pow(fdqpWeights_.forceRatioSqrt, 2); });
     logger.addLogEntry("stabilizer_fdqp_weights_netWrench", [this]() { return std::pow(fdqpWeights_.netWrenchSqrt, 2); });
-    logger.addLogEntry("stabilizer_fdqp_weights_pressure", [this]() { return std::pow(fdqpWeights_.pressureSqrt, 2); });
     logger.addLogEntry("stabilizer_vdc_frequency", [this]() { return vdcFrequency_; });
     logger.addLogEntry("stabilizer_vdc_stiffness", [this]() { return vdcStiffness_; });
     logger.addLogEntry("stabilizer_wrench", [this]() { return distribWrench_; });
@@ -200,6 +200,10 @@ namespace lipm_walking
       Button(
         "Reset CoM integrator",
         [this]() { zmpccIntegrator_.setZero(); }),
+      Checkbox(
+        "Apply CoM admittance only in double support?",
+        [this]() { return zmpccOnlyDS_; },
+        [this]() { zmpccOnlyDS_ = !zmpccOnlyDS_; }),
       ArrayInput(
         "CoM admittance",
         {"Ax", "Ay"},
@@ -209,10 +213,6 @@ namespace lipm_walking
           comAdmittance_.x() = clamp(a.x(), 0., MAX_COM_ADMITTANCE);
           comAdmittance_.y() = clamp(a.y(), 0., MAX_COM_ADMITTANCE);
         }),
-      Checkbox(
-        "Apply CoM admittance only in double support?",
-        [this]() { return zmpccOnlyDS_; },
-        [this]() { zmpccOnlyDS_ = !zmpccOnlyDS_; }),
       NumberInput(
         "CoM integrator leak rate [Hz]",
         [this]() { return zmpccIntegrator_.rate(); },
@@ -260,19 +260,19 @@ namespace lipm_walking
         }),
       ArrayInput(
         "Wrench distribution weights",
-        {"Net wrench", "Ankle", "Pressure"},
+        {"Net wrench", "Ankle torques", "Force ratio"},
         [this]() -> Eigen::Vector3d
         {
           double netWrench = std::pow(fdqpWeights_.netWrenchSqrt, 2);
           double ankleTorque = std::pow(fdqpWeights_.ankleTorqueSqrt, 2);
-          double pressure = std::pow(fdqpWeights_.pressureSqrt, 2);
-          return {netWrench, ankleTorque, pressure};
+          double forceRatio = std::pow(fdqpWeights_.forceRatioSqrt, 2);
+          return {netWrench, ankleTorque, forceRatio};
         },
         [this](const Eigen::Vector3d & weights)
         {
           fdqpWeights_.netWrenchSqrt = std::sqrt(weights(0));
           fdqpWeights_.ankleTorqueSqrt = std::sqrt(weights(1));
-          fdqpWeights_.pressureSqrt = std::sqrt(weights(2));
+          fdqpWeights_.forceRatioSqrt = std::sqrt(weights(2));
         }));
     gui->addElement(
       {"Stabilizer", "Errors"},
@@ -475,21 +475,21 @@ namespace lipm_walking
     double xDist = std::abs(X_c_s.translation().x());
     double yDist = std::abs(X_c_s.translation().y());
     double zDist = std::abs(X_c_s.translation().z());
-    double pressure = footTask->measuredWrench().force().z();
-    return (xDist < 0.03 && yDist < 0.03 && zDist < 0.03 && pressure > 50.);
+    double Fz = footTask->measuredWrench().force().z();
+    return (xDist < 0.03 && yDist < 0.03 && zDist < 0.03 && Fz > 50.);
   }
 
   void Stabilizer::seekTouchdown(std::shared_ptr<mc_tasks::force::CoPTask> footTask)
   {
     constexpr double MAX_VEL = 0.01; // [m] / [s]
-    constexpr double TOUCHDOWN_PRESSURE = 50.;  // [N]
-    constexpr double DESIRED_AFZ = MAX_VEL / TOUCHDOWN_PRESSURE;
-    if (footTask->measuredWrench().force().z() < TOUCHDOWN_PRESSURE)
+    constexpr double TOUCHDOWN_FORCE = 50.;  // [N]
+    constexpr double DESIRED_AFZ = MAX_VEL / TOUCHDOWN_FORCE;
+    if (footTask->measuredWrench().force().z() < TOUCHDOWN_FORCE)
     {
       auto a = footTask->admittance();
       double AFz = clamp(DESIRED_AFZ, 0., 1e-2, "Contact seeking admittance");
       footTask->admittance({a.couple(), {a.force().x(), a.force().y(), AFz}});
-      footTask->targetForce({0., 0., TOUCHDOWN_PRESSURE});
+      footTask->targetForce({0., 0., TOUCHDOWN_FORCE});
     }
   }
 
@@ -521,7 +521,7 @@ namespace lipm_walking
   {
     double LFz = leftFootTask->measuredWrench().force().z();
     double RFz = rightFootTask->measuredWrench().force().z();
-    inTheAir_ = (LFz < MIN_DS_PRESSURE && RFz < MIN_DS_PRESSURE);
+    inTheAir_ = (LFz < MIN_DSP_FZ && RFz < MIN_DSP_FZ);
   }
 
   void Stabilizer::updateZMPFrame()
@@ -548,15 +548,15 @@ namespace lipm_walking
     Eigen::Vector3d n = zmpFrame_.rotation().row(2);
     Eigen::Vector3d p = zmpFrame_.translation();
     const Eigen::Vector3d & force = wrench.force();
-    double pressure = n.dot(force);
-    if (pressure < 1.)
+    double normalForce = n.dot(force);
+    if (normalForce < 1.)
     {
       double lambda = std::pow(pendulum_.omega(), 2);
       return measuredCoM_ + world::gravity / lambda; // default for logging
     }
     const Eigen::Vector3d & moment_0 = wrench.couple();
     Eigen::Vector3d moment_p = moment_0 - p.cross(force);
-    return p + n.cross(moment_p) / pressure;
+    return p + n.cross(moment_p) / normalForce;
   }
 
   void Stabilizer::run()
@@ -651,8 +651,8 @@ namespace lipm_walking
     // -----------
     // CWC X_0_lc* w_l_0 <= 0  -- left foot wrench within contact wrench cone
     // CWC X_0_rc* w_r_0 <= 0  -- right foot wrench within contact wrench cone
-    // (X_0_lc* w_l_0).z() > minPressure  -- minimum left foot contact pressure
-    // (X_0_rc* w_r_0).z() > minPressure  -- minimum right foot contact pressure
+    // (X_0_lc* w_l_0).z() > MIN_DSP_FZ  -- minimum left foot contact force
+    // (X_0_rc* w_r_0).z() > MIN_DSP_FZ  -- minimum right foot contact force
 
     const sva::PTransformd & X_0_lc = leftFootContact.pose;
     const sva::PTransformd & X_0_rc = rightFootContact.pose;
@@ -684,9 +684,9 @@ namespace lipm_walking
 
     // |(1 - lfr) * w_l_lc.force().z() - lfr * w_r_rc.force().z()|^2
     double lfr = leftFootRatio_;
-    auto A_pressure = A.block<1, 12>(18, 0);
-    A_pressure.block<1, 6>(0, 0) = (1 - lfr) * X_0_lc.dualMatrix().bottomRows<1>();
-    A_pressure.block<1, 6>(0, 6) = -lfr * X_0_rc.dualMatrix().bottomRows<1>();
+    auto A_fratio = A.block<1, 12>(18, 0);
+    A_fratio.block<1, 6>(0, 0) = (1 - lfr) * X_0_lc.dualMatrix().bottomRows<1>();
+    A_fratio.block<1, 6>(0, 6) = -lfr * X_0_rc.dualMatrix().bottomRows<1>();
 
     // Apply weights
     A_net *= fdqpWeights_.netWrenchSqrt;
@@ -695,8 +695,8 @@ namespace lipm_walking
     A_rankle *= fdqpWeights_.ankleTorqueSqrt;
     // b_lankle = 0
     // b_rankle = 0
-    A_pressure *= fdqpWeights_.pressureSqrt;
-    // b_pressure = 0
+    A_fratio *= fdqpWeights_.forceRatioSqrt;
+    // b_fratio = 0
 
     Eigen::MatrixXd Q = A.transpose() * A;
     Eigen::VectorXd c = -A.transpose() * b;
@@ -712,12 +712,12 @@ namespace lipm_walking
     // CWC * w_r_rc <= 0
     A_ineq.block<16, 6>(16, 6) = wrenchFaceMatrix_ * X_0_rc.dualMatrix();
     // b_ineq.segment<16>(16) is already zero
-    // w_l_lc.force().z() >= MIN_DS_PRESSURE
+    // w_l_lc.force().z() >= MIN_DSP_FZ
     A_ineq.block<1, 6>(32, 0) = -X_0_lc.dualMatrix().bottomRows<1>();
-    b_ineq(32) = -MIN_DS_PRESSURE;
-    // w_r_rc.force().z() >= MIN_DS_PRESSURE
+    b_ineq(32) = -MIN_DSP_FZ;
+    // w_r_rc.force().z() >= MIN_DSP_FZ
     A_ineq.block<1, 6>(33, 6) = -X_0_rc.dualMatrix().bottomRows<1>();
-    b_ineq(33) = -MIN_DS_PRESSURE;
+    b_ineq(33) = -MIN_DSP_FZ;
 
     qpSolver_.problem(NB_VAR, 0, NB_CONS);
     Eigen::MatrixXd A_eq(0, 0);

src/states/Standing.cpp

@@ -120,10 +120,10 @@ namespace lipm_walking
           [this]() { return std::round(releaseHeight_ * 100.) / 100.; },
           [this](double height) { releaseHeight_ = clamp(height, 0., 0.25); }),
         Label(
-          "Left foot pressure [N]",
+          "Left foot force [N]",
           [&ctl]() { return ctl.realRobot().forceSensor("LeftFootForceSensor").force().z(); }),
         Label(
-          "Right foot pressure [N]",
+          "Right foot force [N]",
           [&ctl]() { return ctl.realRobot().forceSensor("RightFootForceSensor").force().z(); }),
         Button(
           "Go to left foot",
@@ -290,16 +290,16 @@ namespace lipm_walking
 
   bool states::Standing::releaseFootContact(std::shared_ptr<mc_tasks::force::CoPTask> footTask)
   {
-    constexpr double MAX_FOOT_RELEASE_PRESSURE = 50.; // [N]
+    constexpr double MAX_FOOT_RELEASE_FORCE = 50.; // [N]
     auto & stabilizer = controller().stabilizer();
     if (footTask->admittance().couple().x() < 1e-10)
     {
       LOG_WARNING("Foot contact is already released");
       return false;
     }
-    else if (footTask->measuredWrench().force().z() > MAX_FOOT_RELEASE_PRESSURE)
+    else if (footTask->measuredWrench().force().z() > MAX_FOOT_RELEASE_FORCE)
     {
-      LOG_ERROR("Contact pressure is too high to release foot");
+      LOG_ERROR("Contact force is too high to release foot");
       return false;
     }
     sva::PTransformd X_0_f = footTask->surfacePose();