Added check (with tests) for gain_scaling and damping_factor limits

Author: urfeex

ur_controllers/src/force_mode_controller.cpp

@@ -303,8 +303,27 @@ bool ForceModeController::setForceMode(const ur_msgs::srv::SetForceMode::Request
 
   /* The damping factor decides how fast the robot decelarates if no force is present. 0 means no deceleration, 1
    * means quick deceleration*/
+  if (req->damping_factor < 0.0 || req->damping_factor > 1.0) {
+    RCLCPP_ERROR(get_node()->get_logger(), "The damping factor has to be between 0 and 1. Received %f",
+                 req->damping_factor);
+    resp->success = false;
+    return false;
+  }
   force_mode_parameters.damping_factor = req->damping_factor;
+
   /*The gain scaling factor scales the force mode gain. A value larger than 1 may make force mode unstable. */
+  if (req->gain_scaling < 0.0 || req->gain_scaling > 2.0) {
+    RCLCPP_ERROR(get_node()->get_logger(), "The gain scaling has to be between 0 and 2. Received %f",
+                 req->gain_scaling);
+    resp->success = false;
+    return false;
+  }
+  if (req->gain_scaling > 1.0) {
+    RCLCPP_WARN(get_node()->get_logger(),
+                "A gain_scaling >1.0 can make force mode unstable, e.g. in case of collisions or pushing against "
+                "hard surfaces. Received %f",
+                req->gain_scaling);
+  }
   force_mode_parameters.gain_scaling = req->gain_scaling;
 
   force_mode_params_buffer_.writeFromNonRT(force_mode_parameters);

ur_robot_driver/test/integration_test_force_mode.py

@@ -445,3 +445,80 @@ def test_deactivating_controller_stops_force_mode(self, tf_prefix):
         self.assertAlmostEqual(
             trans_before_wait.transform.translation.z, trans_after_wait.transform.translation.z
         )
+
+    def test_params_out_of_range_fails(self, tf_prefix):
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=[
+                    "force_mode_controller",
+                ],
+                deactivate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "joint_trajectory_controller",
+                ],
+            ).ok
+        )
+        self._force_mode_controller_interface = ForceModeInterface(self.node)
+
+        # Create task frame for force mode
+        point = Point(x=0.0, y=0.0, z=0.0)
+        orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
+        task_frame_pose = Pose()
+        task_frame_pose.position = point
+        task_frame_pose.orientation = orientation
+        header = std_msgs.msg.Header(seq=1, frame_id=tf_prefix + "base")
+        header.stamp.sec = int(time.time()) + 1
+        header.stamp.nanosec = 0
+        frame_stamp = PoseStamped()
+        frame_stamp.header = header
+        frame_stamp.pose = task_frame_pose
+
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp, gain_scaling=-0.1
+        )
+        self.assertFalse(res.success)
+
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp, gain_scaling=0
+        )
+        self.assertTrue(res.success)
+        res = self._force_mode_controller_interface.stop_force_mode()
+        self.assertTrue(res.success)
+
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp, gain_scaling=2
+        )
+        self.assertTrue(res.success)
+        res = self._force_mode_controller_interface.stop_force_mode()
+        self.assertTrue(res.success)
+
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp, gain_scaling=2.1
+        )
+        self.assertFalse(res.success)
+
+        # damping factor
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp, damping_factor=-0.1
+        )
+        self.assertFalse(res.success)
+
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp, damping_factor=0
+        )
+        self.assertTrue(res.success)
+        res = self._force_mode_controller_interface.stop_force_mode()
+        self.assertTrue(res.success)
+
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp, damping_factor=1
+        )
+        self.assertTrue(res.success)
+        res = self._force_mode_controller_interface.stop_force_mode()
+        self.assertTrue(res.success)
+
+        res = self._force_mode_controller_interface.start_force_mode(
+            task_frame=frame_stamp, damping_factor=1.1
+        )
+        self.assertFalse(res.success)