Updated force mode functions

to match updated service definition and to match the force mode interface of ROS2.

Author: URJala

Diff for: ur_robot_driver/include/ur_robot_driver/hardware_interface.h

@@ -221,8 +221,8 @@ class HardwareInterface : public hardware_interface::RobotHW
   bool activateSplineInterpolation(std_srvs::SetBoolRequest& req, std_srvs::SetBoolResponse& res);
   bool getRobotSoftwareVersion(ur_msgs::GetRobotSoftwareVersionRequest& req,
                                ur_msgs::GetRobotSoftwareVersionResponse& res);
-  bool startForceMode(ur_msgs::SetForceModeRequest& req, ur_msgs::SetForceModeResponse& res);
-  bool endForceMode(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
+  bool setForceMode(ur_msgs::SetForceModeRequest& req, ur_msgs::SetForceModeResponse& res);
+  bool disableForceMode(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
 
   std::unique_ptr<urcl::UrDriver> ur_driver_;
   std::unique_ptr<DashboardClientROS> dashboard_client_;
@@ -249,7 +249,7 @@ class HardwareInterface : public hardware_interface::RobotHW
   ros::ServiceServer activate_spline_interpolation_srv_;
   ros::ServiceServer get_robot_software_version_srv;
   ros::ServiceServer set_force_mode_srv_;
-  ros::ServiceServer end_force_mode_srv_;
+  ros::ServiceServer disable_force_mode_srv_;
 
   hardware_interface::JointStateInterface js_interface_;
   scaled_controllers::ScaledPositionJointInterface spj_interface_;

Diff for: ur_robot_driver/src/hardware_interface.cpp

@@ -457,11 +457,10 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   set_payload_srv_ = robot_hw_nh.advertiseService("set_payload", &HardwareInterface::setPayload, this);
 
   // Calling this service will set the robot in force mode
-  set_force_mode_srv_ = robot_hw_nh.advertiseService("set_force_mode", &HardwareInterface::setForceMode, this);
+  set_force_mode_srv_ = robot_hw_nh.advertiseService("start_force_mode", &HardwareInterface::setForceMode, this);
 
   // Calling this service will stop the robot from being in force mode
-  disable_force_mode_srv_ =
-      robot_hw_nh.advertiseService("disable_force_mode", &HardwareInterface::disableForceMode, this);
+  disable_force_mode_srv_ = robot_hw_nh.advertiseService("stop_force_mode", &HardwareInterface::disableForceMode, this);
 
   // Call this to activate or deactivate using spline interpolation locally on the UR controller, when forwarding
   // trajectories to the UR robot.
@@ -1240,20 +1239,29 @@ bool HardwareInterface::setForceMode(ur_msgs::SetForceModeRequest& req, ur_msgs:
   selection_vector[4] = req.selection_vector_ry;
   selection_vector[5] = req.selection_vector_rz;
 
-  wrench[0] = req.wrench.wrench.force.x;
-  wrench[1] = req.wrench.wrench.force.y;
-  wrench[2] = req.wrench.wrench.force.z;
-  wrench[3] = req.wrench.wrench.torque.x;
-  wrench[4] = req.wrench.wrench.torque.y;
-  wrench[5] = req.wrench.wrench.torque.z;
-
-  limits[0] = req.limits.twist.linear.x;
-  limits[1] = req.limits.twist.linear.y;
-  limits[2] = req.limits.twist.linear.z;
-  limits[3] = req.limits.twist.angular.x;
-  limits[4] = req.limits.twist.angular.y;
-  limits[5] = req.limits.twist.angular.z;
+  wrench[0] = req.wrench.force.x;
+  wrench[1] = req.wrench.force.y;
+  wrench[2] = req.wrench.force.z;
+  wrench[3] = req.wrench.torque.x;
+  wrench[4] = req.wrench.torque.y;
+  wrench[5] = req.wrench.torque.z;
+
+  limits[0] = req.selection_vector_x ? req.speed_limits.linear.x : req.deviation_limits[0];
+  limits[1] = req.selection_vector_y ? req.speed_limits.linear.y : req.deviation_limits[1];
+  limits[2] = req.selection_vector_z ? req.speed_limits.linear.z : req.deviation_limits[2];
+  limits[3] = req.selection_vector_rx ? req.speed_limits.angular.x : req.deviation_limits[3];
+  limits[4] = req.selection_vector_ry ? req.speed_limits.angular.y : req.deviation_limits[4];
+  limits[5] = req.selection_vector_rz ? req.speed_limits.angular.z : req.deviation_limits[5];
+
+  if (req.type < 1 || req.type > 3)
+  {
+    ROS_ERROR("The force mode type has to be 1, 2, or 3. Received %u", req.type);
+    res.success = false;
+    return false;
+  }
+
   unsigned int type = req.type;
+
   if (ur_driver_->getVersion().major < 5)
   {
     if (gain_scale != 0)

Diff for: ur_robot_driver/test/integration_test.py

@@ -15,7 +15,7 @@
 from std_srvs.srv import Trigger, TriggerRequest
 import tf
 from trajectory_msgs.msg import JointTrajectoryPoint
-from geometry_msgs.msg import Pose, PoseStamped, Quaternion, Point, WrenchStamped, TwistStamped, Wrench, Twist, Vector3
+from geometry_msgs.msg import Pose, PoseStamped, Quaternion, Point, Wrench, Twist, Vector3
 from ur_msgs.srv import SetIO, SetIORequest, GetRobotSoftwareVersion, SetForceModeRequest, SetForceMode
 from ur_msgs.msg import IOStates
 
@@ -128,9 +128,9 @@ def init_robot(self):
             self.fail("Could not reach start force mode service. Make sure that the driver is actually running."
                       " Msg: {}".format(err))
 
-        self.end_force_mode_srv = rospy.ServiceProxy('/ur_hardware_interface/end_force_mode', Trigger)
+        self.stop_force_mode_srv = rospy.ServiceProxy('/ur_hardware_interface/stop_force_mode', Trigger)
         try:
-            self.end_force_mode_srv.wait_for_service(timeout)
+            self.stop_force_mode_srv.wait_for_service(timeout)
         except rospy.exceptions.ROSException as err:
             self.fail("Could not reach end force mode service. Make sure that the driver is actually running."
                       " Msg: {}".format(err))
@@ -159,6 +159,7 @@ def set_robot_to_mode(self, target_mode):
         return self.set_mode_client.get_result().success
 
     def test_force_mode_srv(self):
+        req = SetForceModeRequest()
         point = Point(0.1, 0.1, 0.1)
         orientation = Quaternion(0, 0, 0, 0)
         task_frame_pose = Pose(point, orientation)
@@ -167,29 +168,29 @@ def test_force_mode_srv(self):
         header.stamp.nsecs = 0
         frame_stamp = PoseStamped(header, task_frame_pose)
         compliance = [0,0,1,0,0,1]
-        wrench_vec = Wrench(Vector3(0,0,1),Vector3(0,0,1))
-        wrench_stamp = WrenchStamped(header,wrench_vec)
-        type_spec = 2
-        limits = Twist(Vector3(0.1,0.1,0.1), Vector3(0.1,0.1,0.1))
-        limits_stamp = TwistStamped(header, limits)
+        wrench = Wrench(Vector3(0,0,1),Vector3(0,0,1))
+        type_spec = req.NO_TRANSFORM
+        speed_limits = Twist(Vector3(0.1,0.1,0.1), Vector3(0.1,0.1,0.1))
+        deviation_limits  = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
         damping_factor = 0.8
         gain_scale = 0.8
-        req = SetForceModeRequest()
+
         req.task_frame = frame_stamp
         req.selection_vector_x = compliance[0]
         req.selection_vector_y = compliance[1]
         req.selection_vector_z = compliance[2]
         req.selection_vector_rx = compliance[3]
         req.selection_vector_ry = compliance[4]
         req.selection_vector_rz = compliance[5]  
-        req.wrench = wrench_stamp
+        req.wrench = wrench
         req.type = type_spec
-        req.limits = limits_stamp
+        req.speed_limits = speed_limits
+        req.deviation_limits = deviation_limits
         req.damping_factor = damping_factor
         req.gain_scaling = gain_scale
         res = self.start_force_mode_srv.call(req)
         self.assertEqual(res.success, True)
-        res = self.end_force_mode_srv.call(TriggerRequest())
+        res = self.stop_force_mode_srv.call(TriggerRequest())
         self.assertEqual(res.success, True)