Porting diff drive to moveit1

- Backporting Differential Drive implementation
  MoveIt2 (moveit/moveit2#390)

moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h

@@ -46,6 +46,13 @@ namespace core
 class PlanarJointModel : public JointModel
 {
 public:
+  /** \brief different types of planar joints we support */
+  enum MotionModel
+  {
+    HOLONOMIC,  // default
+    DIFF_DRIVE
+  };
+
   PlanarJointModel(const std::string& name);
 
   void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override;
@@ -75,13 +82,51 @@ class PlanarJointModel : public JointModel
     angular_distance_weight_ = weight;
   }
 
+  double getMinTranslationalDistance() const
+  {
+    return min_translational_distance_;
+  }
+
+  void setMinTranslationalDistance(double min_translational_distance)
+  {
+    min_translational_distance_ = min_translational_distance;
+  }
+
+  MotionModel getMotionModel() const
+  {
+    return motion_model_;
+  }
+
+  void setMotionModel(MotionModel model)
+  {
+    motion_model_ = model;
+  }
+
   /// Make the yaw component of a state's value vector be in the range [-Pi, Pi]. enforceBounds() also calls this
   /// function;
   /// Return true if a change is actually made
   bool normalizeRotation(double* values) const;
 
 private:
   double angular_distance_weight_;
+  MotionModel motion_model_;
+  /// Only used for the differential drive motion model @see computeTurnDriveTurnGeometry
+  double min_translational_distance_;
 };
+/**
+ * @brief Compute the geometry to turn toward the target point, drive straight and then turn to target orientation
+ * @param[in]  from                       A vector representing the initial position [x0, y0, theta0]
+ * @param[in]  to                         A vector representing the target position  [x1, y1, theta1]
+ * @param[in]  min_translational_distance If the translational distance between \p from and \p to is less than this
+ *                                        value the motion will be pure rotation (meters)
+ * @param[out] dx                         x1 - x0 (meters)
+ * @param[out] dy                         y1 - y0 (meters)
+ * @param[out] initial_turn               The initial turn in radians to face the target
+ * @param[out] drive_angle                The orientation in radians that the robot will be driving straight at
+ * @param[out] final_turn                 The final turn in radians to the target orientation
+ */
+void computeTurnDriveTurnGeometry(const double* from, const double* to, const double min_translational_distance,
+                                  double& dx, double& dy, double& initial_turn, double& drive_angle,
+                                  double& final_turn);
 }  // namespace core
 }  // namespace moveit

moveit_core/robot_model/src/planar_joint_model.cpp

@@ -37,6 +37,7 @@
 
 #include <moveit/robot_model/planar_joint_model.h>
 #include <geometric_shapes/check_isometry.h>
+#include <angles/angles.h>
 #include <boost/math/constants/constants.hpp>
 #include <limits>
 #include <cmath>
@@ -45,7 +46,8 @@ namespace moveit
 {
 namespace core
 {
-PlanarJointModel::PlanarJointModel(const std::string& name) : JointModel(name), angular_distance_weight_(1.0)
+PlanarJointModel::PlanarJointModel(const std::string& name)
+  : JointModel(name), angular_distance_weight_(1.0), motion_model_(HOLONOMIC), min_translational_distance_(1e-5)
 {
   type_ = PLANAR;
 
@@ -139,39 +141,127 @@ void PlanarJointModel::getVariableRandomPositionsNearBy(random_numbers::RandomNu
   normalizeRotation(values);
 }
 
+void computeTurnDriveTurnGeometry(const double* from, const double* to, const double min_translational_distance,
+                                  double& dx, double& dy, double& initial_turn, double& drive_angle, double& final_turn)
+{
+  dx = to[0] - from[0];
+  dy = to[1] - from[1];
+  // If the translational distance between from & to states is very small, it will cause an unnecessary rotation since
+  // the robot will try to do the following rather than rotating directly to the orientation of `to` state
+  // 1- Align itself with the line connecting the origin of both states
+  // 2- Move to the origin of `to` state
+  // 3- Rotate so it have the same orientation as `to` state
+  // Example: from=[0.0, 0.0, 0.0] - to=[1e-31, 1e-31, -130°]
+  // here the robot will: rotate 45° -> move to the origin of `to` state -> rotate -175°, rather than rotating directly
+  // to -130°
+  // to fix this we added a joint property (default value is 1e-5) and make the movement pure rotation if the
+  // translational distance is less than this number
+  const double angle_straight_diff = std::hypot(dx, dy) > min_translational_distance ?
+                                         angles::shortest_angular_distance(from[2], std::atan2(dy, dx)) :
+                                         0.0;
+  const double angle_backward_diff =
+      angles::normalize_angle(angle_straight_diff - boost::math::constants::pi<double>());
+  const double move_straight_cost =
+      std::abs(angle_straight_diff) + std::abs(angles::shortest_angular_distance(from[2] + angle_straight_diff, to[2]));
+  const double move_backward_cost =
+      std::abs(angle_backward_diff) + std::abs(angles::shortest_angular_distance(from[2] + angle_backward_diff, to[2]));
+  if (move_straight_cost <= move_backward_cost)
+  {
+    initial_turn = angle_straight_diff;
+  }
+  else
+  {
+    initial_turn = angle_backward_diff;
+  }
+  drive_angle = from[2] + initial_turn;
+  final_turn = angles::shortest_angular_distance(drive_angle, to[2]);
+}
+
 void PlanarJointModel::interpolate(const double* from, const double* to, const double t, double* state) const
 {
-  // interpolate position
-  state[0] = from[0] + (to[0] - from[0]) * t;
-  state[1] = from[1] + (to[1] - from[1]) * t;
+  if (motion_model_ == HOLONOMIC)
+  {
+    // interpolate position
+    state[0] = from[0] + (to[0] - from[0]) * t;
+    state[1] = from[1] + (to[1] - from[1]) * t;
 
-  // interpolate angle
-  double diff = to[2] - from[2];
-  if (fabs(diff) <= boost::math::constants::pi<double>())
-    state[2] = from[2] + diff * t;
-  else
+    // interpolate angle
+    double diff = to[2] - from[2];
+    if (fabs(diff) <= boost::math::constants::pi<double>())
+      state[2] = from[2] + diff * t;
+    else
+    {
+      if (diff > 0.0)
+        diff = 2.0 * boost::math::constants::pi<double>() - diff;
+      else
+        diff = -2.0 * boost::math::constants::pi<double>() - diff;
+      state[2] = from[2] - diff * t;
+      // input states are within bounds, so the following check is sufficient
+      if (state[2] > boost::math::constants::pi<double>())
+        state[2] -= 2.0 * boost::math::constants::pi<double>();
+      else if (state[2] < -boost::math::constants::pi<double>())
+        state[2] += 2.0 * boost::math::constants::pi<double>();
+    }
+  }
+  else if (motion_model_ == DIFF_DRIVE)
   {
-    if (diff > 0.0)
-      diff = 2.0 * boost::math::constants::pi<double>() - diff;
+    double dx, dy, initial_turn, drive_angle, final_turn;
+    computeTurnDriveTurnGeometry(from, to, min_translational_distance_, dx, dy, initial_turn, drive_angle, final_turn);
+
+    double initial_d = fabs(initial_turn) * angular_distance_weight_;
+    double drive_d = hypot(dx, dy);
+    double final_d = fabs(final_turn) * angular_distance_weight_;
+
+    double total_d = initial_d + drive_d + final_d;
+
+    double initial_frac = initial_d / total_d;
+    double drive_frac = drive_d / total_d;
+    double final_frac = final_d / total_d;
+
+    double percent;
+    if (t <= initial_frac)
+    {
+      percent = t / initial_frac;
+      state[0] = from[0];
+      state[1] = from[1];
+      state[2] = from[2] + initial_turn * percent;
+    }
+    else if (t <= initial_frac + drive_frac)
+    {
+      percent = (t - initial_frac) / drive_frac;
+      state[0] = from[0] + dx * percent;
+      state[1] = from[1] + dy * percent;
+      state[2] = drive_angle;
+    }
     else
-      diff = -2.0 * boost::math::constants::pi<double>() - diff;
-    state[2] = from[2] - diff * t;
-    // input states are within bounds, so the following check is sufficient
-    if (state[2] > boost::math::constants::pi<double>())
-      state[2] -= 2.0 * boost::math::constants::pi<double>();
-    else if (state[2] < -boost::math::constants::pi<double>())
-      state[2] += 2.0 * boost::math::constants::pi<double>();
+    {
+      percent = (t - initial_frac - drive_frac) / final_frac;
+      state[0] = to[0];
+      state[1] = to[1];
+      state[2] = drive_angle + final_turn * percent;
+    }
   }
 }
 
 double PlanarJointModel::distance(const double* values1, const double* values2) const
 {
-  double dx = values1[0] - values2[0];
-  double dy = values1[1] - values2[1];
+  if (motion_model_ == HOLONOMIC)
+  {
+    double dx = values1[0] - values2[0];
+    double dy = values1[1] - values2[1];
 
-  double d = fabs(values1[2] - values2[2]);
-  d = (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
-  return sqrt(dx * dx + dy * dy) + angular_distance_weight_ * d;
+    double d = fabs(values1[2] - values2[2]);
+    d = (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
+    return sqrt(dx * dx + dy * dy) + angular_distance_weight_ * d;
+  }
+  else if (motion_model_ == DIFF_DRIVE)
+  {
+    double dx, dy, initial_turn, drive_angle, final_turn;
+    computeTurnDriveTurnGeometry(values1, values2, min_translational_distance_, dx, dy, initial_turn, drive_angle,
+                                 final_turn);
+    return hypot(dx, dy) + angular_distance_weight_ * (fabs(initial_turn) + fabs(final_turn));
+  }
+  return 0.0;
 }
 
 bool PlanarJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds, double margin) const

moveit_core/robot_model/src/robot_model.cpp

@@ -976,6 +976,112 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const
         break;
       }
     }
+    for (const srdf::Model::JointProperty& property : srdf_model.getJointProperties(new_joint_model->getName()))
+    {
+      if (property.property_name_ == "angular_distance_weight")
+      {
+        double angular_distance_weight;
+        try
+        {
+          std::string::size_type sz;
+          angular_distance_weight = std::stod(property.value_, &sz);
+          if (sz != property.value_.size())
+          {
+            ROS_WARN_STREAM_NAMED(LOGNAME, "Extra characters after property " << property.property_name_ << " for joint "
+                                                                              << property.joint_name_ << " as double: '"
+                                                                              << property.value_.substr(sz) << "'");
+          }
+        }
+        catch (const std::invalid_argument& e)
+        {
+          ROS_ERROR_STREAM_NAMED(LOGNAME, "Unable to parse property " << property.property_name_ << " for joint "
+                                                                      << property.joint_name_ << " as double: '"
+                                                                      << property.value_ << "'");
+          continue;
+        }
+
+        if (new_joint_model->getType() == JointModel::JointType::PLANAR)
+        {
+          ((PlanarJointModel*)new_joint_model)->setAngularDistanceWeight(angular_distance_weight);
+        }
+        else if (new_joint_model->getType() == JointModel::JointType::FLOATING)
+        {
+          ((FloatingJointModel*)new_joint_model)->setAngularDistanceWeight(angular_distance_weight);
+        }
+        else
+        {
+          ROS_ERROR_NAMED(LOGNAME, "Cannot apply property %s to joint type: %s",
+                                   property.property_name_.c_str(),
+                                   new_joint_model->getTypeName().c_str());
+        }
+      }
+      else if (property.property_name_ == "motion_model")
+      {
+        if (new_joint_model->getType() != JointModel::JointType::PLANAR)
+        {
+          ROS_ERROR_NAMED(LOGNAME, "Cannot apply property %s to joint type: %s",
+                          property.property_name_.c_str(),
+                          new_joint_model->getTypeName().c_str());
+          continue;
+        }
+
+        PlanarJointModel::MotionModel motion_model;
+        if (property.value_ == "holonomic")
+        {
+          motion_model = PlanarJointModel::MotionModel::HOLONOMIC;
+        }
+        else if (property.value_ == "diff_drive")
+        {
+          motion_model = PlanarJointModel::MotionModel::DIFF_DRIVE;
+        }
+        else
+        {
+          ROS_ERROR_STREAM_NAMED(LOGNAME, "Unknown value for property " << property.property_name_ << " ("
+                                                                        << property.joint_name_ << "): '" << property.value_
+                                                                        << "'");
+          ROS_ERROR_NAMED(LOGNAME, "Valid values are 'holonomic' and 'diff_drive'");
+          continue;
+        }
+
+        ((PlanarJointModel*)new_joint_model)->setMotionModel(motion_model);
+      }
+      else if (property.property_name_ == "min_translational_distance")
+      {
+        if (new_joint_model->getType() != JointModel::JointType::PLANAR)
+        {
+          ROS_ERROR_NAMED(LOGNAME, "Cannot apply property %s to joint type: %s",
+                          property.property_name_.c_str(),
+                          new_joint_model->getTypeName().c_str());
+          continue;
+        }
+        double min_translational_distance;
+        try
+        {
+          std::string::size_type sz;
+          min_translational_distance = std::stod(property.value_, &sz);
+          if (sz != property.value_.size())
+          {
+            ROS_WARN_STREAM_NAMED(LOGNAME, "Extra characters after property " << property.property_name_ << " for joint "
+                                                                       << property.joint_name_ << " as double: '"
+                                                                       << property.value_.substr(sz) << "'");
+          }
+        }
+        catch (const std::invalid_argument& e)
+        {
+          ROS_ERROR_NAMED(LOGNAME, "Unable to parse property %s for joint %s as double: '%s'",
+                          property.property_name_.c_str(),
+                          property.joint_name_.c_str(),
+                          property.value_.c_str());
+          continue;
+        }
+
+        ((PlanarJointModel*)new_joint_model)->setMinTranslationalDistance(min_translational_distance);
+      }
+      else
+      {
+        ROS_ERROR_NAMED(LOGNAME, "Unknown joint property: %s", property.property_name_.c_str());
+      }
+    }
   }
 
   return new_joint_model;

moveit_core/utils/include/moveit/utils/robot_model_test_utils.h

@@ -185,6 +185,13 @@ class RobotModelBuilder
   RobotModelBuilder& addEndEffector(const std::string& name, const std::string& parent_link,
                                     const std::string& parent_group = "", const std::string& component_group = "");
 
+  /** \brief Adds a new joint property
+   *  \param[in] joint_name The name of the joint the property is specified for
+   *  \param[in] property_name The joint property name
+   *  \param[in] value The value of the joint property
+   */
+  void addJointProperty(const std::string& joint_name, const std::string& property_name, const std::string& value);
+
   /** \} */
 
   /** \brief Returns true if the building process so far has been valid. */

moveit_core/utils/src/robot_model_test_utils.cpp

@@ -394,6 +394,12 @@ RobotModelBuilder& RobotModelBuilder::addEndEffector(const std::string& name, co
   return *this;
 }
 
+void RobotModelBuilder::addJointProperty(const std::string& joint_name, const std::string& property_name,
+                                         const std::string& value)
+{
+  srdf_writer_->joint_properties_[joint_name].push_back({ joint_name, property_name, value });
+}
+
 bool RobotModelBuilder::isValid()
 {
   return is_valid_;