Add vibration boundaries to the yaml so that we may easily change this value when tuning.

Author: RPKrijnen

march_joint_inertia_controller/config/joint_inertia_controller.yaml

@@ -37,18 +37,20 @@ march:
           std_samples: 100 # number of samples used to calculate the standard deviation
           lambda: 0.96 # Usually chosen between 0.9 and 1.0, but not bigger than one or less than zero
           alpha_filter_size: 12 # Determines the filter size in the alpha calculation, recommended between 8 and 32
+          vibration_boundaries: {1.4, 1.9}
           control_points: # All arrays must be of same size. Each inertia corresponds to a gain
-            inertia: [0.0, 1.0]
-            p: [150, 300]
-            d: [0, 5]
+            inertia: {0.0, 1.0}
+            p: {150, 300}
+            d: {0, 5}
         linear:
           std_samples: 100 # number of samples used to calculate the standard deviation
           lambda: 0.96
           alpha_filter_size: 12
+          vibration_boundaries: {1.4, 1.9}
           control_points:
-            inertia: [0.0, 1.0]
-            p: [150, 300]
-            d: [0, 5]
+            inertia: {0.0, 1.0}
+            p: {150, 300}
+            d: {0, 5}
       constraints:
         left_ankle:
           trajectory: 0.305

march_joint_inertia_controller/include/march_joint_inertia_controller/inertia_estimator.h

@@ -47,6 +47,7 @@ class InertiaEstimator
 
   void setLambda(double lambda);
   void setAccSize(size_t acc_size);
+  void setVibrationBoundaries(std::vector<double> boundaries);
 
   /**
    * \brief Applies the Butterworth filter over the last two samples and returns the resulting filtered value
@@ -92,8 +93,8 @@ class InertiaEstimator
   std::list<double> standard_deviation;
 
 private:
-  double min_alpha_ = 1.4;  // You might want to be able to adjust this value from a yaml/launch file
-  double max_alpha_ = 1.9;  // You might want to be able to adjust this value from a yaml/launch file
+  double min_alpha_;  // You might want to be able to adjust this value from a yaml/launch file
+  double max_alpha_;  // You might want to be able to adjust this value from a yaml/launch file
 
   // This is a sixth order butterworth filter with a cutoff frequency at 15Hz in Second Order Sections form
   std::vector<std::vector<double>> sos_{

march_joint_inertia_controller/include/march_joint_inertia_controller/joint_trajectory_inertia_controller.h

@@ -65,21 +65,26 @@ class HardwareInterfaceAdapter<hardware_interface::EffortJointInterface, joint_t
     // Obtain inertia estimator parameters from server
     double lambda[2];
     int alpha_filter_size[2];
+    std::vector<std::vector<double>> vibration_boundaries{ { 0.0, 0.0 }, { 0.0, 0.0 } };
+    std::vector<double> default_vibration = { 0.0, 1.0 };
     ros::NodeHandle rotary_estimator_nh(nh, std::string("inertia_estimator/rotary"));
     rotary_estimator_nh.param("std_samples", samples_, 100);
     rotary_estimator_nh.param("lambda", lambda[0], 1.0);
     rotary_estimator_nh.param("alpha_filter_size", alpha_filter_size[0], 12);
+    rotary_estimator_nh.param("vibration_boundaries", vibration_boundaries[0], default_vibration);
 
     ros::NodeHandle linear_estimator_nh(nh, std::string("inertia_estimator/linear"));
     linear_estimator_nh.param("lambda", lambda[1], 1.0);
     linear_estimator_nh.param("alpha_filter_size", alpha_filter_size[1], 12);
+    linear_estimator_nh.param("vibration_boundaries", vibration_boundaries[1], default_vibration);
 
     // Initialize the estimator parameters
     inertia_estimators_.resize(num_joints_);
     for (unsigned int j = 0; j < num_joints_; ++j)
     {
       inertia_estimators_[j].setLambda(lambda[(int)floor(j / 2) % 2]);  // Produce sequence 00110011
       inertia_estimators_[j].setAccSize(alpha_filter_size[(int)floor(j / 2) % 2]);
+      inertia_estimators_[j].setVibrationBoundaries(vibration_boundaries[(int)floor(j / 2) % 2]);
       //      pub_[j] = nh.advertise<std_msgs::Float64>("/inertia_publisher/" + joint_handles[j].getName(), 100);
       this->pub_[j] = std::make_unique<realtime_tools::RealtimePublisher<std_msgs::Float64>>(
           nh, "/inertia_publisher/" + joint_handles[j].getName(), 4);

march_joint_inertia_controller/src/inertia_estimator.cpp

@@ -73,6 +73,12 @@ void InertiaEstimator::setAccSize(size_t acc_size)
   filtered_acceleration_array_.resize(acc_size_, 0.0);
 }
 
+void InertiaEstimator::setVibrationBoundaries(std::vector<double> boundaries)
+{
+  min_alpha_ = boundaries[0];
+  max_alpha_ = boundaries[1];
+}
+
 double InertiaEstimator::getJointInertia()
 {
   return joint_inertia_;
@@ -135,8 +141,8 @@ void InertiaEstimator::inertiaEstimate()
   K_a_ = alphaCalculation();
   auto ita = filtered_acceleration_array_.begin();
   auto itt = filtered_joint_torque_.begin();
-  const double torque_e = *itt - *(itt++);
-  const double acc_e = *ita - *(ita++);
+  const double torque_e = *itt - *(++itt);
+  const double acc_e = *ita - *(++ita);
   joint_inertia_ = (torque_e - (acc_e * joint_inertia_)) * K_i_ * K_a_ + joint_inertia_;
 }
 
@@ -152,15 +158,15 @@ double InertiaEstimator::alphaCalculation()
 double InertiaEstimator::gainCalculation()
 {
   auto it = filtered_acceleration_array_.begin();
-  auto error = *it - *(it++);
+  auto error = *it - *(++it);
   return (corr_coeff_ * error) / (lambda_ + corr_coeff_ * pow(error, 2));
 }
 
 // Calculate the correlation coefficient of the acceleration buffer
 void InertiaEstimator::correlationCalculation()
 {
   auto it = filtered_acceleration_array_.begin();
-  corr_coeff_ = corr_coeff_ / (lambda_ + corr_coeff_ * pow(*it - *(it++), 2));
+  corr_coeff_ = corr_coeff_ / (lambda_ + corr_coeff_ * pow(*it - *(++it), 2));
   const double large_number = 10e8;
   if (corr_coeff_ > large_number)
   {
@@ -185,7 +191,7 @@ double InertiaEstimator::vibrationCalculation()
 double InertiaEstimator::discreteSpeedDerivative(const ros::Duration& period)
 {
   auto it = velocity_array_.begin();
-  return (*it - *(it++)) / period.toSec();
+  return (*it - *(++it)) / period.toSec();
 }
 
 void InertiaEstimator::initP(unsigned int samples)