Merge pull request #34 from clearpathrobotics/use_control_period_on_t…

…wist

Use control period on twist

diff_drive_controller/CMakeLists.txt

@@ -22,8 +22,6 @@ find_package(Boost REQUIRED
 
 find_package(Eigen REQUIRED)
 
-find_package(sophus REQUIRED)
-
 find_package(Ceres REQUIRED)
 
 add_message_files(

diff_drive_controller/include/diff_drive_controller/covariance.h

@@ -41,6 +41,8 @@
 
 #include <Eigen/Dense>
 
+#include <boost/array.hpp>
+
 #include <limits>
 
 namespace diff_drive_controller
@@ -220,21 +222,22 @@ static void msgToCovariance(
 }
 
 /**
- * \brief Check is a matrix M is symmetric, i.e. M' M = Id or low(M) = up(M)
+ * \brief Check is a matrix M is symmetric, i.e. M' M == Id or low(M) == up(M)
  * where low and up are the lower and upper triangular matrices of M
  * \param [in] M Square matrix
  * \return True if the matrix is symmetric
  */
 template <typename T, int N>
-bool isSymmetric(const Eigen::Matrix<T, N, N>& M)
+bool isSymmetric(const Eigen::Matrix<T, N, N>& M,
+    const double eps = std::numeric_limits<T>::epsilon())
 {
-  // Note that triangularView doesn't help to much to simplify the check,
-  // so we iterate on the lower and upper triangular matrixes directly:
+  // Note that triangularView doesn't help too much to simplify the check,
+  // so we iterate on the lower and upper triangular matrices directly:
   for (size_t i = 0; i < N - 1; ++i)
   {
     for (size_t j = i + 1; j < N; ++j)
     {
-      if (std::abs(M(i, j) - M(j, i)) > std::numeric_limits<T>::epsilon())
+      if (std::abs(M(i, j) - M(j, i)) > eps)
       {
         return false;
       }
@@ -337,7 +340,9 @@ T conditionNumber(const Eigen::Matrix<T, N, N>& M)
 
   const T s_min = svd.singularValues().minCoeff();
 
-  return s_min == 0 ?
+  // Note that the singular values computed with Jacobi SVD are all positive, so
+  // we don't need to use abs().
+  return s_min == T(0) ?
          std::numeric_limits<T>::infinity() :
          svd.singularValues().maxCoeff() / s_min;
 }

diff_drive_controller/include/diff_drive_controller/integrate_function.h

@@ -56,7 +56,6 @@ namespace diff_drive_controller
 
       /**
        * \brief Constructor
-       * \param [in] functor Integrate functor
        */
       IntegrateFunction()
       {}

diff_drive_controller/include/diff_drive_controller/odometry.h

@@ -44,7 +44,6 @@
 
 #include <ros/time.h>
 #include <Eigen/Core>
-#include <sophus/se2.hpp>
 #include <boost/accumulators/accumulators.hpp>
 #include <boost/accumulators/statistics/stats.hpp>
 #include <boost/accumulators/statistics/rolling_mean.hpp>
@@ -65,9 +64,6 @@ namespace diff_drive_controller
   class Odometry
   {
   public:
-    /// SO(2) and SE(2) Lie Groups:
-    typedef Sophus::SE2d SE2;
-
     /// Covariance matrices:
     typedef Eigen::Matrix3d Covariance;
 
@@ -104,24 +100,24 @@ namespace diff_drive_controller
      * \param[in] right_position Right wheel position [rad]
      * \param[in] left_velocity  Left  wheel velocity [rad/s]
      * \param[in] right_velocity Right wheel velocity [rad/s]
-     * \param[in] time           Current time
+     * \param[in] dt             Time step (control period) [s]
      * \return true if the odometry is actually updated
      */
     bool updateCloseLoop(
         const double left_position, const double right_position,
         const double left_velocity, const double right_velocity,
-        const ros::Time &time);
+        const double dt);
 
     /**
      * \brief Updates the odometry class with latest velocity command, i.e. in
      * open loop
      * \param[in] linear  Linear  velocity [m/s]
      * \param[in] angular Angular velocity [rad/s]
-     * \param[in] time    Current time
+     * \param[in] dt      Time step (control period) [s]
      * \return true if the odometry is actually updated
      */
     bool updateOpenLoop(const double linear, const double angular,
-        const ros::Time &time);
+        const double dt);
 
     /**
      * \brief Update the odometry twist with the (internal) incremental pose,
@@ -273,11 +269,11 @@ namespace diff_drive_controller
      * \param[in] dp_r  Right wheel position increment [rad]
      * \param[in] v_l   Left  wheel velocity [rad/s]
      * \param[in] v_r   Right wheel velocity [rad/s]
-     * \param[in] time  Current time
+     * \param[in] dt    Time step (control period) [s]
      * \return true if the odometry is actually updated
      */
     bool update(const double dp_l, const double dp_r,
-        const double v_l, const double v_r, const ros::Time& time);
+        const double v_l, const double v_r, const double dt);
 
     /**
      * \brief Updates the (internal) incremental odometry with latest left and
@@ -292,9 +288,6 @@ namespace diff_drive_controller
      */
     void resetAccumulators();
 
-    /// Current timestamp:
-    ros::Time timestamp_;
-
     /// Timestamp for last twist computed, ie. since when the (internal)
     /// incremental pose has been computed:
     ros::Time timestamp_twist_;

diff_drive_controller/include/diff_drive_controller/rigid_body_motion.h

@@ -78,6 +78,8 @@ static void integrate_motion(double& x, double &y, double &yaw,
  * \param[in] v_y   Velocity/Twist y   component
  * \param[in] v_yaw Velocity/Twist yaw component
  * \param[in] dt Time step
+ * \param[out] J_pose Jacobian wrt the pose
+ * \param[out] J_twist Jacobian wrt the velocity/twist
  */
 static void integrate_motion(double& x, double &y, double &yaw,
     const double v_x, const double v_y, const double v_yaw,

diff_drive_controller/package.xml

@@ -26,7 +26,6 @@
   <build_depend>tf</build_depend>
   <build_depend>urdf</build_depend>
   <build_depend>eigen</build_depend>
-  <build_depend>sophus</build_depend>
   <build_depend>libceres-dev</build_depend>
   <build_depend>roslint</build_depend>
   <build_depend>boost</build_depend>

diff_drive_controller/src/diff_drive_controller.cpp

@@ -629,11 +629,11 @@ namespace diff_drive_controller
     // Update odometry:
     if (open_loop_)
     {
-      odometry_.updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, time);
+      odometry_.updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, control_period);
     }
     else
     {
-      odometry_.updateCloseLoop(left_position, right_position, left_velocity, right_velocity, time);
+      odometry_.updateCloseLoop(left_position, right_position, left_velocity, right_velocity, control_period);
     }
 
     // Publish odometry message:

diff_drive_controller/src/odometry.cpp

@@ -60,8 +60,7 @@ namespace diff_drive_controller
   const double Odometry::DEFAULT_POSE_COVARIANCE = 1e-6;
 
   Odometry::Odometry(size_t velocity_rolling_window_size)
-  : timestamp_(0.0)
-  , timestamp_twist_(0.0)
+  : timestamp_twist_(0.0)
   , x_(0.0)
   , y_(0.0)
   , heading_(0.0)
@@ -102,13 +101,13 @@ namespace diff_drive_controller
   {
     // Reset accumulators and timestamp:
     resetAccumulators();
-    timestamp_ = timestamp_twist_ = time;
+    timestamp_twist_ = time;
   }
 
   bool Odometry::updateCloseLoop(
       const double left_position, const double right_position,
       const double left_velocity, const double right_velocity,
-      const ros::Time &time)
+      const double dt)
   {
     /// Estimate wheels position increment using previous and current position:
     const double left_position_increment  = left_position  - left_position_previous_;
@@ -120,11 +119,11 @@ namespace diff_drive_controller
 
     /// Update pose and twist:
     return update(left_position_increment, right_position_increment,
-        left_velocity, right_velocity, time);
+        left_velocity, right_velocity, dt);
   }
 
   bool Odometry::updateOpenLoop(const double linear, const double angular,
-      const ros::Time& time)
+      const double dt)
   {
     /// Compute wheel velocities, i.e. Inverse Kinematics:
     // @todo we should expose a method to compute this:
@@ -140,18 +139,14 @@ namespace diff_drive_controller
     const double v_l = (linear - angular * wheel_separation_ / 2.0) / left_wheel_radius_;
     const double v_r = (linear + angular * wheel_separation_ / 2.0) / right_wheel_radius_;
 
-    /// Compute time step:
-    const double dt = (time - timestamp_).toSec();
-    timestamp_ = time;
-
     /// Update pose and twist:
-    return update(v_l * dt, v_r * dt, v_l, v_r, time);
+    return update(v_l * dt, v_r * dt, v_l, v_r, dt);
   }
 
   bool Odometry::update(
       const double dp_l, const double dp_r,
       const double v_l, const double v_r,
-      const ros::Time& time)
+      const double dt)
   {
     /// Integrate odometry pose:
     IntegrateFunction::PoseJacobian J_pose;
@@ -166,12 +161,7 @@ namespace diff_drive_controller
                        J_meas * meas_covariance  * J_meas.transpose();
 
     /// Update incremental pose:
-    // @todo in principle there's no need to use v_l, v_r at all, but this
-    // should be decided from outside, so here we should use:
-    // updateIncrementalPose(v_l * dt, v_r * dt);
-    // which could be equivalent to dp_l, dp_r or not.
-    // Note that we need to obtain dt from the time (which it's not used now)
-    updateIncrementalPose(dp_l, dp_r);
+    updateIncrementalPose(v_l * dt, v_r * dt);
 
     return true;
   }

diff_drive_controller/test/diff_drive_test.cpp

@@ -114,40 +114,17 @@ TEST_F(DiffDriveControllerTest, testNoMove)
 
   // check we have a positive definite covariance matrix
   // isCovariance = isSymmetric + isPositiveDefinite
-  const Eigen::IOFormat HeavyFmt(
-      Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");
-
-  typedef Eigen::SelfAdjointEigenSolver<PoseCovariance> PoseEigenSolver;
-  typedef Eigen::SelfAdjointEigenSolver<TwistCovariance> TwistEigenSolver;
-
-  PoseEigenSolver pose_eigensolver(pose_covariance);
-  TwistEigenSolver twist_eigensolver(twist_covariance);
-
-  EXPECT_TRUE(isSymmetric(pose_covariance))
-    << "Pose covariance =\n" << pose_covariance.format(HeavyFmt);
-  EXPECT_TRUE(isPositiveDefinite(pose_covariance, no_tag()))
-    << "Pose covariance =\n" << pose_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << pose_eigensolver.eigenvalues().transpose().format(HeavyFmt);
-  EXPECT_LT(conditionNumber(pose_covariance),
-      POSE_COVARIANCE_MAX_CONDITION_NUMBER)
-    << "Pose covariance =\n" << pose_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << pose_eigensolver.eigenvalues().transpose().format(HeavyFmt);
-
-  EXPECT_TRUE(isSymmetric(twist_covariance))
-    << "Twist covariance =\n" << twist_covariance.format(HeavyFmt);
-  EXPECT_TRUE(isPositiveDefinite(twist_covariance, no_tag()))
-    << "Twist covariance =\n" << twist_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << twist_eigensolver.eigenvalues().transpose().format(HeavyFmt);
-  EXPECT_LT(conditionNumber(pose_covariance),
-      TWIST_COVARIANCE_MAX_CONDITION_NUMBER)
-    << "Twist covariance =\n" << twist_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << twist_eigensolver.eigenvalues().transpose().format(HeavyFmt);
+  testCovariance(pose_covariance, POSE_COVARIANCE_MAX_CONDITION_NUMBER);
+  testCovariance(twist_covariance, TWIST_COVARIANCE_MAX_CONDITION_NUMBER);
 
   // when the robot doesn't move the twist covariance should be exactly the
   // minimum twist covariance, which is defined as a diagonal covariance matrix
   // like this:
   //   Odometry::DEFAULT_MINIMUM_TWIST_COVARIANCE * TwistCovariance::Identity()
   // where Odometry::DEFAULT_MINIMUM_TWIST_COVARIANCE == 1e-9
+  const Eigen::IOFormat HeavyFmt(
+      Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");
+
   TwistCovariance minimum_twist_covariance = 1e-9 * TwistCovariance::Identity();
   EXPECT_TRUE(((twist_covariance - minimum_twist_covariance).array() == 0).all())
     << "Twist covariance =\n" << twist_covariance.format(HeavyFmt) << "\n"
@@ -306,34 +283,8 @@ TEST_F(DiffDriveControllerTest, testForward)
 
   // check we have a positive definite covariance matrix
   // isCovariance = isSymmetric + isPositiveDefinite
-  const Eigen::IOFormat HeavyFmt(
-      Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");
-
-  typedef Eigen::SelfAdjointEigenSolver<PoseCovariance> PoseEigenSolver;
-  typedef Eigen::SelfAdjointEigenSolver<TwistCovariance> TwistEigenSolver;
-
-  PoseEigenSolver pose_eigensolver(pose_covariance);
-  TwistEigenSolver twist_eigensolver(twist_covariance);
-
-  EXPECT_TRUE(isSymmetric(pose_covariance))
-    << "Pose covariance =\n" << pose_covariance.format(HeavyFmt);
-  EXPECT_TRUE(isPositiveDefinite(pose_covariance, no_tag()))
-    << "Pose covariance =\n" << pose_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << pose_eigensolver.eigenvalues().transpose().format(HeavyFmt);
-  EXPECT_LT(conditionNumber(pose_covariance),
-      POSE_COVARIANCE_MAX_CONDITION_NUMBER)
-    << "Pose covariance =\n" << pose_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << pose_eigensolver.eigenvalues().transpose().format(HeavyFmt);
-
-  EXPECT_TRUE(isSymmetric(twist_covariance))
-    << "Twist covariance =\n" << twist_covariance.format(HeavyFmt);
-  EXPECT_TRUE(isPositiveDefinite(twist_covariance, no_tag()))
-    << "Twist covariance =\n" << twist_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << twist_eigensolver.eigenvalues().transpose().format(HeavyFmt);
-  EXPECT_LT(conditionNumber(pose_covariance),
-      TWIST_COVARIANCE_MAX_CONDITION_NUMBER)
-    << "Twist covariance =\n" << twist_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << twist_eigensolver.eigenvalues().transpose().format(HeavyFmt);
+  testCovariance(pose_covariance, POSE_COVARIANCE_MAX_CONDITION_NUMBER);
+  testCovariance(twist_covariance, TWIST_COVARIANCE_MAX_CONDITION_NUMBER);
 
   // Take the last two odometry messages:
   std::vector<nav_msgs::Odometry> odoms = getLastOdoms();
@@ -406,6 +357,9 @@ TEST_F(DiffDriveControllerTest, testForward)
   EXPECT_NEAR(yaw_0, yaw, std::numeric_limits<double>::epsilon());
 
   // Check new pose covariance is equal to the expected one:
+  const Eigen::IOFormat HeavyFmt(
+      Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");
+
   EXPECT_TRUE(((pose_covariance_0_expected - pose_covariance_0).array().abs() < 1e-5).all())
     << "Pose covariance actual =\n" << pose_covariance_0.format(HeavyFmt)
     << "\nPose covariance expected =\n" << pose_covariance_0_expected.format(HeavyFmt);
@@ -493,37 +447,8 @@ TEST_F(DiffDriveControllerTest, testTurn)
 
   // check we have a positive definite covariance matrix
   // isCovariance = isSymmetric + isPositiveDefinite
-  const Eigen::IOFormat HeavyFmt(
-      Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");
-
-  typedef Eigen::SelfAdjointEigenSolver<PoseCovariance> PoseEigenSolver;
-  typedef Eigen::SelfAdjointEigenSolver<TwistCovariance> TwistEigenSolver;
-
-  PoseEigenSolver pose_eigensolver(pose_covariance);
-  TwistEigenSolver twist_eigensolver(twist_covariance);
-
-  EXPECT_TRUE(pose_eigensolver.info() == Eigen::Success);
-  EXPECT_TRUE(twist_eigensolver.info() == Eigen::Success);
-
-  EXPECT_TRUE(isSymmetric(pose_covariance))
-    << "Pose covariance =\n" << pose_covariance.format(HeavyFmt);
-  EXPECT_TRUE(isPositiveDefinite(pose_covariance, no_tag()))
-    << "Pose covariance =\n" << pose_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << pose_eigensolver.eigenvalues().transpose().format(HeavyFmt);
-  EXPECT_LT(conditionNumber(pose_covariance),
-      POSE_COVARIANCE_MAX_CONDITION_NUMBER)
-    << "Pose covariance =\n" << pose_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << pose_eigensolver.eigenvalues().transpose().format(HeavyFmt);
-
-  EXPECT_TRUE(isSymmetric(twist_covariance))
-    << "Twist covariance =\n" << twist_covariance.format(HeavyFmt);
-  EXPECT_TRUE(isPositiveDefinite(twist_covariance, no_tag()))
-    << "Twist covariance =\n" << twist_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << twist_eigensolver.eigenvalues().transpose().format(HeavyFmt);
-  EXPECT_LT(conditionNumber(pose_covariance),
-      TWIST_COVARIANCE_MAX_CONDITION_NUMBER)
-    << "Twist covariance =\n" << twist_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << twist_eigensolver.eigenvalues().transpose().format(HeavyFmt);
+  testCovariance(pose_covariance, POSE_COVARIANCE_MAX_CONDITION_NUMBER);
+  testCovariance(twist_covariance, TWIST_COVARIANCE_MAX_CONDITION_NUMBER);
 
   // Take the last two odometry messages:
   std::vector<nav_msgs::Odometry> odoms = getLastOdoms();
@@ -596,6 +521,9 @@ TEST_F(DiffDriveControllerTest, testTurn)
   EXPECT_NEAR(yaw_0, yaw, ORIENTATION_TOLERANCE);
 
   // Check new pose covariance is equal to the expected one:
+  const Eigen::IOFormat HeavyFmt(
+      Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");
+
   EXPECT_TRUE(((pose_covariance_0_expected - pose_covariance_0).array().abs() < 1e-5).all())
     << "Pose covariance actual =\n" << pose_covariance_0.format(HeavyFmt)
     << "\nPose covariance expected =\n" << pose_covariance_0_expected.format(HeavyFmt);