Merge pull request #278 from project-march/feature/PM-458-implement-inertia-calculations-in-a-controller

Feature/PM-458: implement inertia calculations in a controller

Author: RutgerVanBeek

march_joint_inertia_controller/CMakeLists.txt

@@ -4,23 +4,42 @@ project(march_joint_inertia_controller)
 add_compile_options(-std=c++14 -Wall -Wextra -Werror)
 
 find_package(catkin REQUIRED COMPONENTS
+    actionlib
+    actionlib_msgs
+    control_msgs
     controller_interface
+    controller_manager
+    joint_state_controller
+    joint_trajectory_controller
+    march_hardware
+    march_shared_resources
     pluginlib
+    realtime_tools
     roscpp
+    std_msgs
+    urdf
 )
 
 catkin_package(
     INCLUDE_DIRS include
     LIBRARIES ${PROJECT_NAME}
     CATKIN_DEPENDS
+    actionlib_msgs
+    control_msgs
     controller_interface
     pluginlib
     roscpp
+    std_msgs
 )
 
 include_directories(include SYSTEM ${catkin_INCLUDE_DIRS})
 
-add_library(${PROJECT_NAME} src/joint_inertia_controller.cpp)
+add_library(${PROJECT_NAME}
+    include/${PROJECT_NAME}/inertia_estimator.h
+    include/${PROJECT_NAME}/joint_trajectory_inertia_controller.h
+    src/inertia_estimator.cpp
+    src/joint_inertia_controller.cpp
+    src/joint_trajectory_inertia_controller.cpp)
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 
 install(DIRECTORY include/${PROJECT_NAME}/

march_joint_inertia_controller/config/joint_inertia_controller.yaml

@@ -1,12 +1,94 @@
+# This is a template for the controllers.yaml used in the HWI and in the simulation, note the inertia_estimator.
+
 march:
-  joint_inertia_controller:
-    type: joint_inertia_controller/InertiaController
-    joints:
-      - left_ankle
-      - left_hip_aa
-      - left_hip_fe
-      - left_knee
-      - right_ankle
-      - right_hip_aa
-      - right_hip_fe
-      - right_knee
+  controller:
+    pdb_state:
+      type: march_pdb_state_controller/MarchPdbStateController
+      publish_rate: 50
+    joint_state:
+      type: joint_state_controller/JointStateController
+      publish_rate: 250
+    temperature_sensor:
+      type: march_temperature_sensor_controller/MarchTemperatureSensorController
+      publish_rate: 1
+    joint_inertia_controller:
+      type: joint_inertia_controller/InertiaController
+      allow_partial_joints_goal: true
+      joints:
+        - left_ankle
+        - left_hip_aa
+        - left_hip_fe
+        - left_knee
+        - right_ankle
+        - right_hip_aa
+        - right_hip_fe
+        - right_knee
+      gains: # Required because we're controlling an effort interface
+        left_ankle: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true}
+        left_hip_aa: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true}
+        left_hip_fe: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true}
+        left_knee: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true}
+        right_ankle: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true}
+        right_hip_aa: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true}
+        right_hip_fe: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true}
+        right_knee: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true}
+      inertia_estimator:
+        rotary:
+          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:
+            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
+      constraints:
+        left_ankle:
+          trajectory: 0.305
+          goal: 0.305
+        left_hip_aa:
+          trajectory: 0.305
+          goal: 0.305
+        left_hip_fe:
+          trajectory: 0.305
+          goal: 0.305
+        left_knee:
+          trajectory: 0.305
+          goal: 0.305
+        right_ankle:
+          trajectory: 0.305
+          goal: 0.305
+        right_hip_aa:
+          trajectory: 0.305
+          goal: 0.305
+        right_hip_fe:
+          trajectory: 0.305
+          goal: 0.305
+        right_knee:
+          trajectory: 0.305
+          goal: 0.305

march_joint_inertia_controller/controller_plugins.xml

@@ -1,5 +1,8 @@
-<library path="lib/libjoint_inertia_controller_lib">
-  <class name="joint_inertia_controller/InertiaController"
-         type="joint_inertia_controller::InertiaController"
-         base_class_type="controller_interface::ControllerBase" />
+<library path="lib/libmarch_joint_inertia_controller">
+    <class name="joint_inertia_controller/InertiaController"
+           type="joint_inertia_controller::InertiaController"
+           base_class_type="controller_interface::ControllerBase"/>
+    <class name="inertia_trajectory_controller/JointTrajectoryController"
+           type="inertia_trajectory_controller::JointTrajectoryController"
+           base_class_type="controller_interface::ControllerBase"/>
 </library>

march_joint_inertia_controller/include/march_joint_inertia_controller/inertia_estimator.h

@@ -0,0 +1,140 @@
+// Copyright 2020 Project March.
+
+#ifndef MARCH_WS_INERTIA_ESTIMATOR_H
+#define MARCH_WS_INERTIA_ESTIMATOR_H
+
+#include <hardware_interface/joint_command_interface.h>
+#include <list>
+#include <ros/ros.h>
+#include <trajectory_interface/quintic_spline_segment.h>
+#include <urdf/model.h>
+
+/**
+ * \brief determines the absolute value of vector a and stores it into b so that a remains unchanged
+ *
+ * @param[in] a input vector with values that need to be converted to absolutes
+ * @param[out] b output vector with copies of the absolute values of a.
+ */
+std::list<double> absolute(const std::list<double>& a);
+
+/**
+ * \brief determiness the mean value of the given vector
+ *
+ * @param[in] a input vector from which the mean gets calculated
+ * @returns the mean value of a
+ */
+double mean(const std::list<double>& a);
+
+/**
+ * \brief Class that bundles functionality to estimate inertia on a Revolute Joint.
+ *
+ * @param[in] joint the jointhandle used for obtaining current speed and effort.
+ * @param[in] lambda sets the lambda used in the RLS filter defaulted to 0.96
+ * @param[in] acc_size the size of the vector wherein the calculated acceleration is stored. This vector length is also
+ *  used in determining the length of the velocity vector and the filtered acceleration vector. It is of specific
+ *  size to calculate the alpha value. Defaulted to 12.
+ */
+class InertiaEstimator
+{
+public:
+  /**
+   * \brief Default constructor
+   */
+  InertiaEstimator(double lambda = 0.96, size_t acc_size = 12);
+
+  double getAcceleration();
+  double getJointInertia();
+
+  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
+   */
+  void applyButter();
+
+  /**
+   * \brief Estimate the inertia using the acceleration and torque
+   */
+  void inertiaEstimate();
+  /**
+   * \brief Calculate a discrete derivative of the speed measurements
+   */
+  double discreteSpeedDerivative(const ros::Duration& period);
+  /**
+   * \brief Calculate the alpha coefficient for the inertia estimate
+   */
+  double alphaCalculation();
+  /**
+   * \brief Calculate the inertia gain for the inertia estimate
+   */
+  double gainCalculation();
+  /**
+   * \brief Calculate the correlation coefficient of the acceleration buffer
+   */
+  void correlationCalculation();
+  /**
+   * \brief Calculate the vibration based on the acceleration
+   */
+  double vibrationCalculation();
+
+  /**
+   * \brief Fill the rolling buffers with corresponding values.
+   */
+  void fillBuffers(double velocity, double effort, const ros::Duration& period);
+
+  /**
+   * \brief Calculate the initial correlation coefficient
+   */
+  void initP(unsigned int samples);
+
+  // Vector to be filled with samples of acceleration to determine the standard deviation from
+  std::list<double> standard_deviation;
+
+private:
+  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_{
+    { 9.16782507e-09, 1.83356501e-08, 9.16782507e-09, 1.00000000e+00, -1.82520938e+00, 8.33345838e-01 },
+    { 1.00000000e+00, 2.00000000e+00, 1.00000000e+00, 1.00000000e+00, -1.86689228e+00, 8.75214548e-01 },
+    { 1.00000000e+00, 2.00000000e+00, 1.00000000e+00, 1.00000000e+00, -1.94377925e+00, 9.52444269e-01 }
+  };
+
+  // z1 and z2 vectors are containers for previous time-step values in the butterworth filter
+  // Might be neater to design a separate class for the filter application for arbitrary filter sizes, datasets and
+  // number of signals such as: https://github.com/scipy/scipy/blob/v1.5.2/scipy/signal/_sosfilt.pyx
+  std::vector<double> z1a;
+  std::vector<double> z2a;
+
+  std::vector<double> z1t;
+  std::vector<double> z2t;
+
+  // Default length 12 for the alpha calculation
+  size_t acc_size_ = 12;
+  std::list<double> acceleration_array_;
+  // Equal to two, because two values are needed to calculate the derivative
+  size_t vel_size_ = acc_size_;
+  std::list<double> velocity_array_;
+  std::list<double> filtered_acceleration_array_;
+  // Of length 3 for the butterworth filter
+  size_t torque_size_ = 3;
+  std::list<double> joint_torque_;
+  // Of length 2 for the error calculation
+  size_t fil_tor_size_ = 2;
+  std::list<double> filtered_joint_torque_;
+
+  // Correlation coefficient used to calculate the inertia gain
+  double corr_coeff_;
+  double vibration_;
+  double K_a_;
+  double K_i_;
+  double mean_of_absolute_;
+  double absolute_of_mean_;
+  double joint_inertia_;
+  double lambda_;
+};
+
+#endif  // MARCH_WS_INERTIA_ESTIMATOR_H

march_joint_inertia_controller/include/march_joint_inertia_controller/joint_inertia_controller.h

@@ -3,23 +3,76 @@
 #ifndef MARCH_HARDWARE_JOINT_INERTIA_CONTROLLER_H
 #define MARCH_HARDWARE_JOINT_INERTIA_CONTROLLER_H
 
+#include <control_toolbox/pid.h>
 #include <controller_interface/controller.h>
-#include <hardware_interface/joint_command_interface.h>
-#include <pluginlib/class_list_macros.h>
+#include "march_joint_inertia_controller/inertia_estimator.h"
+#include <std_msgs/Float64.h>
 
 namespace joint_inertia_controller
 {
-class InertiaController : public controller_interface::Controller<hardware_interface::PositionJointInterface>
+class InertiaController : public controller_interface::Controller<hardware_interface::EffortJointInterface>
 {
 public:
-  bool init(hardware_interface::PositionJointInterface* hw, ros::NodeHandle& n);
-  void update(const ros::Time& time, const ros::Duration& period);
-  void starting(const ros::Time& time);
-  void stopping(const ros::Time& time);
+  struct Commands
+  {
+    double position;    // Last commanded position
+    double velocity;    // Last commanded velocity
+    bool has_velocity;  // false if no velocity command has been specified
+  };
+
+  InertiaController() = default;
+  ~InertiaController() override;
+
+  bool init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle& n) override;
+  void update(const ros::Time& time, const ros::Duration& period) override;
+  void starting(const ros::Time& time) override;
+  void stopping(const ros::Time& time) override;
+  void setCommand(double pos_command);
+  void setCommand(double pos_command, double vel_command);
+  void commandCB(const std_msgs::Float64ConstPtr& msg);
+
+  /**
+   * \brief Get the PID parameters
+   */
+  void getGains(double& p, double& i, double& d, double& i_max, double& i_min);
+  /**
+   * \brief Get the PID parameters
+   */
+  void getGains(double& p, double& i, double& d, double& i_max, double& i_min, bool& antiwindup);
+
+  /**
+   * \brief Print debug info to console
+   */
+  void printDebug();
+
+  /**
+   * \brief Set the PID parameters
+   */
+  void setGains(const double& p, const double& i, const double& d, const double& i_max, const double& i_min,
+                const bool& antiwindup = false);
+
+  /**
+   * \brief Return the current position of the corresponding joint
+   */
+  double getPosition();
+
+  /**
+   * \brief Return the name of the corresponding joint is string format
+   */
+  std::string getJointName();
 
 private:
+  control_toolbox::Pid pid_controller_; /**< Internal PID controller. */
+
+  ros::Subscriber sub_command_;
   hardware_interface::JointHandle joint_;
-  double init_pos_;
+
+  urdf::JointConstSharedPtr joint_urdf_;
+  std::string joint_name_;
+  realtime_tools::RealtimeBuffer<Commands> command_;
+  Commands command_struct_;  // pre-allocated memory that is re-used to set the realtime buffer
+
+  InertiaEstimator inertia_estimator_;
 };
 }  // namespace joint_inertia_controller