Added additional code to support a minimum steering difference optimi…

…sation that prevents continuous steering joints from moving more than a quatre turn. Modified the test yaml and added a description of the new param. Added new param enable_min_steering_difference to enable or disable this new functionality, default off.
ethz-asl · Aug 25, 2020 · 341a73c · 341a73c
1 parent 93537a7
commit 341a73c
Show file tree

Hide file tree

Showing 3 changed files with 59 additions and 5 deletions.
diff --git a/swerve_controller/include/swerve_controller/swerve_controller.h b/swerve_controller/include/swerve_controller/swerve_controller.h
@@ -40,6 +40,7 @@
 #include <cmath>
 #include <string>
 #include <boost/optional.hpp>
+#include <algorithm>
 
 #include <controller_interface/multi_interface_controller.h>
 #include <hardware_interface/joint_command_interface.h>
@@ -148,19 +149,30 @@ namespace swerve_controller
         /// Whether to publish odometry to tf or not:
         bool enable_odom_tf_;
 
+        // Whether to enable minimum steering difference by checking whether the calculated angle or its
+        // polar opposite angle are closer to the previously set one
+        bool enable_min_steering_difference_;
+
         /// Speed limiters:
         CommandTwist last1_cmd_;
         CommandTwist last0_cmd_;
         SpeedLimiter limiter_lin_;
         SpeedLimiter limiter_ang_;
 
+        double lf_steering_last;
+        double rf_steering_last;
+        double lh_steering_last;
+        double rh_steering_last;
+
     private:
         void updateOdometry(const ros::Time &time);
 
         void updateCommand(const ros::Time &time, const ros::Duration &period);
 
         void brake();
 
+        void minSteeringDifference(double &steering, double &previous_steering, double &speed);
+
         bool clipSteeringAngle(double &steering, double &speed);
 
         void cmdVelCallback(const geometry_msgs::Twist &command);

diff --git a/swerve_controller/src/swerve_controller.cpp b/swerve_controller/src/swerve_controller.cpp
@@ -50,8 +50,9 @@ namespace swerve_controller
           max_steering_angle_(M_PI),
           cmd_vel_timeout_(0.5),
           base_frame_id_("base_link"),
-          enable_odom_tf_(true)
-    {
+          enable_odom_tf_(true),
+          enable_min_steering_difference_(false)
+	{
     }
 
     bool SwerveController::init(hardware_interface::RobotHW *robot_hw,
@@ -110,6 +111,10 @@ namespace swerve_controller
         ROS_INFO_STREAM_NAMED(name_, "Publishing to tf is "
                                          << (enable_odom_tf_ ? "enabled" : "disabled"));
 
+        controller_nh.param("enable_min_steering_difference", enable_min_steering_difference_, enable_min_steering_difference_);
+        ROS_INFO_STREAM_NAMED(name_, "Minimum steering difference is "
+                                        << (enable_min_steering_difference_ ? "enabled" : "disabled"));
+
         // Get velocity and acceleration limits from the parameter server
         controller_nh.param("linear/x/has_velocity_limits",
                             limiter_lin_.has_velocity_limits,
@@ -367,10 +372,19 @@ namespace swerve_controller
             !clipSteeringAngle(lh_steering, lh_speed) ||
             !clipSteeringAngle(rh_steering, rh_speed))
         {
+            ROS_WARN("Braking because clipped steering angle was impossible to reach");
             brake();
             return;
         }
 
+        // Guarantee minimum angle difference to next steer angle by using previous steer angle
+        if (enable_min_steering_difference_){
+            minSteeringDifference(lf_steering, lf_steering_last, lf_speed);
+            minSteeringDifference(rf_steering, rf_steering_last, rf_speed);
+            minSteeringDifference(lh_steering, lh_steering_last, lh_speed);
+            minSteeringDifference(rh_steering, rh_steering_last, rh_speed);
+        }
+
         // Set wheels velocities
         if (lf_wheel_joint_ && rf_wheel_joint_ && lh_wheel_joint_ && rh_wheel_joint_)
         {
@@ -388,6 +402,11 @@ namespace swerve_controller
             lh_steering_joint_->setCommand(lh_steering);
             rh_steering_joint_->setCommand(rh_steering);
         }
+
+        lf_steering_last = lf_steering;
+        rf_steering_last = rf_steering;
+        lh_steering_last = lh_steering;
+        rh_steering_last = rh_steering;
     }
 
     void SwerveController::brake()
@@ -435,6 +454,26 @@ namespace swerve_controller
         return true;
     }
 
+    void SwerveController::minSteeringDifference(double &steering, double &previous_steering, double &speed)
+    {
+        double polarSteering = 0;
+        if (steering > 0)
+        {
+            polarSteering = steering - M_PI;
+        }
+        else
+        {
+            polarSteering = steering + M_PI;
+        }
+
+        // Take whichever is closer, the computed angle or its polar opposite angle
+        if (std::abs(previous_steering - steering) > std::abs(previous_steering - polarSteering))
+        {
+            steering = polarSteering;
+            speed = -speed;
+        }
+    }
+
     void SwerveController::cmdVelCallback(const geometry_msgs::Twist &command)
     {
         if (isRunning())

diff --git a/swerve_controller/test/config/swervebot_control.yaml b/swerve_controller/test/config/swervebot_control.yaml
@@ -19,8 +19,11 @@ swervebot:
         rh_steering: "leg_rh_joint"
 
         # Range of motion of steering motors
-        min_steering_angle: -1.58
-        max_steering_angle: 1.58
+        # min_steering_angle: -1.58
+        # max_steering_angle: 1.58
+
+        # Minimise steering difference between previous and next steering angle (continuous steering joints only)
+        # enable_min_steering_difference: true
 
         # Other
         publish_rate: 50
@@ -46,4 +49,4 @@ swervebot:
             wheel_lf_joint: {p: 50.0, i: 2.0, d: 0.001}
             wheel_rf_joint: {p: 50.0, i: 2.0, d: 0.001}
             wheel_lh_joint: {p: 50.0, i: 2.0, d: 0.001}
-            wheel_rh_joint: {p: 50.0, i: 2.0, d: 0.001}
+            wheel_rh_joint: {p: 50.0, i: 2.0, d: 0.001}