Force mode arguments

ur_robot_driver/include/ur_robot_driver/hardware_interface.h

@@ -56,6 +56,7 @@
 #include <ur_msgs/SetSpeedSliderFraction.h>
 #include <ur_msgs/SetPayload.h>
 #include <ur_msgs/GetRobotSoftwareVersion.h>
+#include <ur_msgs/SetForceMode.h>
 
 #include <cartesian_interface/cartesian_command_interface.h>
 #include <cartesian_interface/cartesian_state_handle.h>
@@ -220,6 +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 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_;
@@ -245,6 +248,8 @@ class HardwareInterface : public hardware_interface::RobotHW
   ros::ServiceServer set_payload_srv_;
   ros::ServiceServer activate_spline_interpolation_srv_;
   ros::ServiceServer get_robot_software_version_srv;
+  ros::ServiceServer set_force_mode_srv_;
+  ros::ServiceServer disable_force_mode_srv_;
 
   hardware_interface::JointStateInterface js_interface_;
   scaled_controllers::ScaledPositionJointInterface spj_interface_;

ur_robot_driver/src/hardware_interface.cpp

@@ -456,6 +456,12 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   // Setup the mounted payload through a ROS service
   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("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("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.
   activate_spline_interpolation_srv_ = robot_hw_nh.advertiseService(
@@ -1201,6 +1207,83 @@ bool HardwareInterface::getRobotSoftwareVersion(ur_msgs::GetRobotSoftwareVersion
   return true;
 }
 
+bool HardwareInterface::setForceMode(ur_msgs::SetForceModeRequest& req, ur_msgs::SetForceModeResponse& res)
+{
+  // 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;
+  task_frame[2] = req.task_frame.pose.position.x;
+  KDL::Rotation rot = KDL::Rotation::Quaternion(req.task_frame.pose.orientation.x, req.task_frame.pose.orientation.y,
+                                                req.task_frame.pose.orientation.z, req.task_frame.pose.orientation.w);
+  task_frame[3] = rot.GetRot().x();
+  task_frame[4] = rot.GetRot().y();
+  task_frame[5] = rot.GetRot().z();
+
+  selection_vector[0] = req.selection_vector_x;
+  selection_vector[1] = req.selection_vector_y;
+  selection_vector[2] = req.selection_vector_z;
+  selection_vector[3] = req.selection_vector_rx;
+  selection_vector[4] = req.selection_vector_ry;
+  selection_vector[5] = req.selection_vector_rz;
+
+  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)
+    {
+      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;
+}
+
+bool HardwareInterface::disableForceMode(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res)
+{
+  res.success = this->ur_driver_->endForceMode();
+  return res.success;
+}
+
 void HardwareInterface::commandCallback(const std_msgs::StringConstPtr& msg)
 {
   std::string str = msg->data;

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, GetRobotSoftwareVersion
+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
 
 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.stop_force_mode_srv = rospy.ServiceProxy('/ur_hardware_interface/stop_force_mode', Trigger)
+        try:
+            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))
 
         self.get_robot_software_version = rospy.ServiceProxy("ur_hardware_interface/get_robot_software_version", GetRobotSoftwareVersion)
         try:
@@ -143,6 +158,41 @@ 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):
+        req = SetForceModeRequest()
+        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 = 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.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
+        req.type = type_spec
+        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.stop_force_mode_srv.call(TriggerRequest())
+        self.assertEqual(res.success, True)
+
 
     def test_joint_trajectory_position_interface(self):
         """Test robot movement"""