Fix tolerance handling

UniversalRobots · Oct 2, 2024 · 1389c3c · 1389c3c
1 parent bd9652e
commit 1389c3c
Show file tree

Hide file tree

Showing 4 changed files with 76 additions and 32 deletions.
diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp
@@ -180,9 +180,10 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI
   std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>> joint_velocity_state_interface_;
   std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>> joint_acceleration_state_interface_;
 
-  bool check_positions(std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal);
-  bool check_velocities(std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal);
-  bool check_accelerations(std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal);
+  bool check_goal_tolerances(std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal);
+  bool check_goal_positions(std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal);
+  bool check_goal_velocities(std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal);
+  bool check_goal_accelerations(std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal);
 
   trajectory_msgs::msg::JointTrajectory active_joint_traj_;
   // std::vector<control_msgs::msg::JointTolerance> path_tolerance_;

diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp
@@ -40,6 +40,8 @@
 
 #include <algorithm>
 #include <cmath>
+#include <sstream>
+
 #include <controller_interface/controller_interface.hpp>
 #include <rclcpp/logging.hpp>
 #include <builtin_interfaces/msg/duration.hpp>
@@ -316,27 +318,50 @@ rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callb
   }
 
   // Check that all parts of the trajectory are valid.
-  if (!check_positions(goal)) {
+  if (!check_goal_positions(goal)) {
     RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected");
     return rclcpp_action::GoalResponse::REJECT;
   }
-  if (!check_velocities(goal)) {
+  if (!check_goal_velocities(goal)) {
     RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected");
     return rclcpp_action::GoalResponse::REJECT;
   }
-  if (!check_accelerations(goal)) {
+  if (!check_goal_accelerations(goal)) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected");
+    return rclcpp_action::GoalResponse::REJECT;
+  }
+  if (!check_goal_tolerances(goal)) {
     RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected");
     return rclcpp_action::GoalResponse::REJECT;
   }
-
-  // TODO(fexner): Validate goal
-  // - Do the tolerances contain correct joint names?
-  // - Do the tolerances contain correct values?
 
   return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
 }
 
-bool PassthroughTrajectoryController::check_positions(
+bool PassthroughTrajectoryController::check_goal_tolerances(
+    std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal)
+{
+  auto& tolerances = goal->goal_tolerance;
+  auto joint_names_internal = joint_names_.readFromRT();
+  if (!tolerances.empty()) {
+    for (auto& tol : tolerances) {
+      auto found_it = std::find(joint_names_internal->begin(), joint_names_internal->end(), tol.name);
+      if (found_it == joint_names_internal->end()) {
+        RCLCPP_ERROR(get_node()->get_logger(),
+                     "Tolerance for joint '%s' given. This joint is not known to this controller.", tol.name.c_str());
+        return false;
+      }
+    }
+    if (tolerances.size() != number_of_joints_) {
+      RCLCPP_ERROR(get_node()->get_logger(), "Tolerances for %lu joints given. This controller knows %lu joints.",
+                   tolerances.size(), number_of_joints_.load());
+      return false;
+    }
+  }
+  return true;
+}
+
+bool PassthroughTrajectoryController::check_goal_positions(
     std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal)
 {
   for (uint32_t i = 0; i < goal->trajectory.points.size(); i++) {
@@ -354,7 +379,7 @@ bool PassthroughTrajectoryController::check_positions(
   return true;
 }
 
-bool PassthroughTrajectoryController::check_velocities(
+bool PassthroughTrajectoryController::check_goal_velocities(
     std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal)
 {
   for (uint32_t i = 0; i < goal->trajectory.points.size(); i++) {
@@ -385,7 +410,7 @@ bool PassthroughTrajectoryController::check_velocities(
   return true;
 }
 
-bool PassthroughTrajectoryController::check_accelerations(
+bool PassthroughTrajectoryController::check_goal_accelerations(
     std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal)
 {
   for (uint32_t i = 0; i < goal->trajectory.points.size(); i++) {
@@ -448,17 +473,27 @@ void PassthroughTrajectoryController::goal_accepted_callback(
 
   // sort goal tolerances to match internal joint order
   std::vector<control_msgs::msg::JointTolerance> goal_tolerances;
-  auto joint_names_internal = joint_names_.readFromRT();
-  for (auto& joint_name : *joint_names_internal) {
-    auto found_it =
-        std::find_if(goal_handle->get_goal()->goal_tolerance.begin(), goal_handle->get_goal()->goal_tolerance.end(),
-                     [&joint_name](auto& tol) { return tol.name == joint_name; });
-    if (found_it != goal_handle->get_goal()->goal_tolerance.end()) {
-      goal_tolerances.push_back(*found_it);
+  if (!goal_handle->get_goal()->goal_tolerance.empty()) {
+    auto joint_names_internal = joint_names_.readFromRT();
+    std::stringstream ss;
+    ss << "Using goal tolerances\n";
+    for (auto& joint_name : *joint_names_internal) {
+      auto found_it =
+          std::find_if(goal_handle->get_goal()->goal_tolerance.begin(), goal_handle->get_goal()->goal_tolerance.end(),
+                       [&joint_name](auto& tol) { return tol.name == joint_name; });
+      if (found_it != goal_handle->get_goal()->goal_tolerance.end()) {
+        goal_tolerances.push_back(*found_it);
+        ss << joint_name << " -- position: " << found_it->position << ", velocity: " << found_it->velocity
+           << ", acceleration: " << found_it->acceleration << std::endl;
+      }
     }
+    RCLCPP_INFO_STREAM(get_node()->get_logger(), ss.str());
   }
   goal_tolerance_.writeFromNonRT(goal_tolerances);
   goal_time_tolerance_.writeFromNonRT(goal_handle->get_goal()->goal_time_tolerance);
+  RCLCPP_INFO_STREAM(get_node()->get_logger(),
+                     "Goal time tolerance: " << duration_to_double(goal_handle->get_goal()->goal_time_tolerance)
+                                             << " sec");
 
   // Action handling will be done from the timer callback to avoid those things in the realtime
   // thread. First, we delete the existing (if any) timer by resetting the pointer and then create a new
@@ -479,32 +514,33 @@ bool PassthroughTrajectoryController::check_goal_tolerance()
   auto goal_tolerance = goal_tolerance_.readFromRT();
   auto joint_mapping = joint_trajectory_mapping_.readFromRT();
   auto joint_names_internal = joint_names_.readFromRT();
+  if (goal_tolerance->empty()) {
+    return true;
+  }
+
   for (size_t i = 0; i < number_of_joints_; ++i) {
     const std::string joint_name = joint_names_internal->at(i);
     const auto& joint_tol = goal_tolerance->at(i);
     const auto& setpoint = active_joint_traj_.points.back().positions[joint_mapping->at(joint_name)];
     const double joint_pos = joint_position_state_interface_[i].get().get_value();
     if (std::abs(joint_pos - setpoint) > joint_tol.position) {
-      RCLCPP_ERROR(get_node()->get_logger(), "Joint %s should be at position %f, but is at position %f",
-                   joint_position_state_interface_[i].get().get_name().c_str(), setpoint, joint_pos);
+      // RCLCPP_ERROR(
+      // get_node()->get_logger(), "Joint %s should be at position %f, but is at position %f, where tolerance is %f",
+      // joint_position_state_interface_[i].get().get_name().c_str(), setpoint, joint_pos, joint_tol.position);
       return false;
     }
 
     if (!active_joint_traj_.points.back().velocities.empty()) {
       const double joint_vel = joint_velocity_state_interface_[i].get().get_value();
       const auto& expected_vel = active_joint_traj_.points.back().velocities[joint_mapping->at(joint_name)];
       if (std::abs(joint_vel - expected_vel) > joint_tol.velocity) {
-        RCLCPP_ERROR(get_node()->get_logger(), "Joint %s should be at position %f, but is at position %f",
-                     joint_position_state_interface_[i].get().get_name().c_str(), setpoint, joint_pos);
         return false;
       }
     }
     if (!active_joint_traj_.points.back().accelerations.empty()) {
       const double joint_vel = joint_acceleration_state_interface_[i].get().get_value();
       const auto& expected_vel = active_joint_traj_.points.back().accelerations[joint_mapping->at(joint_name)];
       if (std::abs(joint_vel - expected_vel) > joint_tol.acceleration) {
-        RCLCPP_ERROR(get_node()->get_logger(), "Joint %s should be at position %f, but is at position %f",
-                     joint_position_state_interface_[i].get().get_name().c_str(), setpoint, joint_pos);
         return false;
       }
     }

diff --git a/ur_robot_driver/scripts/example_move.py b/ur_robot_driver/scripts/example_move.py
@@ -134,7 +134,7 @@ def execute_trajectory(self, traj_name):
         goal = FollowJointTrajectory.Goal()
         goal.trajectory = self.goals[traj_name]
 
-        goal.goal_time_tolerance = Duration(sec=0, nanosec=1000)
+        goal.goal_time_tolerance = Duration(sec=0, nanosec=500000000)
         goal.goal_tolerance = [JointTolerance(position=0.01, name=self.joints[i]) for i in range(6)]
 
         self._send_goal_future = self._action_client.send_goal_async(goal)
@@ -158,7 +158,7 @@ def get_result_callback(self, future):
             time.sleep(2)
             self.execute_next_trajectory()
         else:
-            raise RuntimeError("Executing trajectory failed")
+            raise RuntimeError("Executing trajectory failed. " + result.error_string)
 
     @staticmethod
     def error_code_to_str(error_code):
@@ -182,6 +182,8 @@ def main(args=None):
     jtc_client = JTCClient()
     try:
         rclpy.spin(jtc_client)
+    except RuntimeError as err:
+        jtc_client.get_logger().error(str(err))
     except SystemExit:
         rclpy.logging.get_logger("jtc_client").info("Done")
 

diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py
@@ -280,7 +280,10 @@ def test_trajectory_scaled_aborts_on_violation(self, tf_prefix):
                 Duration(sec=6, nanosec=50000000),
                 [-1.0 for j in ROBOT_JOINTS],
             ),  # physically unfeasible
-            (Duration(sec=8, nanosec=0), [-1.5 for j in ROBOT_JOINTS]),  # physically unfeasible
+            (
+                Duration(sec=8, nanosec=0),
+                [-1.5 for j in ROBOT_JOINTS],
+            ),  # physically unfeasible
         ]
 
         trajectory = JointTrajectory(
@@ -392,7 +395,7 @@ def test_passthrough_trajectory(self, tf_prefix):
             self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)
         # Test impossible goal tolerance, should fail.
         goal_tolerance = [
-            JointTolerance(position=0.000000000000000001, name=ROBOT_JOINTS[i])
+            JointTolerance(position=0.000000001, name=tf_prefix + ROBOT_JOINTS[i])
             for i in range(len(ROBOT_JOINTS))
         ]
         goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
@@ -410,9 +413,11 @@ def test_passthrough_trajectory(self, tf_prefix):
             )
 
         # Test impossible goal time
-        goal_tolerance = [JointTolerance(position=0.01) for i in range(6)]
+        goal_tolerance = [
+            JointTolerance(position=0.01, name=tf_prefix + ROBOT_JOINTS[i]) for i in range(6)
+        ]
         goal_time_tolerance.sec = 0
-        goal_time_tolerance.nanosec = 1000
+        goal_time_tolerance.nanosec = 10
         goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
             trajectory=trajectory,
             goal_time_tolerance=goal_time_tolerance,