Merge pull request #78 from avidbots/refactor-dynamics-limits

Made dynamics limits a generic class, improved its function in TricycleDrive plugin

docs/included_plugins/diff_drive.rst

@@ -91,4 +91,18 @@ velocities and odometries are w.r.t. the robot origin
                              0, 0, 0, 0, 0, 0
                              0, 0, 0, 0, 0, 0
                              0, 0, 0, 0, 0, 0
-                             0, 0, 0, 0, 0, 0]
+                             0, 0, 0, 0, 0, 0]
+
+      # optional, defaults each parameter to 0.0 which means "no limit"
+      # sets dynamics constraints on angular velocity, acceleration (in rads/sec; rads/sec/sec)
+      angular_dynamics:
+        acceleration_limit: 0.0   # max acceleration (away from 0), in rads/s/s; 0.0 means "no limit"
+        deceleration_limit: 0.0   # max deceleration (towards 0), in rads/s/s; 0.0 means "no limit"; left blank, will default to acceleration_limit value
+        velocity_limit: 0.0       # max absolute velocity in rads/s; 0.0 means "no limit"
+
+      # optional, defaults each parameter to 0.0 which means "no limit"
+      # sets dynamics constraints on linear velocity, acceleration (in m/s; m/s/s)
+      linear_dynamics:
+        acceleration_limit: 0.0   # max acceleration (away from 0), in m/s/s; 0.0 means "no limit"
+        deceleration_limit: 0.0   # max deceleration (towards 0), in m/s/s; 0.0 means "no limit"; left blank, will default to acceleration_limit value
+        velocity_limit: 0.0       # max absolute velocity in m/s; 0.0 means "no limit"

docs/included_plugins/tricycle_drive.rst

@@ -122,11 +122,28 @@ has no effect on the actual motion of the robot.
       # the steering angle will not exceed this absolute limit
       max_steer_angle: 0.0
 
+      # optional, defaults each parameter to 0.0 which means "no limit"
+      # sets dynamics constraints on angular velocity, acceleration (in rads/sec; rads/sec/sec)
+      angular_dynamics:
+        acceleration_limit: 0.0   # max acceleration (away from 0), in rads/s/s; 0.0 means "no limit"
+        deceleration_limit: 0.0   # max deceleration (towards 0), in rads/s/s; 0.0 means "no limit"; left blank, will default to acceleration_limit value
+        velocity_limit: 0.0       # max absolute velocity in rads/s; 0.0 means "no limit"
+
+      # optional, defaults each parameter to 0.0 which means "no limit"
+      # sets dynamics constraints on linear velocity, acceleration (in m/s; m/s/s)
+      linear_dynamics:
+        acceleration_limit: 0.0   # max acceleration (away from 0), in m/s/s; 0.0 means "no limit"
+        deceleration_limit: 0.0   # max deceleration (towards 0), in m/s/s; 0.0 means "no limit"; left blank, will default to acceleration_limit value
+        velocity_limit: 0.0       # max absolute velocity in m/s; 0.0 means "no limit"
+        
+
+      # DEPRECATED; Use angular_dynamics.velocity_limit instead
       # optional, defaults to 0.0 which means no limit
       # sets the steering angular velocity limit (absolute, rad/s)
       # the steering angular velocity will not exceed this absolute limit
       max_angular_velocity: 0.0
 
+      # DEPRECATED; Use angular_dynamics.acceleration_limit instead
       # optional, defaults to 0.0 which means no limit
       # sets the steering angular acceleration limit (absolute, rad/s^2)
       # the steering angular acceleration will not exceed this absolute limit

flatland_plugins/CMakeLists.txt

@@ -72,6 +72,7 @@ add_library(flatland_plugins_lib
   src/laser.cpp
   src/tricycle_drive.cpp
   src/diff_drive.cpp
+  src/dynamics_limits.cpp
   src/model_tf_publisher.cpp
   src/update_timer.cpp
   src/bumper.cpp
@@ -140,6 +141,9 @@ if(CATKIN_ENABLE_TESTING)
                     test/laser_test.cpp)
   target_link_libraries(laser_test flatland_plugins_lib)
 
+  catkin_add_gtest(dynamics_limits_test test/dynamics_limits_test.cpp)
+  target_link_libraries(dynamics_limits_test flatland_plugins_lib)
+
   add_rostest_gtest(tricycle_drive_test test/tricycle_drive_test.test
                     test/tricycle_drive_test.cpp)
   target_link_libraries(tricycle_drive_test flatland_plugins_lib)

flatland_plugins/include/flatland_plugins/diff_drive.h

@@ -46,6 +46,7 @@
 
 #include <Box2D/Box2D.h>
 #include <flatland_plugins/update_timer.h>
+#include <flatland_plugins/dynamics_limits.h>
 #include <flatland_server/model_plugin.h>
 #include <flatland_server/timekeeper.h>
 #include <geometry_msgs/Twist.h>
@@ -74,6 +75,10 @@ class DiffDrive : public flatland_server::ModelPlugin {
   tf::TransformBroadcaster tf_broadcaster;  ///< For publish ROS TF
   bool enable_odom_pub_;   ///< YAML parameter to enable odom publishing
   bool enable_twist_pub_;  ///< YAML parameter to enable twist publishing
+  DynamicsLimits angular_dynamics_; ///< Angular dynamics constraints
+  DynamicsLimits linear_dynamics_;  ///< Linear dynamics constraints
+  double angular_velocity_ = 0.0;
+  double linear_velocity_ = 0.0;
 
   std::default_random_engine rng_;
   std::array<std::normal_distribution<double>, 6> noise_gen_;

flatland_plugins/include/flatland_plugins/dynamics_limits.h

@@ -0,0 +1,100 @@
+/*
+ *  ______                   __  __              __
+ * /\  _  \           __    /\ \/\ \            /\ \__
+ * \ \ \L\ \  __  __ /\_\   \_\ \ \ \____    ___\ \ ,_\   ____
+ *  \ \  __ \/\ \/\ \\/\ \  /'_` \ \ '__`\  / __`\ \ \/  /',__\
+ *   \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\
+ *    \ \_\ \_\ \___/  \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/
+ *     \/_/\/_/\/__/    \/_/\/__,_ /\/___/  \/___/  \/__/\/___/
+ * @copyright Copyright 2021 Avidbots Corp.
+ * @name	Dynamics_Limits.h
+ * @brief   A generic acceleration, deceleration and velocity bounding utility class
+ * @author  Joseph Duchesne
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, Avidbots Corp.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *      copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Avidbots Corp. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+#ifndef FLATLAND_PLUGINS_DYNAMICS_LIMITS_H
+#define FLATLAND_PLUGINS_DYNAMICS_LIMITS_H
+
+#include <flatland_server/yaml_reader.h>
+#include <yaml-cpp/yaml.h>
+
+namespace flatland_plugins {
+
+/**
+ * This class implements the model plugin class and provides laser data
+ * for the given configurations
+ */
+class DynamicsLimits {
+ public:
+  double acceleration_limit_ = 0.0;  // Maximum rate of change in velocity in the direction away from zero. Zero value disables this limit.
+  double deceleration_limit_ = 0.0; // Maximum rate of change in velocity in the direction towards zero. Zero value disables this limit.
+  double velocity_limit_ = 0.0;      // Maximum rate of change in position (in either direction). Zero value disables this limit.
+
+
+  /**
+   * @brief               blank constructor, no-op class
+   */
+  DynamicsLimits() {};
+
+  /**
+   * @name         Load configuration from a yaml object
+   * @brief        Constructor from yaml configuration file node
+   * @param[in]    YAML::Node& configuration node
+   */
+   void Configure(const YAML::Node &config);
+
+  /**
+   * @name          Saturate
+   * @brief         Apply dynamics limits
+   * @param[in]     config The plugin YAML node
+   */
+   static double Saturate(double in, double lower, double upper);
+
+  /**
+   * @name          Apply
+   * @brief         Apply dynamics limits based on class configured limits
+   * @param[in]     velocity The current velocity (units/second)
+   * @param[in]     target_velocity The target velocity requested (units/second)
+   * @param[in]     timestep  The timestep (seconds) (used to calculate acceleration from delta velocity)
+   * @return        The new velocity result after target velocity has been subjected to limits
+   */
+   double Limit(double velocity, double target_velocity, double timestep);
+
+};
+
+};
+
+#endif  // FLATLAND_PLUGINS_DYNAMICS_LIMITS_H

flatland_plugins/include/flatland_plugins/tricycle_drive.h

@@ -46,6 +46,7 @@
 
 #include <Box2D/Box2D.h>
 #include <flatland_plugins/update_timer.h>
+#include <flatland_plugins/dynamics_limits.h>
 #include <flatland_server/model_plugin.h>
 #include <flatland_server/timekeeper.h>
 #include <geometry_msgs/Twist.h>
@@ -73,9 +74,12 @@ class TricycleDrive : public flatland_server::ModelPlugin {
   double max_steer_angle_;         ///< max abs. steering allowed [rad]
   double max_steer_velocity_;      ///< max abs. steering velocity [rad/s]
   double max_steer_acceleration_;  ///< max abs. steering acceleration [rad/s^2]
+  DynamicsLimits angular_dynamics_; ///< Angular dynamics constraints
+  DynamicsLimits linear_dynamics_;  ///< Linear dynamics constraints
   double delta_command_;  ///< The current target (commanded) wheel angle
   double theta_f_;        ///< The current angular offset of the front wheel
   double d_delta_;        ///< The current angular speed of the front wheel
+  double v_f_ = 0.0;      ///< The current velocity at the front wheel
 
   geometry_msgs::Twist twist_msg_;
   nav_msgs::Odometry odom_msg_;

flatland_plugins/src/diff_drive.cpp

@@ -83,6 +83,12 @@ void DiffDrive::OnInitialize(const YAML::Node& config) {
       reader.Get<double>("pub_rate", std::numeric_limits<double>::infinity());
   update_timer_.SetRate(pub_rate);
 
+  // Angular dynamics constraints
+  angular_dynamics_.Configure(reader.SubnodeOpt("angular_dynamics", YamlReader::MAP).Node());
+
+  // Linear dynamics constraints
+  linear_dynamics_.Configure(reader.SubnodeOpt("linear_dynamics", YamlReader::MAP).Node());
+
   // by default the covariance diagonal is the variance of actual noise
   // generated, non-diagonal elements are zero assuming the noises are
   // independent, we also don't care about linear z, angular x, and angular y
@@ -168,6 +174,34 @@ void DiffDrive::BeforePhysicsStep(const Timekeeper& timekeeper) {
   b2Vec2 position = b2body->GetPosition();
   float angle = b2body->GetAngle();
 
+  // Apply dynamics limits
+  double dt = timekeeper.GetStepSize();
+  linear_velocity_ = linear_dynamics_.Limit(linear_velocity_, twist_msg_.linear.x, dt);
+  angular_velocity_ = angular_dynamics_.Limit(angular_velocity_, twist_msg_.angular.z, dt);
+
+  // we apply the twist velocities, this must be done every physics step to make
+  // sure Box2D solver applies the correct velocity through out. The velocity
+  // given in the twist message should be in the local frame
+  b2Vec2 linear_vel_local(linear_velocity_, 0);
+  b2Vec2 linear_vel = b2body->GetWorldVector(linear_vel_local);
+  float angular_vel = angular_velocity_;  // angular is independent of frames
+
+  // we want the velocity vector in the world frame at the center of mass
+
+  // V_cm = V_o + W x r_cm/o
+  // velocity at center of mass equals to the velocity at the body origin plus,
+  // angular velocity cross product the displacement from the body origin to the
+  // center of mass
+
+  // r is the vector from body origin to the CM in world frame
+  b2Vec2 r = b2body->GetWorldCenter() - position;
+  b2Vec2 linear_vel_cm = linear_vel + angular_vel * b2Vec2(-r.y, r.x);
+
+  b2body->SetLinearVelocity(linear_vel_cm);
+  b2body->SetAngularVelocity(angular_vel);
+
+  // Update odom+ground truth messages if needed
+
   if (publish) {
     // get the state of the body and publish the data
     b2Vec2 linear_vel_local =
@@ -232,26 +266,6 @@ void DiffDrive::BeforePhysicsStep(const Timekeeper& timekeeper) {
     tf_broadcaster.sendTransform(odom_tf);
   }
 
-  // we apply the twist velocities, this must be done every physics step to make
-  // sure Box2D solver applies the correct velocity through out. The velocity
-  // given in the twist message should be in the local frame
-  b2Vec2 linear_vel_local(twist_msg_.linear.x, 0);
-  b2Vec2 linear_vel = b2body->GetWorldVector(linear_vel_local);
-  float angular_vel = twist_msg_.angular.z;  // angular is independent of frames
-
-  // we want the velocity vector in the world frame at the center of mass
-
-  // V_cm = V_o + W x r_cm/o
-  // velocity at center of mass equals to the velocity at the body origin plus,
-  // angular velocity cross product the displacement from the body origin to the
-  // center of mass
-
-  // r is the vector from body origin to the CM in world frame
-  b2Vec2 r = b2body->GetWorldCenter() - position;
-  b2Vec2 linear_vel_cm = linear_vel + angular_vel * b2Vec2(-r.y, r.x);
-
-  b2body->SetLinearVelocity(linear_vel_cm);
-  b2body->SetAngularVelocity(angular_vel);
 }
 }