diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index 3d9c785be..8e34503a1 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -81,6 +81,7 @@ namespace ur_controllers * 5.0: The robot finished executing the trajectory. */ const double TRANSFER_STATE_IDLE = 0.0; +const double TRANSFER_STATE_NEW_TRAJECTORY = 6.0; const double TRANSFER_STATE_WAITING_FOR_POINT = 1.0; const double TRANSFER_STATE_TRANSFERRING = 2.0; const double TRANSFER_STATE_TRANSFER_DONE = 3.0; @@ -174,6 +175,7 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI std::optional> scaling_state_interface_; std::optional> abort_command_interface_; + std::optional> trajectory_size_command_interface_; std::optional> transfer_command_interface_; std::optional> time_from_start_command_interface_; diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 27f709cf7..7e7aa0d4d 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -132,6 +132,7 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co config.names.push_back(tf_prefix + "trajectory_passthrough/abort"); config.names.emplace_back(tf_prefix + "trajectory_passthrough/transfer_state"); config.names.emplace_back(tf_prefix + "trajectory_passthrough/time_from_start"); + config.names.emplace_back(tf_prefix + "trajectory_passthrough/trajectory_size"); return config; } @@ -180,6 +181,19 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_activat } } + { + const std::string interface_name = passthrough_params_.tf_prefix + "trajectory_passthrough/" + "trajectory_size"; + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != command_interfaces_.end()) { + trajectory_size_command_interface_ = *it; + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in command interfaces.", interface_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + } + const std::string tf_prefix = passthrough_params_.tf_prefix; { const std::string interface_name = tf_prefix + "trajectory_passthrough/transfer_state"; @@ -254,7 +268,9 @@ controller_interface::return_type PassthroughTrajectoryController::update(const active_trajectory_elapsed_time_ = rclcpp::Duration(0, 0); max_trajectory_time_ = rclcpp::Duration::from_seconds(duration_to_double(active_joint_traj_.points.back().time_from_start)); - write_success &= transfer_command_interface_->get().set_value(TRANSFER_STATE_WAITING_FOR_POINT); + write_success &= transfer_command_interface_->get().set_value(TRANSFER_STATE_NEW_TRAJECTORY); + write_success &= + trajectory_size_command_interface_->get().set_value(static_cast(active_joint_traj_.points.size())); } auto active_goal_time_tol = goal_time_tolerance_.readFromRT(); auto joint_mapping = joint_trajectory_mapping_.readFromRT(); diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 845720fec..87e2fd59f 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -167,8 +167,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface void stop_force_mode(); void check_passthrough_trajectory_controller(); void trajectory_done_callback(urcl::control::TrajectoryResult result); - bool has_accelerations(std::vector> accelerations); - bool has_velocities(std::vector> velocities); + bool is_valid_joint_information(std::vector> data); urcl::vector6d_t urcl_position_commands_; urcl::vector6d_t urcl_position_commands_old_; @@ -240,6 +239,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface // Passthrough trajectory controller interface values double passthrough_trajectory_transfer_state_; double passthrough_trajectory_abort_; + double passthrough_trajectory_size_; bool passthrough_trajectory_controller_running_; urcl::vector6d_t passthrough_trajectory_positions_; urcl::vector6d_t passthrough_trajectory_velocities_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index a8a75b5f3..cc08ccb43 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -45,6 +45,7 @@ #include "ur_client_library/exceptions.h" #include "ur_client_library/ur/tool_communication.h" #include "ur_client_library/ur/version_information.h" +#include "ur_client_library/ur/robot_receive_timeout.h" #include #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -93,9 +94,10 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys freedrive_mode_abort_ = 0.0; passthrough_trajectory_transfer_state_ = 0.0; passthrough_trajectory_abort_ = 0.0; - trajectory_joint_positions_.clear(); - trajectory_joint_velocities_.clear(); - trajectory_joint_accelerations_.clear(); + passthrough_trajectory_size_ = 0.0; + trajectory_joint_positions_.reserve(32768); + trajectory_joint_velocities_.reserve(32768); + trajectory_joint_accelerations_.reserve(32768); for (const hardware_interface::ComponentInfo& joint : info_.joints) { if (joint.command_interfaces.size() != 2) { @@ -381,6 +383,9 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back( hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, "abort", &passthrough_trajectory_abort_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, "trajectory_size", + &passthrough_trajectory_size_)); + for (size_t i = 0; i < 6; ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, "setpoint_positions_" + std::to_string(i), @@ -540,6 +545,9 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou driver_config.handle_program_state = std::bind(&URPositionHardwareInterface::handleRobotProgramState, this, std::placeholders::_1); ur_driver_ = std::make_unique(driver_config); + if (ur_driver_->getControlFrequency() != info_.rw_rate) { + ur_driver_->resetRTDEClient(output_recipe_filename, input_recipe_filename, info_.rw_rate); + } } catch (urcl::ToolCommNotAvailable& e) { RCLCPP_FATAL_STREAM(rclcpp::get_logger("URPositionHardwareInterface"), "See parameter use_tool_communication"); @@ -786,7 +794,9 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp: ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_NOOP); } else if (passthrough_trajectory_controller_running_) { - ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); + ur_driver_->writeTrajectoryControlMessage( + urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, 0, + urcl::RobotReceiveTimeout::millisec(1000 * 5.0 / static_cast(info_.rw_rate))); check_passthrough_trajectory_controller(); } else { ur_driver_->writeKeepalive(); @@ -1322,59 +1332,102 @@ void URPositionHardwareInterface::stop_force_mode() void URPositionHardwareInterface::check_passthrough_trajectory_controller() { static double last_time = 0.0; + static size_t point_index_received = 0; + static size_t point_index_sent = 0; + static bool trajectory_started = false; // See passthrough_trajectory_controller.hpp for an explanation of the passthrough_trajectory_transfer_state_ values. // We should abort and are not in state IDLE if (passthrough_trajectory_abort_ == 1.0 && passthrough_trajectory_transfer_state_ != 0.0) { ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL); + } else if (passthrough_trajectory_transfer_state_ == 6.0) { + if (passthrough_trajectory_size_ != trajectory_joint_positions_.size()) { + RCLCPP_INFO(get_logger(), "Got a new trajectory with %lu points.", + static_cast(passthrough_trajectory_size_)); + trajectory_joint_positions_.resize(passthrough_trajectory_size_); + trajectory_joint_velocities_.resize(passthrough_trajectory_size_); + trajectory_joint_accelerations_.resize(passthrough_trajectory_size_); + trajectory_times_.resize(passthrough_trajectory_size_); + point_index_received = 0; + point_index_sent = 0; + trajectory_started = false; + last_time = 0.0; + passthrough_trajectory_transfer_state_ = 1.0; + } } else if (passthrough_trajectory_transfer_state_ == 2.0) { passthrough_trajectory_abort_ = 0.0; - trajectory_joint_positions_.push_back(passthrough_trajectory_positions_); + trajectory_joint_positions_[point_index_received] = passthrough_trajectory_positions_; - trajectory_times_.push_back(passthrough_trajectory_time_from_start_ - last_time); + trajectory_times_[point_index_received] = passthrough_trajectory_time_from_start_ - last_time; last_time = passthrough_trajectory_time_from_start_; - if (!std::isnan(passthrough_trajectory_velocities_[0])) { - trajectory_joint_velocities_.push_back(passthrough_trajectory_velocities_); - } - if (!std::isnan(passthrough_trajectory_accelerations_[0])) { - trajectory_joint_accelerations_.push_back(passthrough_trajectory_accelerations_); - } + trajectory_joint_velocities_[point_index_received] = passthrough_trajectory_velocities_; + trajectory_joint_accelerations_[point_index_received] = passthrough_trajectory_accelerations_; + + point_index_received++; passthrough_trajectory_transfer_state_ = 1.0; - /* When all points have been read, write them to the physical robot controller.*/ + + // Once we received enough points so we can move for at least 5 cycles, we start executing + if ((passthrough_trajectory_time_from_start_ > 5.0 / static_cast(info_.rw_rate) || + point_index_received == passthrough_trajectory_size_ - 1) && + !trajectory_started) { + ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + trajectory_joint_positions_.size()); + trajectory_started = true; + } } else if (passthrough_trajectory_transfer_state_ == 3.0) { - /* Tell robot controller how many points are in the trajectory. */ - ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, - trajectory_joint_positions_.size()); + passthrough_trajectory_abort_ = 0.0; + passthrough_trajectory_transfer_state_ = 4.0; + } else if (passthrough_trajectory_transfer_state_ == 4.0) { + if (point_index_sent == trajectory_joint_positions_.size()) { + trajectory_joint_positions_.clear(); + trajectory_joint_accelerations_.clear(); + trajectory_joint_velocities_.clear(); + trajectory_times_.clear(); + last_time = 0.0; + } + } + + // We basically get a setpoint from the controller in each cycle. We send all the points that we + // already received down to the hardware. + if (trajectory_started && point_index_sent <= trajectory_joint_positions_.size() && + point_index_sent < point_index_received) { + bool error = false; /* Write the appropriate type of point depending on the combination of positions, velocities and accelerations. */ - if (!has_velocities(trajectory_joint_velocities_) && !has_accelerations(trajectory_joint_accelerations_)) { - for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { - ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], urcl::vector6d_t{ 0, 0, 0, 0, 0, 0 }, - trajectory_times_[i]); - } - } else if (has_velocities(trajectory_joint_velocities_) && !has_accelerations(trajectory_joint_accelerations_)) { - for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { - ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], trajectory_joint_velocities_[i], - trajectory_times_[i]); - } - } else if (!has_velocities(trajectory_joint_velocities_) && has_accelerations(trajectory_joint_accelerations_)) { - for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { - ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], trajectory_joint_accelerations_[i], - trajectory_times_[i]); - } - } else if (has_velocities(trajectory_joint_velocities_) && has_accelerations(trajectory_joint_accelerations_)) { - for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { - ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], trajectory_joint_velocities_[i], - trajectory_joint_accelerations_[i], trajectory_times_[i]); + for (size_t i = point_index_sent; i < point_index_received; i++) { + if (is_valid_joint_information(trajectory_joint_positions_)) { + if (!is_valid_joint_information(trajectory_joint_velocities_) && + !is_valid_joint_information(trajectory_joint_accelerations_)) { + ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], urcl::vector6d_t{ 0, 0, 0, 0, 0, 0 }, + trajectory_times_[i]); + } else if (is_valid_joint_information(trajectory_joint_velocities_) && + !is_valid_joint_information(trajectory_joint_accelerations_)) { + ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], trajectory_joint_velocities_[i], + trajectory_times_[i]); + } else if (!is_valid_joint_information(trajectory_joint_velocities_) && + is_valid_joint_information(trajectory_joint_accelerations_)) { + RCLCPP_ERROR(get_logger(), "Accelerations but no velocities given. If you want to specify accelerations with " + "a 0 velocity, please do that explicitly."); + error = true; + break; + + } else if (is_valid_joint_information(trajectory_joint_velocities_) && + is_valid_joint_information(trajectory_joint_accelerations_)) { + ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], trajectory_joint_velocities_[i], + trajectory_joint_accelerations_[i], trajectory_times_[i]); + } + } else { + RCLCPP_ERROR(get_logger(), "Trajectory points without position information are not supported."); + error = true; + break; } + point_index_sent++; + } + if (error) { + passthrough_trajectory_abort_ = 1.0; + passthrough_trajectory_transfer_state_ = 5.0; + return; } - trajectory_joint_positions_.clear(); - trajectory_joint_accelerations_.clear(); - trajectory_joint_velocities_.clear(); - trajectory_times_.clear(); - last_time = 0.0; - passthrough_trajectory_abort_ = 0.0; - passthrough_trajectory_transfer_state_ = 4.0; } } @@ -1389,14 +1442,9 @@ void URPositionHardwareInterface::trajectory_done_callback(urcl::control::Trajec return; } -bool URPositionHardwareInterface::has_velocities(std::vector> velocities) -{ - return (velocities.size() > 0); -} - -bool URPositionHardwareInterface::has_accelerations(std::vector> accelerations) +bool URPositionHardwareInterface::is_valid_joint_information(std::vector> data) { - return (accelerations.size() > 0); + return (data.size() > 0 && !std::isnan(data[0][0])); } } // namespace ur_robot_driver diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index 91e73a888..68a2b73a7 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -289,6 +289,7 @@ +