From a87eb27996589bd7ae2f9c938d408bb44deffbce Mon Sep 17 00:00:00 2001 From: URJala Date: Wed, 8 Jan 2025 14:22:47 +0000 Subject: [PATCH] Updated force mode functions to match updated service definition and to match the force mode interface of ROS2. --- .../ur_robot_driver/hardware_interface.h | 6 +-- ur_robot_driver/src/hardware_interface.cpp | 40 +++++++++++-------- ur_robot_driver/test/integration_test.py | 25 ++++++------ 3 files changed, 40 insertions(+), 31 deletions(-) diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h index ba2366ad..a2db3e17 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h +++ b/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 ur_driver_; std::unique_ptr 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 --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index b8736e10..0747671a 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/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 --git a/ur_robot_driver/test/integration_test.py b/ur_robot_driver/test/integration_test.py index c6294038..5fa4f584 100755 --- a/ur_robot_driver/test/integration_test.py +++ b/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,14 +168,13 @@ 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] @@ -182,14 +182,15 @@ def test_force_mode_srv(self): 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)