From 5c4d56072ad529e7e486579098ad10bccbdc199f Mon Sep 17 00:00:00 2001 From: urmarp Date: Mon, 30 Jan 2023 17:24:19 +0100 Subject: [PATCH] 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 | 439 ++++++++++-------- 2 files changed, 241 insertions(+), 204 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 7182bdc4..775da5bb 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h @@ -216,7 +216,8 @@ class HardwareInterface : public hardware_interface::RobotHW void commandCallback(const std_msgs::StringConstPtr& msg); bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& res); bool activateSplineInterpolation(std_srvs::SetBoolRequest& req, std_srvs::SetBoolResponse& 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_; @@ -241,7 +242,8 @@ class HardwareInterface : public hardware_interface::RobotHW ros::ServiceServer tare_sensor_srv_; ros::ServiceServer set_payload_srv_; ros::ServiceServer activate_spline_interpolation_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 24a3adf4..de16ad17 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -456,7 +456,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. @@ -1178,258 +1181,290 @@ bool HardwareInterface::setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::Set 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) +bool HardwareInterface::startForceMode(ur_msgs::SetForceModeRequest& req, ur_msgs::SetForceModeResponse& res) { - std::string str = msg->data; - if (str.back() != '\n') + bool HardwareInterface::setForceMode(ur_msgs::SetForceModeRequest & req, ur_msgs::SetForceModeResponse & res) { - str.append("\n"); - } + 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; + + 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') + { + 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) -{ - use_spline_interpolation_ = req.data; - if (use_spline_interpolation_) - { - res.message = "Activated spline interpolation in forward trajectory mode."; - } - 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: { - 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::SUCCESS; + ROS_INFO_STREAM("Forwarded trajectory finished successful."); + break; } - else if (point.velocities.size() == 6) + case urcl::control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED: { - 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::PREEMPTED; + ROS_INFO_STREAM("Forwarded trajectory execution preempted by user."); + break; } - else + case urcl::control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE: { - ROS_ERROR_THROTTLE(1, "Spline interpolation using positions only is not supported."); + final_state = hardware_interface::ExecutionState::ABORTED; + ROS_INFO_STREAM("Forwarded trajectory execution failed."); + break; + } + default: + { + 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)