From 72acfbd63dc6fc826dd646f96a84e9c21bfefe89 Mon Sep 17 00:00:00 2001 From: urmarp Date: Mon, 9 Jan 2023 09:06:48 +0100 Subject: [PATCH 1/5] Added service to set force mode through ROS --- .../ur_robot_driver/hardware_interface.h | 3 ++ ur_robot_driver/src/hardware_interface.cpp | 28 +++++++++++++++++++ 2 files changed, 31 insertions(+) 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..1f240d00 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,7 @@ 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); std::unique_ptr ur_driver_; std::unique_ptr dashboard_client_; @@ -245,6 +247,7 @@ 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_; 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..0cdb33fd 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -456,6 +456,9 @@ 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("set_force_mode", &HardwareInterface::setForceMode, 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 +1204,31 @@ bool HardwareInterface::getRobotSoftwareVersion(ur_msgs::GetRobotSoftwareVersion return true; } +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; + } + urcl::vector6d_t task_frame; + urcl::vector6uint32_t selection_vector; + urcl::vector6d_t wrench; + urcl::vector6d_t limits; + for (size_t i = 0; i < 6; i++) + { + task_frame[i] = req.task_frame[i]; + selection_vector[i] = req.selection_vector[i]; + wrench[i] = req.wrench[i]; + limits[i] = req.limits[i]; + } + unsigned int type = req.type; + res.success = this->ur_driver_->startForceMode(task_frame, selection_vector, wrench, type, limits); + return res.success; +} + void HardwareInterface::commandCallback(const std_msgs::StringConstPtr& msg) { std::string str = msg->data; From a0bb9322d10ed4f13b3f3d6c70c67f1014f82a4d Mon Sep 17 00:00:00 2001 From: urmarp Date: Mon, 30 Jan 2023 17:24:19 +0100 Subject: [PATCH 2/5] Changed setForce mode to startForceMode and endForceMode. Also made changes to account for changes in msg in PR (https://github.com/ros-industrial/ur_msgs/pull/20) --- .../ur_robot_driver/hardware_interface.h | 6 ++- ur_robot_driver/src/hardware_interface.cpp | 49 ++++++++++++++++--- 2 files changed, 45 insertions(+), 10 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 1f240d00..1cdd0790 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h @@ -221,7 +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 startForceMode(ur_msgs::SetForceModeRequest& req, ur_msgs::SetForceModeResponse& res); + bool endForceMode(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res); std::unique_ptr ur_driver_; std::unique_ptr dashboard_client_; @@ -247,7 +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 start_force_mode_srv_; + ros::ServiceServer end_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 0cdb33fd..4a5c54a5 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -457,7 +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); + start_force_mode_srv_ = robot_hw_nh.advertiseService("start_force_mode", &HardwareInterface::startForceMode, this); + + // Calling this service will stop the robot from being in force mode + end_force_mode_srv_ = robot_hw_nh.advertiseService("end_force_mode", &HardwareInterface::endForceMode, this); // Call this to activate or deactivate using spline interpolation locally on the UR controller, when forwarding // trajectories to the UR robot. @@ -1217,18 +1220,48 @@ bool HardwareInterface::setForceMode(ur_msgs::SetForceModeRequest& req, ur_msgs: urcl::vector6uint32_t selection_vector; urcl::vector6d_t wrench; urcl::vector6d_t limits; - for (size_t i = 0; i < 6; i++) - { - task_frame[i] = req.task_frame[i]; - selection_vector[i] = req.selection_vector[i]; - wrench[i] = req.wrench[i]; - limits[i] = req.limits[i]; - } + + 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.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; + unsigned int type = req.type; res.success = this->ur_driver_->startForceMode(task_frame, selection_vector, wrench, type, limits); return res.success; } +bool HardwareInterface::endForceMode(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; From 34181303a5704b794b8d619a7f566bb89b8d53e2 Mon Sep 17 00:00:00 2001 From: URJala Date: Tue, 16 Jul 2024 14:38:45 +0000 Subject: [PATCH 3/5] Add test of force mode and add arguments in hardware_interface --- ur_robot_driver/src/hardware_interface.cpp | 478 +++++++++++---------- ur_robot_driver/test/integration_test.py | 51 ++- 2 files changed, 292 insertions(+), 237 deletions(-) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 4a5c54a5..96f45919 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -1199,296 +1199,302 @@ bool HardwareInterface::setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::Set bool HardwareInterface::getRobotSoftwareVersion(ur_msgs::GetRobotSoftwareVersionRequest& req, ur_msgs::GetRobotSoftwareVersionResponse& res) { - urcl::VersionInformation version_info = this->ur_driver_->getVersion(); - res.major = version_info.major; - res.minor = version_info.minor; - res.bugfix = version_info.bugfix; - res.build = version_info.build; - return true; -} - -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; + 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.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; + 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; } - urcl::vector6d_t task_frame; - urcl::vector6uint32_t selection_vector; - urcl::vector6d_t wrench; - urcl::vector6d_t limits; - - 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.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; - - unsigned int type = req.type; - res.success = this->ur_driver_->startForceMode(task_frame, selection_vector, wrench, type, limits); - return res.success; -} - -bool HardwareInterface::endForceMode(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; - if (str.back() != '\n') + bool HardwareInterface::endForceMode(std_srvs::TriggerRequest & req, std_srvs::TriggerResponse & res) { - str.append("\n"); + res.success = this->ur_driver_->endForceMode(); + return res.success; } - if (ur_driver_ == nullptr) + void HardwareInterface::commandCallback(const std_msgs::StringConstPtr& msg) { - throw std::runtime_error("Trying to use the ur_driver_ member before it is initialized. This should not happen, " - "please contact the package maintainer."); - } + std::string str = msg->data; + if (str.back() != '\n') + { + str.append("\n"); + } - if (ur_driver_->sendScript(str)) - { - ROS_DEBUG_STREAM("Sent script to robot"); - } - else - { - ROS_ERROR_STREAM("Error sending script to robot"); - } -} + if (ur_driver_ == nullptr) + { + throw std::runtime_error("Trying to use the ur_driver_ member before it is initialized. This should not happen, " + "please contact the package maintainer."); + } -bool HardwareInterface::activateSplineInterpolation(std_srvs::SetBoolRequest& req, std_srvs::SetBoolResponse& res) -{ - use_spline_interpolation_ = req.data; - if (use_spline_interpolation_) - { - res.message = "Activated spline interpolation in forward trajectory mode."; + if (ur_driver_->sendScript(str)) + { + ROS_DEBUG_STREAM("Sent script to robot"); + } + else + { + ROS_ERROR_STREAM("Error sending script to robot"); + } } - else + + bool HardwareInterface::activateSplineInterpolation(std_srvs::SetBoolRequest & req, std_srvs::SetBoolResponse & res) { - res.message = "Deactivated spline interpolation in forward trajectory mode."; + use_spline_interpolation_ = req.data; + if (use_spline_interpolation_) + { + res.message = "Activated spline interpolation in forward trajectory mode."; + } + else + { + res.message = "Deactivated spline interpolation in forward trajectory mode."; + } + res.success = true; + return true; } - res.success = true; - return true; -} -void HardwareInterface::publishRobotAndSafetyMode() -{ - if (robot_mode_pub_) + void HardwareInterface::publishRobotAndSafetyMode() { - if (robot_mode_pub_->msg_.mode != robot_mode_) + if (robot_mode_pub_) { - if (robot_mode_pub_->trylock()) + if (robot_mode_pub_->msg_.mode != robot_mode_) { - robot_mode_pub_->msg_.mode = robot_mode_; - robot_mode_pub_->unlockAndPublish(); + if (robot_mode_pub_->trylock()) + { + robot_mode_pub_->msg_.mode = robot_mode_; + robot_mode_pub_->unlockAndPublish(); + } } } - } - if (safety_mode_pub_) - { - if (safety_mode_pub_->msg_.mode != safety_mode_) + if (safety_mode_pub_) { - if (safety_mode_pub_->trylock()) + if (safety_mode_pub_->msg_.mode != safety_mode_) { - safety_mode_pub_->msg_.mode = safety_mode_; - safety_mode_pub_->unlockAndPublish(); + if (safety_mode_pub_->trylock()) + { + safety_mode_pub_->msg_.mode = safety_mode_; + safety_mode_pub_->unlockAndPublish(); + } } } } -} -bool HardwareInterface::checkControllerClaims(const std::set& claimed_resources) -{ - for (const std::string& it : joint_names_) + bool HardwareInterface::checkControllerClaims(const std::set& claimed_resources) { + for (const std::string& it : joint_names_) + { + for (const std::string& jt : claimed_resources) + { + if (it == jt) + { + return true; + } + } + } for (const std::string& jt : claimed_resources) { - if (it == jt) + if ("tool0_controller" == jt) { return true; } } + return false; } - for (const std::string& jt : claimed_resources) + + void HardwareInterface::startJointInterpolation(const hardware_interface::JointTrajectory& trajectory) { - if ("tool0_controller" == jt) + size_t point_number = trajectory.trajectory.points.size(); + ROS_DEBUG("Starting joint-based trajectory forward"); + ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, point_number); + double last_time = 0.0; + for (size_t i = 0; i < point_number; i++) { - return true; + trajectory_msgs::JointTrajectoryPoint point = trajectory.trajectory.points[i]; + urcl::vector6d_t p; + p[0] = point.positions[0]; + p[1] = point.positions[1]; + p[2] = point.positions[2]; + p[3] = point.positions[3]; + p[4] = point.positions[4]; + p[5] = point.positions[5]; + double next_time = point.time_from_start.toSec(); + if (!use_spline_interpolation_) + { + ur_driver_->writeTrajectoryPoint(p, false, next_time - last_time); + } + else // Use spline interpolation + { + if (point.velocities.size() == 6 && point.accelerations.size() == 6) + { + urcl::vector6d_t v, a; + v[0] = point.velocities[0]; + v[1] = point.velocities[1]; + v[2] = point.velocities[2]; + v[3] = point.velocities[3]; + v[4] = point.velocities[4]; + v[5] = point.velocities[5]; + + a[0] = point.accelerations[0]; + a[1] = point.accelerations[1]; + a[2] = point.accelerations[2]; + a[3] = point.accelerations[3]; + a[4] = point.accelerations[4]; + a[5] = point.accelerations[5]; + ur_driver_->writeTrajectorySplinePoint(p, v, a, next_time - last_time); + } + else if (point.velocities.size() == 6) + { + urcl::vector6d_t v; + v[0] = point.velocities[0]; + v[1] = point.velocities[1]; + v[2] = point.velocities[2]; + v[3] = point.velocities[3]; + v[4] = point.velocities[4]; + v[5] = point.velocities[5]; + ur_driver_->writeTrajectorySplinePoint(p, v, next_time - last_time); + } + else + { + ROS_ERROR_THROTTLE(1, "Spline interpolation using positions only is not supported."); + } + } + last_time = next_time; } + ROS_DEBUG("Finished Sending Trajectory"); } - return false; -} -void HardwareInterface::startJointInterpolation(const hardware_interface::JointTrajectory& trajectory) -{ - size_t point_number = trajectory.trajectory.points.size(); - ROS_DEBUG("Starting joint-based trajectory forward"); - ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, point_number); - double last_time = 0.0; - for (size_t i = 0; i < point_number; i++) - { - trajectory_msgs::JointTrajectoryPoint point = trajectory.trajectory.points[i]; - urcl::vector6d_t p; - p[0] = point.positions[0]; - p[1] = point.positions[1]; - p[2] = point.positions[2]; - p[3] = point.positions[3]; - p[4] = point.positions[4]; - p[5] = point.positions[5]; - double next_time = point.time_from_start.toSec(); - if (!use_spline_interpolation_) + void HardwareInterface::startCartesianInterpolation(const hardware_interface::CartesianTrajectory& trajectory) + { + size_t point_number = trajectory.trajectory.points.size(); + ROS_DEBUG("Starting cartesian trajectory forward"); + ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, point_number); + double last_time = 0.0; + for (size_t i = 0; i < point_number; i++) { - ur_driver_->writeTrajectoryPoint(p, false, next_time - last_time); + cartesian_control_msgs::CartesianTrajectoryPoint point = trajectory.trajectory.points[i]; + urcl::vector6d_t p; + p[0] = point.pose.position.x; + p[1] = point.pose.position.y; + p[2] = point.pose.position.z; + + KDL::Rotation rot = KDL::Rotation::Quaternion(point.pose.orientation.x, point.pose.orientation.y, + point.pose.orientation.z, point.pose.orientation.w); + + // UR robots use axis angle representation. + p[3] = rot.GetRot().x(); + p[4] = rot.GetRot().y(); + p[5] = rot.GetRot().z(); + double next_time = point.time_from_start.toSec(); + ur_driver_->writeTrajectoryPoint(p, true, next_time - last_time); + last_time = next_time; } - else // Use spline interpolation + ROS_DEBUG("Finished Sending Trajectory"); + } + + void HardwareInterface::cancelInterpolation() + { + ROS_DEBUG("Cancelling Trajectory"); + ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL); + } + + void HardwareInterface::passthroughTrajectoryDoneCb(urcl::control::TrajectoryResult result) + { + hardware_interface::ExecutionState final_state; + switch (result) { - if (point.velocities.size() == 6 && point.accelerations.size() == 6) + case urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS: + { + final_state = hardware_interface::ExecutionState::SUCCESS; + ROS_INFO_STREAM("Forwarded trajectory finished successful."); + break; + } + case urcl::control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED: { - urcl::vector6d_t v, a; - v[0] = point.velocities[0]; - v[1] = point.velocities[1]; - v[2] = point.velocities[2]; - v[3] = point.velocities[3]; - v[4] = point.velocities[4]; - v[5] = point.velocities[5]; - - a[0] = point.accelerations[0]; - a[1] = point.accelerations[1]; - a[2] = point.accelerations[2]; - a[3] = point.accelerations[3]; - a[4] = point.accelerations[4]; - a[5] = point.accelerations[5]; - ur_driver_->writeTrajectorySplinePoint(p, v, a, next_time - last_time); + final_state = hardware_interface::ExecutionState::PREEMPTED; + ROS_INFO_STREAM("Forwarded trajectory execution preempted by user."); + break; } - else if (point.velocities.size() == 6) + case urcl::control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE: { - urcl::vector6d_t v; - v[0] = point.velocities[0]; - v[1] = point.velocities[1]; - v[2] = point.velocities[2]; - v[3] = point.velocities[3]; - v[4] = point.velocities[4]; - v[5] = point.velocities[5]; - ur_driver_->writeTrajectorySplinePoint(p, v, next_time - last_time); + final_state = hardware_interface::ExecutionState::ABORTED; + ROS_INFO_STREAM("Forwarded trajectory execution failed."); + break; } - else + default: { - ROS_ERROR_THROTTLE(1, "Spline interpolation using positions only is not supported."); + std::stringstream ss; + ss << "Unknown trajectory result: " << urcl::toUnderlying(result); + throw(std::invalid_argument(ss.str())); } } - last_time = next_time; - } - ROS_DEBUG("Finished Sending Trajectory"); -} - -void HardwareInterface::startCartesianInterpolation(const hardware_interface::CartesianTrajectory& trajectory) -{ - size_t point_number = trajectory.trajectory.points.size(); - ROS_DEBUG("Starting cartesian trajectory forward"); - ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, point_number); - double last_time = 0.0; - for (size_t i = 0; i < point_number; i++) - { - cartesian_control_msgs::CartesianTrajectoryPoint point = trajectory.trajectory.points[i]; - urcl::vector6d_t p; - p[0] = point.pose.position.x; - p[1] = point.pose.position.y; - p[2] = point.pose.position.z; - - KDL::Rotation rot = KDL::Rotation::Quaternion(point.pose.orientation.x, point.pose.orientation.y, - point.pose.orientation.z, point.pose.orientation.w); - - // UR robots use axis angle representation. - p[3] = rot.GetRot().x(); - p[4] = rot.GetRot().y(); - p[5] = rot.GetRot().z(); - double next_time = point.time_from_start.toSec(); - ur_driver_->writeTrajectoryPoint(p, true, next_time - last_time); - last_time = next_time; - } - ROS_DEBUG("Finished Sending Trajectory"); -} - -void HardwareInterface::cancelInterpolation() -{ - ROS_DEBUG("Cancelling Trajectory"); - ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL); -} -void HardwareInterface::passthroughTrajectoryDoneCb(urcl::control::TrajectoryResult result) -{ - hardware_interface::ExecutionState final_state; - switch (result) - { - case urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS: - { - final_state = hardware_interface::ExecutionState::SUCCESS; - ROS_INFO_STREAM("Forwarded trajectory finished successful."); - break; - } - case urcl::control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED: + if (joint_forward_controller_running_) { - final_state = hardware_interface::ExecutionState::PREEMPTED; - ROS_INFO_STREAM("Forwarded trajectory execution preempted by user."); - break; + jnt_traj_interface_.setDone(final_state); } - case urcl::control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE: + else if (cartesian_forward_controller_running_) { - final_state = hardware_interface::ExecutionState::ABORTED; - ROS_INFO_STREAM("Forwarded trajectory execution failed."); - break; + cart_traj_interface_.setDone(final_state); } - default: + else { - std::stringstream ss; - ss << "Unknown trajectory result: " << urcl::toUnderlying(result); - throw(std::invalid_argument(ss.str())); + ROS_ERROR_STREAM("Received forwarded trajectory result with no forwarding controller running."); } } - - if (joint_forward_controller_running_) - { - jnt_traj_interface_.setDone(final_state); - } - else if (cartesian_forward_controller_running_) - { - cart_traj_interface_.setDone(final_state); - } - else - { - ROS_ERROR_STREAM("Received forwarded trajectory result with no forwarding controller running."); - } -} } // namespace ur_driver PLUGINLIB_EXPORT_CLASS(ur_driver::HardwareInterface, hardware_interface::RobotHW) diff --git a/ur_robot_driver/test/integration_test.py b/ur_robot_driver/test/integration_test.py index 34ef1516..c6294038 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, WrenchStamped, TwistStamped, 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.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.get_robot_software_version = rospy.ServiceProxy("ur_hardware_interface/get_robot_software_version", GetRobotSoftwareVersion) try: @@ -143,6 +158,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""" From 6e924db90b7919a8ac8b83fdd3eeb6eb84336423 Mon Sep 17 00:00:00 2001 From: URJala Date: Wed, 8 Jan 2025 12:33:11 +0000 Subject: [PATCH 4/5] Service name parity between Ros1 and 2 --- .../ur_robot_driver/hardware_interface.h | 2 +- ur_robot_driver/src/hardware_interface.cpp | 492 +++++++++--------- 2 files changed, 251 insertions(+), 243 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 1cdd0790..ba2366ad 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h @@ -248,7 +248,7 @@ 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 start_force_mode_srv_; + ros::ServiceServer set_force_mode_srv_; ros::ServiceServer end_force_mode_srv_; hardware_interface::JointStateInterface js_interface_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 96f45919..23b0b425 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -457,10 +457,11 @@ 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 - start_force_mode_srv_ = robot_hw_nh.advertiseService("start_force_mode", &HardwareInterface::startForceMode, this); + set_force_mode_srv_ = robot_hw_nh.advertiseService("set_force_mode", &HardwareInterface::setForceMode, this); // Calling this service will stop the robot from being in force mode - end_force_mode_srv_ = robot_hw_nh.advertiseService("end_force_mode", &HardwareInterface::endForceMode, this); + disable_force_mode_srv_ = + robot_hw_nh.advertiseService("disable_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. @@ -1199,302 +1200,309 @@ bool HardwareInterface::setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::Set bool HardwareInterface::getRobotSoftwareVersion(ur_msgs::GetRobotSoftwareVersionRequest& req, ur_msgs::GetRobotSoftwareVersionResponse& res) { - 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.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; - 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 + urcl::VersionInformation version_info = this->ur_driver_->getVersion(); + res.major = version_info.major; + res.minor = version_info.minor; + res.bugfix = version_info.bugfix; + res.build = version_info.build; + 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.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; + unsigned int type = req.type; + if (ur_driver_->getVersion().major < 5) + { + if (gain_scale != 0) { - res.success = this->ur_driver_->startForceMode(task_frame, selection_vector, wrench, type, limits, damping_factor, - gain_scale); + ROS_WARN("Force mode gain scaling cannot be used on CB3 robots. The specified gain scaling will be ignored."); } - return res.success; + res.success = this->ur_driver_->startForceMode(task_frame, selection_vector, wrench, type, limits, damping_factor); } - - bool HardwareInterface::endForceMode(std_srvs::TriggerRequest & req, std_srvs::TriggerResponse & res) + else { - res.success = this->ur_driver_->endForceMode(); - return res.success; + res.success = this->ur_driver_->startForceMode(task_frame, selection_vector, wrench, type, limits, damping_factor, + gain_scale); } + return res.success; +} - void HardwareInterface::commandCallback(const std_msgs::StringConstPtr& msg) +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; + if (str.back() != '\n') { - std::string str = msg->data; - if (str.back() != '\n') - { - str.append("\n"); - } + str.append("\n"); + } - if (ur_driver_ == nullptr) - { - throw std::runtime_error("Trying to use the ur_driver_ member before it is initialized. This should not happen, " - "please contact the package maintainer."); - } + if (ur_driver_ == nullptr) + { + throw std::runtime_error("Trying to use the ur_driver_ member before it is initialized. This should not happen, " + "please contact the package maintainer."); + } - if (ur_driver_->sendScript(str)) - { - ROS_DEBUG_STREAM("Sent script to robot"); - } - else - { - ROS_ERROR_STREAM("Error sending script to robot"); - } + if (ur_driver_->sendScript(str)) + { + ROS_DEBUG_STREAM("Sent script to robot"); } + else + { + ROS_ERROR_STREAM("Error sending script to robot"); + } +} - bool HardwareInterface::activateSplineInterpolation(std_srvs::SetBoolRequest & req, std_srvs::SetBoolResponse & res) +bool HardwareInterface::activateSplineInterpolation(std_srvs::SetBoolRequest& req, std_srvs::SetBoolResponse& res) +{ + use_spline_interpolation_ = req.data; + if (use_spline_interpolation_) { - use_spline_interpolation_ = req.data; - if (use_spline_interpolation_) - { - res.message = "Activated spline interpolation in forward trajectory mode."; - } - else - { - res.message = "Deactivated spline interpolation in forward trajectory mode."; - } - res.success = true; - return true; + res.message = "Activated spline interpolation in forward trajectory mode."; + } + else + { + res.message = "Deactivated spline interpolation in forward trajectory mode."; } + res.success = true; + return true; +} - void HardwareInterface::publishRobotAndSafetyMode() +void HardwareInterface::publishRobotAndSafetyMode() +{ + if (robot_mode_pub_) { - if (robot_mode_pub_) + if (robot_mode_pub_->msg_.mode != robot_mode_) { - if (robot_mode_pub_->msg_.mode != robot_mode_) + if (robot_mode_pub_->trylock()) { - if (robot_mode_pub_->trylock()) - { - robot_mode_pub_->msg_.mode = robot_mode_; - robot_mode_pub_->unlockAndPublish(); - } + robot_mode_pub_->msg_.mode = robot_mode_; + robot_mode_pub_->unlockAndPublish(); } } - if (safety_mode_pub_) + } + if (safety_mode_pub_) + { + if (safety_mode_pub_->msg_.mode != safety_mode_) { - if (safety_mode_pub_->msg_.mode != safety_mode_) + if (safety_mode_pub_->trylock()) { - if (safety_mode_pub_->trylock()) - { - safety_mode_pub_->msg_.mode = safety_mode_; - safety_mode_pub_->unlockAndPublish(); - } + safety_mode_pub_->msg_.mode = safety_mode_; + safety_mode_pub_->unlockAndPublish(); } } } +} - bool HardwareInterface::checkControllerClaims(const std::set& claimed_resources) +bool HardwareInterface::checkControllerClaims(const std::set& claimed_resources) +{ + for (const std::string& it : joint_names_) { - for (const std::string& it : joint_names_) - { - for (const std::string& jt : claimed_resources) - { - if (it == jt) - { - return true; - } - } - } for (const std::string& jt : claimed_resources) { - if ("tool0_controller" == jt) + if (it == jt) { return true; } } - return false; } - - void HardwareInterface::startJointInterpolation(const hardware_interface::JointTrajectory& trajectory) + for (const std::string& jt : claimed_resources) { - size_t point_number = trajectory.trajectory.points.size(); - ROS_DEBUG("Starting joint-based trajectory forward"); - ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, point_number); - double last_time = 0.0; - for (size_t i = 0; i < point_number; i++) + if ("tool0_controller" == jt) { - trajectory_msgs::JointTrajectoryPoint point = trajectory.trajectory.points[i]; - urcl::vector6d_t p; - p[0] = point.positions[0]; - p[1] = point.positions[1]; - p[2] = point.positions[2]; - p[3] = point.positions[3]; - p[4] = point.positions[4]; - p[5] = point.positions[5]; - double next_time = point.time_from_start.toSec(); - if (!use_spline_interpolation_) - { - ur_driver_->writeTrajectoryPoint(p, false, next_time - last_time); - } - else // Use spline interpolation - { - if (point.velocities.size() == 6 && point.accelerations.size() == 6) - { - urcl::vector6d_t v, a; - v[0] = point.velocities[0]; - v[1] = point.velocities[1]; - v[2] = point.velocities[2]; - v[3] = point.velocities[3]; - v[4] = point.velocities[4]; - v[5] = point.velocities[5]; - - a[0] = point.accelerations[0]; - a[1] = point.accelerations[1]; - a[2] = point.accelerations[2]; - a[3] = point.accelerations[3]; - a[4] = point.accelerations[4]; - a[5] = point.accelerations[5]; - ur_driver_->writeTrajectorySplinePoint(p, v, a, next_time - last_time); - } - else if (point.velocities.size() == 6) - { - urcl::vector6d_t v; - v[0] = point.velocities[0]; - v[1] = point.velocities[1]; - v[2] = point.velocities[2]; - v[3] = point.velocities[3]; - v[4] = point.velocities[4]; - v[5] = point.velocities[5]; - ur_driver_->writeTrajectorySplinePoint(p, v, next_time - last_time); - } - else - { - ROS_ERROR_THROTTLE(1, "Spline interpolation using positions only is not supported."); - } - } - last_time = next_time; + return true; } - ROS_DEBUG("Finished Sending Trajectory"); } + return false; +} - void HardwareInterface::startCartesianInterpolation(const hardware_interface::CartesianTrajectory& trajectory) - { - size_t point_number = trajectory.trajectory.points.size(); - ROS_DEBUG("Starting cartesian trajectory forward"); - ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, point_number); - double last_time = 0.0; - for (size_t i = 0; i < point_number; i++) +void HardwareInterface::startJointInterpolation(const hardware_interface::JointTrajectory& trajectory) +{ + size_t point_number = trajectory.trajectory.points.size(); + ROS_DEBUG("Starting joint-based trajectory forward"); + ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, point_number); + double last_time = 0.0; + for (size_t i = 0; i < point_number; i++) + { + trajectory_msgs::JointTrajectoryPoint point = trajectory.trajectory.points[i]; + urcl::vector6d_t p; + p[0] = point.positions[0]; + p[1] = point.positions[1]; + p[2] = point.positions[2]; + p[3] = point.positions[3]; + p[4] = point.positions[4]; + p[5] = point.positions[5]; + double next_time = point.time_from_start.toSec(); + if (!use_spline_interpolation_) { - cartesian_control_msgs::CartesianTrajectoryPoint point = trajectory.trajectory.points[i]; - urcl::vector6d_t p; - p[0] = point.pose.position.x; - p[1] = point.pose.position.y; - p[2] = point.pose.position.z; - - KDL::Rotation rot = KDL::Rotation::Quaternion(point.pose.orientation.x, point.pose.orientation.y, - point.pose.orientation.z, point.pose.orientation.w); - - // UR robots use axis angle representation. - p[3] = rot.GetRot().x(); - p[4] = rot.GetRot().y(); - p[5] = rot.GetRot().z(); - double next_time = point.time_from_start.toSec(); - ur_driver_->writeTrajectoryPoint(p, true, next_time - last_time); - last_time = next_time; + ur_driver_->writeTrajectoryPoint(p, false, next_time - last_time); } - ROS_DEBUG("Finished Sending Trajectory"); - } - - void HardwareInterface::cancelInterpolation() - { - ROS_DEBUG("Cancelling Trajectory"); - ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL); - } - - void HardwareInterface::passthroughTrajectoryDoneCb(urcl::control::TrajectoryResult result) - { - hardware_interface::ExecutionState final_state; - switch (result) + else // Use spline interpolation { - case urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS: - { - final_state = hardware_interface::ExecutionState::SUCCESS; - ROS_INFO_STREAM("Forwarded trajectory finished successful."); - break; - } - case urcl::control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED: + if (point.velocities.size() == 6 && point.accelerations.size() == 6) { - final_state = hardware_interface::ExecutionState::PREEMPTED; - ROS_INFO_STREAM("Forwarded trajectory execution preempted by user."); - break; + urcl::vector6d_t v, a; + v[0] = point.velocities[0]; + v[1] = point.velocities[1]; + v[2] = point.velocities[2]; + v[3] = point.velocities[3]; + v[4] = point.velocities[4]; + v[5] = point.velocities[5]; + + a[0] = point.accelerations[0]; + a[1] = point.accelerations[1]; + a[2] = point.accelerations[2]; + a[3] = point.accelerations[3]; + a[4] = point.accelerations[4]; + a[5] = point.accelerations[5]; + ur_driver_->writeTrajectorySplinePoint(p, v, a, next_time - last_time); } - case urcl::control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE: + else if (point.velocities.size() == 6) { - final_state = hardware_interface::ExecutionState::ABORTED; - ROS_INFO_STREAM("Forwarded trajectory execution failed."); - break; + urcl::vector6d_t v; + v[0] = point.velocities[0]; + v[1] = point.velocities[1]; + v[2] = point.velocities[2]; + v[3] = point.velocities[3]; + v[4] = point.velocities[4]; + v[5] = point.velocities[5]; + ur_driver_->writeTrajectorySplinePoint(p, v, next_time - last_time); } - default: + else { - std::stringstream ss; - ss << "Unknown trajectory result: " << urcl::toUnderlying(result); - throw(std::invalid_argument(ss.str())); + ROS_ERROR_THROTTLE(1, "Spline interpolation using positions only is not supported."); } } + last_time = next_time; + } + ROS_DEBUG("Finished Sending Trajectory"); +} - if (joint_forward_controller_running_) +void HardwareInterface::startCartesianInterpolation(const hardware_interface::CartesianTrajectory& trajectory) +{ + size_t point_number = trajectory.trajectory.points.size(); + ROS_DEBUG("Starting cartesian trajectory forward"); + ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, point_number); + double last_time = 0.0; + for (size_t i = 0; i < point_number; i++) + { + cartesian_control_msgs::CartesianTrajectoryPoint point = trajectory.trajectory.points[i]; + urcl::vector6d_t p; + p[0] = point.pose.position.x; + p[1] = point.pose.position.y; + p[2] = point.pose.position.z; + + KDL::Rotation rot = KDL::Rotation::Quaternion(point.pose.orientation.x, point.pose.orientation.y, + point.pose.orientation.z, point.pose.orientation.w); + + // UR robots use axis angle representation. + p[3] = rot.GetRot().x(); + p[4] = rot.GetRot().y(); + p[5] = rot.GetRot().z(); + double next_time = point.time_from_start.toSec(); + ur_driver_->writeTrajectoryPoint(p, true, next_time - last_time); + last_time = next_time; + } + ROS_DEBUG("Finished Sending Trajectory"); +} + +void HardwareInterface::cancelInterpolation() +{ + ROS_DEBUG("Cancelling Trajectory"); + ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL); +} + +void HardwareInterface::passthroughTrajectoryDoneCb(urcl::control::TrajectoryResult result) +{ + hardware_interface::ExecutionState final_state; + switch (result) + { + case urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS: { - jnt_traj_interface_.setDone(final_state); + final_state = hardware_interface::ExecutionState::SUCCESS; + ROS_INFO_STREAM("Forwarded trajectory finished successful."); + break; } - else if (cartesian_forward_controller_running_) + case urcl::control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED: { - cart_traj_interface_.setDone(final_state); + final_state = hardware_interface::ExecutionState::PREEMPTED; + ROS_INFO_STREAM("Forwarded trajectory execution preempted by user."); + break; } - else + case urcl::control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE: + { + final_state = hardware_interface::ExecutionState::ABORTED; + ROS_INFO_STREAM("Forwarded trajectory execution failed."); + break; + } + default: { - ROS_ERROR_STREAM("Received forwarded trajectory result with no forwarding controller running."); + std::stringstream ss; + ss << "Unknown trajectory result: " << urcl::toUnderlying(result); + throw(std::invalid_argument(ss.str())); } } + + if (joint_forward_controller_running_) + { + jnt_traj_interface_.setDone(final_state); + } + else if (cartesian_forward_controller_running_) + { + cart_traj_interface_.setDone(final_state); + } + else + { + ROS_ERROR_STREAM("Received forwarded trajectory result with no forwarding controller running."); + } +} } // namespace ur_driver PLUGINLIB_EXPORT_CLASS(ur_driver::HardwareInterface, hardware_interface::RobotHW) From e6d31238221ce33d4dd8aaabe84cc6e2091a6ee3 Mon Sep 17 00:00:00 2001 From: URJala Date: Wed, 8 Jan 2025 14:22:47 +0000 Subject: [PATCH 5/5] 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 23b0b425..7b41bfbc 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)