Add test of force mode and add arguments in hardware_interface

ur_robot_driver/src/hardware_interface.cpp

@@ -1185,17 +1185,19 @@ bool HardwareInterface::startForceMode(ur_msgs::SetForceModeRequest& req, ur_msg
 {
   bool HardwareInterface::setForceMode(ur_msgs::SetForceModeRequest & req, ur_msgs::SetForceModeResponse & res)
   {
-    if (req.task_frame.size() != 6 || req.selection_vector.size() != 6 || req.wrench.size() != 6 ||
-        req.limits.size() != 6)
-    {
-      URCL_LOG_WARN("Size of received SetForceMode message is wrong");
-      res.success = false;
-      return false;
-    }
+    // This may need to be added back in depending on the final format of the srv file
+    // if (req.limits.size() != 6)
+    // {
+    //   URCL_LOG_WARN("Size of received SetForceMode message is wrong");
+    //   res.success = false;
+    //   return false;
+    // }
     urcl::vector6d_t task_frame;
     urcl::vector6uint32_t selection_vector;
     urcl::vector6d_t wrench;
     urcl::vector6d_t limits;
+    double damping_factor = req.damping_factor;
+    double gain_scale = req.gain_scaling;
 
     task_frame[0] = req.task_frame.pose.position.x;
     task_frame[1] = req.task_frame.pose.position.x;
@@ -1226,9 +1228,21 @@ bool HardwareInterface::startForceMode(ur_msgs::SetForceModeRequest& req, ur_msg
     limits[3] = req.limits.twist.angular.x;
     limits[4] = req.limits.twist.angular.y;
     limits[5] = req.limits.twist.angular.z;
-
     unsigned int type = req.type;
-    res.success = this->ur_driver_->startForceMode(task_frame, selection_vector, wrench, type, limits);
+    if (ur_driver_->getVersion().major < 5)
+    {
+      if (gain_scale != 0)
+      {
+        ROS_WARN("Force mode gain scaling cannot be used on CB3 robots. The specified gain scaling will be ignored.");
+      }
+      res.success =
+          this->ur_driver_->startForceMode(task_frame, selection_vector, wrench, type, limits, damping_factor);
+    }
+    else
+    {
+      res.success = this->ur_driver_->startForceMode(task_frame, selection_vector, wrench, type, limits, damping_factor,
+                                                     gain_scale);
+    }
     return res.success;
   }
 

ur_robot_driver/test/integration_test.py

@@ -15,7 +15,8 @@
 from std_srvs.srv import Trigger, TriggerRequest
 import tf
 from trajectory_msgs.msg import JointTrajectoryPoint
-from ur_msgs.srv import SetIO, SetIORequest
+from geometry_msgs.msg import Pose, PoseStamped, Quaternion, Point, WrenchStamped, TwistStamped, Wrench, Twist, Vector3
+from ur_msgs.srv import SetIO, SetIORequest, SetForceModeRequest, SetForceMode
 from ur_msgs.msg import IOStates
 
 from cartesian_control_msgs.msg import (
@@ -119,6 +120,20 @@ def init_robot(self):
                 "Could not reach resend_robot_program service. Make sure that the driver is "
                 "actually running in headless mode."
                 " Msg: {}".format(err))
+
+        self.start_force_mode_srv = rospy.ServiceProxy('/ur_hardware_interface/start_force_mode', SetForceMode)
+        try:
+            self.start_force_mode_srv.wait_for_service(timeout)
+        except rospy.exceptions.ROSException as err:
+            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)
+        try:
+            self.end_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))
 
         self.script_publisher = rospy.Publisher("/ur_hardware_interface/script_command", std_msgs.msg.String, queue_size=1)
         self.tf_listener = tf.TransformListener()
@@ -135,6 +150,40 @@ def set_robot_to_mode(self, target_mode):
         self.set_mode_client.wait_for_result()
         return self.set_mode_client.get_result().success
 
+    def test_force_mode_srv(self):
+        point = Point(0.1, 0.1, 0.1)
+        orientation = Quaternion(0, 0, 0, 0)
+        task_frame_pose = Pose(point, orientation)
+        header = std_msgs.msg.Header(seq=1, frame_id="robot")
+        header.stamp.secs = int(time.time())+1
+        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)
+        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.type = type_spec
+        req.limits = limits_stamp
+        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())
+        self.assertEqual(res.success, True)
+
 
     def test_joint_trajectory_position_interface(self):
         """Test robot movement"""