Fix joint limit handling when velocities aren't included in robot state

moveit_core/robot_model/src/revolute_joint_model.cpp

@@ -163,7 +163,9 @@ bool RevoluteJointModel::satisfiesPositionBounds(const double* values, const Bou
   if (continuous_)
     return true;
   else
+  {
     return !(values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin);
+  }
 }
 
 bool RevoluteJointModel::harmonizePosition(double* values, const JointModel::Bounds& other_bounds) const

moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp

@@ -169,50 +169,41 @@ void OMPLInterface::loadPlannerConfigurations()
   for (const std::string& group_name : robot_model_->getJointModelGroupNames())
   {
     // the set of planning parameters that can be specific for the group (inherited by configurations of that group)
-    static const std::string KNOWN_GROUP_PARAMS[] = { "projection_evaluator", "longest_valid_segment_fraction",
-                                                      "enforce_joint_model_state_space",
-                                                      "enforce_constrained_state_space" };
+    // with their expected parameter type
+    static const std::pair<std::string, rclcpp::ParameterType> KNOWN_GROUP_PARAMS[] = {
+      { "projection_evaluator", rclcpp::ParameterType::PARAMETER_STRING },
+      { "longest_valid_segment_fraction", rclcpp::ParameterType::PARAMETER_DOUBLE },
+      { "enforce_joint_model_state_space", rclcpp::ParameterType::PARAMETER_BOOL },
+      { "enforce_constrained_state_space", rclcpp::ParameterType::PARAMETER_BOOL }
+    };
 
     const std::string group_name_param = parameter_namespace_ + "." + group_name;
 
     // get parameters specific for the robot planning group
     std::map<std::string, std::string> specific_group_params;
-    for (const std::string& k : KNOWN_GROUP_PARAMS)
+    for (const auto& k : KNOWN_GROUP_PARAMS)
     {
-      std::string param_name{ group_name };
+      std::string param_name{ group_name_param };
       param_name += ".";
-      param_name += k;
+      param_name += k.first;
       if (node_->has_parameter(param_name))
       {
-        std::string value;
-        if (node_->get_parameter(param_name, value))
+        const rclcpp::Parameter parameter = node_->get_parameter(param_name);
+        if (parameter.get_type() != k.second)
         {
-          if (!value.empty())
-            specific_group_params[k] = value;
-          continue;
-        }
-
-        double value_d;
-        if (node_->get_parameter(param_name, value_d))
-        {
-          // convert to string using no locale
-          specific_group_params[k] = moveit::core::toString(value_d);
-          continue;
-        }
-
-        int value_i;
-        if (node_->get_parameter(param_name, value_i))
-        {
-          specific_group_params[k] = std::to_string(value_i);
-          continue;
-        }
-
-        bool value_b;
-        if (node_->get_parameter(param_name, value_b))
-        {
-          specific_group_params[k] = std::to_string(value_b);
+          RCLCPP_ERROR_STREAM(LOGGER, "Invalid type for parameter '" << k.first << "' expected ["
+                                                                     << rclcpp::to_string(k.second) << "] got ["
+                                                                     << rclcpp::to_string(parameter.get_type()) << "]");
           continue;
         }
+        if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING)
+          specific_group_params[k.first] = parameter.as_string();
+        else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
+          specific_group_params[k.first] = moveit::core::toString(parameter.as_double());
+        else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
+          specific_group_params[k.first] = std::to_string(parameter.as_int());
+        else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_BOOL)
+          specific_group_params[k.first] = std::to_string(parameter.as_bool());
       }
     }
 
@@ -239,7 +230,7 @@ void OMPLInterface::loadPlannerConfigurations()
 
     // get parameters specific to each planner type
     std::vector<std::string> config_names;
-    if (node_->get_parameter(group_name + ".planner_configs", config_names))
+    if (node_->get_parameter(group_name_param + ".planner_configs", config_names))
     {
       for (const auto& planner_id : config_names)
       {

moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h

@@ -156,7 +156,7 @@ class ServoCalcs
   void enforceVelLimits(Eigen::ArrayXd& delta_theta);
 
   /** \brief Avoid overshooting joint limits */
-  bool enforcePositionLimits();
+  bool enforcePositionLimits(trajectory_msgs::msg::JointTrajectory& joint_trajectory) const;
 
   /** \brief Possibly calculate a velocity scaling factor, due to proximity of
    * singularity and direction of motion

moveit_ros/moveit_servo/src/servo_calcs.cpp

@@ -582,7 +582,7 @@ bool ServoCalcs::internalServoUpdate(Eigen::ArrayXd& delta_theta,
   composeJointTrajMessage(internal_joint_state_, joint_trajectory);
 
   // Enforce SRDF position limits, might halt if needed, set prev_vel to 0
-  if (!enforcePositionLimits())
+  if (!enforcePositionLimits(joint_trajectory))
   {
     suddenHalt(joint_trajectory);
     status_ = StatusCode::JOINT_BOUND;
@@ -809,43 +809,52 @@ void ServoCalcs::enforceVelLimits(Eigen::ArrayXd& delta_theta)
   }
 }
 
-bool ServoCalcs::enforcePositionLimits()
+bool ServoCalcs::enforcePositionLimits(trajectory_msgs::msg::JointTrajectory& joint_trajectory) const
 {
+  // Halt if we're past a joint margin and joint velocity is moving even farther past
   bool halting = false;
+  double joint_angle = 0;
 
   for (auto joint : joint_model_group_->getActiveJointModels())
   {
-    // Halt if we're past a joint margin and joint velocity is moving even farther past
-    double joint_angle = 0;
     for (std::size_t c = 0; c < original_joint_state_.name.size(); ++c)
     {
+      // Use the most recent robot joint state
       if (original_joint_state_.name[c] == joint->getName())
       {
         joint_angle = original_joint_state_.position.at(c);
         break;
       }
     }
+
     if (!current_state_->satisfiesPositionBounds(joint, -parameters_->joint_limit_margin))
     {
       const std::vector<moveit_msgs::msg::JointLimits> limits = joint->getVariableBoundsMsg();
 
       // Joint limits are not defined for some joints. Skip them.
       if (!limits.empty())
       {
-        if ((current_state_->getJointVelocities(joint)[0] < 0 &&
+        // Check if pending velocity command is moving in the right direction
+        auto joint_itr =
+            std::find(joint_trajectory.joint_names.begin(), joint_trajectory.joint_names.end(), joint->getName());
+        auto joint_idx = std::distance(joint_trajectory.joint_names.begin(), joint_itr);
+
+        if ((joint_trajectory.points[0].velocities[joint_idx] < 0 &&
              (joint_angle < (limits[0].min_position + parameters_->joint_limit_margin))) ||
-            (current_state_->getJointVelocities(joint)[0] > 0 &&
+            (joint_trajectory.points[0].velocities[joint_idx] > 0 &&
              (joint_angle > (limits[0].max_position - parameters_->joint_limit_margin))))
         {
           rclcpp::Clock& clock = *node_->get_clock();
           RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD,
                                       node_->get_name()
                                           << " " << joint->getName() << " close to a position limit. Halting.");
           halting = true;
+          break;
         }
       }
     }
   }
+
   return !halting;
 }