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 67e27a222..15798c349 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -192,6 +192,11 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double system_interface_initialized_; bool async_thread_shutdown_; + // Passthrough trajectory controller interface values + urcl::vector6d_t passthrough_trajectory_positions_; + urcl::vector6d_t passthrough_trajectory_velocities_; + urcl::vector6d_t passthrough_trajectory_accelerations_; + // payload stuff urcl::vector3d_t payload_center_of_gravity_; double payload_mass_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 5826cf595..14a8ef48d 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -95,9 +95,9 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys system_interface_initialized_ = 0.0; for (const hardware_interface::ComponentInfo& joint : info_.joints) { - if (joint.command_interfaces.size() != 2) { + if (joint.command_interfaces.size() != 5) { RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"), - "Joint '%s' has %zu command interfaces found. 2 expected.", joint.name.c_str(), + "Joint '%s' has %zu command interfaces found. 5 expected.", joint.name.c_str(), joint.command_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } @@ -243,6 +243,15 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &urcl_velocity_commands_[i])); + + command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, "passthrough_position", + &passthrough_trajectory_positions_[i])); + + command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, "passthrough_velocity", + &passthrough_trajectory_velocities_[i])); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "passthrough_acceleration", &passthrough_trajectory_accelerations_[i])); } // Obtain the tf_prefix from the urdf so that we can have the general interface multiple times // NOTE using the tf_prefix at this point is some kind of workaround. One should actually go through the list of gpio