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 a7eeeb3e..a2db3e17 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h @@ -56,6 +56,7 @@ #include #include #include +#include #include #include @@ -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 ur_driver_; std::unique_ptr 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_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 411d54ae..7b41bfbc 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/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; diff --git a/ur_robot_driver/test/integration_test.py b/ur_robot_driver/test/integration_test.py index 34ef1516..5fa4f584 100755 --- a/ur_robot_driver/test/integration_test.py +++ b/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"""