From f8585d12b70f019e291f49d6c47ea50206385db5 Mon Sep 17 00:00:00 2001 From: Katie Hughes Date: Wed, 26 Feb 2025 16:55:14 -0500 Subject: [PATCH] change name to spot joint controller --- spot_controllers/CMakeLists.txt | 10 ++--- ...ntroller.hpp => spot_joint_controller.hpp} | 29 ++++++++----- ... => spot_joint_controller_parameters.yaml} | 2 +- spot_controllers/spot_controllers.xml | 2 +- ...ntroller.cpp => spot_joint_controller.cpp} | 42 +++++++++---------- spot_ros2_control/README.md | 4 +- .../spot_default_controllers_with_arm.yaml | 6 +-- .../spot_default_controllers_without_arm.yaml | 6 +-- .../examples/set_gripper_gains.py | 2 +- .../launch/spot_ros2_control.launch.py | 2 +- 10 files changed, 54 insertions(+), 51 deletions(-) rename spot_controllers/include/spot_controllers/{spot_forward_controller.hpp => spot_joint_controller.hpp} (65%) rename spot_controllers/include/spot_controllers/{spot_forward_controller_parameters.yaml => spot_joint_controller_parameters.yaml} (93%) rename spot_controllers/src/{spot_forward_controller.cpp => spot_joint_controller.cpp} (78%) diff --git a/spot_controllers/CMakeLists.txt b/spot_controllers/CMakeLists.txt index ae5f351af..0f90af030 100644 --- a/spot_controllers/CMakeLists.txt +++ b/spot_controllers/CMakeLists.txt @@ -39,8 +39,8 @@ generate_parameter_library( ) generate_parameter_library( - spot_forward_controller_parameters - include/spot_controllers/spot_forward_controller_parameters.yaml + spot_joint_controller_parameters + include/spot_controllers/spot_joint_controller_parameters.yaml ) # Add the hardware interface @@ -48,7 +48,7 @@ add_library( spot_controllers SHARED src/forward_state_controller.cpp - src/spot_forward_controller.cpp + src/spot_joint_controller.cpp ) target_compile_features(spot_controllers PUBLIC cxx_std_20) target_include_directories(spot_controllers PUBLIC @@ -58,7 +58,7 @@ target_include_directories(spot_controllers PUBLIC target_link_libraries( spot_controllers PUBLIC forward_state_controller_parameters - spot_forward_controller_parameters + spot_joint_controller_parameters forward_command_controller::forward_command_controller ) ament_target_dependencies( @@ -78,7 +78,7 @@ install( DESTINATION include/${PROJECT_NAME} ) -install(TARGETS spot_controllers forward_state_controller_parameters spot_forward_controller_parameters +install(TARGETS spot_controllers forward_state_controller_parameters spot_joint_controller_parameters EXPORT export_spot_controllers ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/spot_controllers/include/spot_controllers/spot_forward_controller.hpp b/spot_controllers/include/spot_controllers/spot_joint_controller.hpp similarity index 65% rename from spot_controllers/include/spot_controllers/spot_forward_controller.hpp rename to spot_controllers/include/spot_controllers/spot_joint_controller.hpp index a29d3b0a6..a4ca3e09a 100644 --- a/spot_controllers/include/spot_controllers/spot_forward_controller.hpp +++ b/spot_controllers/include/spot_controllers/spot_joint_controller.hpp @@ -1,3 +1,7 @@ +// File modified. Modifications Copyright (c) 2025 Boston Dynamics AI Institute LLC. +// All rights reserved. + +// -------------------------------------------------------------- // Copyright 2021 Stogl Robotics Consulting UG (haftungsbescrhänkt) // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -18,33 +22,36 @@ #include #include -#include +#include #include "controller_interface/controller_interface.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.hpp" -#include "sensor_msgs/msg/joint_state.hpp" #include "spot_controllers/visibility_control.h" #include "spot_msgs/msg/joint_command.hpp" #include "std_msgs/msg/float64_multi_array.hpp" namespace spot_controllers { using CmdType = spot_msgs::msg::JointCommand; -// using CmdType = sensor_msgs::msg::JointState; /** - * \brief Forward command controller for a set of joints and interfaces. + * \brief Controller for Spot capable of streaming any combination of position, velocity, load, k_q_p, and k_qd_p + * to the Spot hardware interface for a given set of joints. * - * This class forwards the command signal down to a set of joints or interfaces. + * This controller parses the information provided in the JointCommand message received to determine which command + * interfaces to control. * * Subscribes to: - * - \b joint_commands (spot_msgs::msg::JointCommand) : The commands to apply. + * - \b joint_commands (spot_msgs::msg::JointCommand) : The commands to apply. The `name` field should contain the + * names of the joints to command (this can be any number of joints, from one to all of the joints on the robot). + * For each joint name entry, choose which interface/s you want to control by filling out the correspoinding index in + * the other fields of the JointCommand message (position, velocity, effort, k_q_p, and/or k_q_d) */ -class SpotForwardController : public controller_interface::ControllerInterface { +class SpotJointController : public controller_interface::ControllerInterface { public: - SpotForwardController(); + SpotJointController(); - ~SpotForwardController() = default; + ~SpotJointController() = default; controller_interface::InterfaceConfiguration command_interface_configuration() const override; @@ -61,8 +68,8 @@ class SpotForwardController : public controller_interface::ControllerInterface { controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; protected: - using Params = spot_forward_controller::Params; - using ParamListener = spot_forward_controller::ParamListener; + using Params = spot_joint_controller::Params; + using ParamListener = spot_joint_controller::ParamListener; std::shared_ptr param_listener_; Params params_; diff --git a/spot_controllers/include/spot_controllers/spot_forward_controller_parameters.yaml b/spot_controllers/include/spot_controllers/spot_joint_controller_parameters.yaml similarity index 93% rename from spot_controllers/include/spot_controllers/spot_forward_controller_parameters.yaml rename to spot_controllers/include/spot_controllers/spot_joint_controller_parameters.yaml index 2a37affdf..b22fe7ade 100644 --- a/spot_controllers/include/spot_controllers/spot_forward_controller_parameters.yaml +++ b/spot_controllers/include/spot_controllers/spot_joint_controller_parameters.yaml @@ -1,4 +1,4 @@ -spot_forward_controller: +spot_joint_controller: joints: { type: string_array, default_value: [], diff --git a/spot_controllers/spot_controllers.xml b/spot_controllers/spot_controllers.xml index 656b42963..0cdd5ecc0 100644 --- a/spot_controllers/spot_controllers.xml +++ b/spot_controllers/spot_controllers.xml @@ -4,7 +4,7 @@ General passthrough controller that can forward commands for a set of joints over a set of interfaces. - + Forwarding controller useful for forwarding sets of joint commands (including gains) to Spot's hardware interface. diff --git a/spot_controllers/src/spot_forward_controller.cpp b/spot_controllers/src/spot_joint_controller.cpp similarity index 78% rename from spot_controllers/src/spot_forward_controller.cpp rename to spot_controllers/src/spot_joint_controller.cpp index 02e7bca36..c05eb37ef 100644 --- a/spot_controllers/src/spot_forward_controller.cpp +++ b/spot_controllers/src/spot_joint_controller.cpp @@ -16,7 +16,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "spot_controllers/spot_forward_controller.hpp" +#include "spot_controllers/spot_joint_controller.hpp" #include #include @@ -28,16 +28,16 @@ #include "rclcpp/qos.hpp" namespace spot_controllers { -SpotForwardController::SpotForwardController() +SpotJointController::SpotJointController() : controller_interface::ControllerInterface(), rt_command_ptr_(nullptr), joints_command_subscriber_(nullptr) {} -controller_interface::CallbackReturn SpotForwardController::on_init() { +controller_interface::CallbackReturn SpotJointController::on_init() { auto param_listener = std::make_shared(get_node()); params_ = param_listener->get_params(); return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn SpotForwardController::on_configure( +controller_interface::CallbackReturn SpotJointController::on_configure( const rclcpp_lifecycle::State& /*previous_state*/) { joints_command_subscriber_ = get_node()->create_subscription("~/joint_commands", rclcpp::SystemDefaultsQoS(), [this](const CmdType::SharedPtr msg) { @@ -48,11 +48,11 @@ controller_interface::CallbackReturn SpotForwardController::on_configure( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::InterfaceConfiguration SpotForwardController::command_interface_configuration() const { +controller_interface::InterfaceConfiguration SpotJointController::command_interface_configuration() const { controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; for (const auto& joint : params_.joints) { - // these are the interfaces that get claimed. TODO(katie) add k_q_p and k_qd_p once i switch to JointCommand msg + // these are the interfaces that get claimed command_interfaces_config.names.push_back(joint + "/" + "position"); command_interfaces_config.names.push_back(joint + "/" + "velocity"); command_interfaces_config.names.push_back(joint + "/" + "effort"); @@ -63,12 +63,12 @@ controller_interface::InterfaceConfiguration SpotForwardController::command_inte return command_interfaces_config; } -controller_interface::InterfaceConfiguration SpotForwardController::state_interface_configuration() const { +controller_interface::InterfaceConfiguration SpotJointController::state_interface_configuration() const { // this controller has no knowledge of robot state... but it could (i.e. basic command validation?) return controller_interface::InterfaceConfiguration{controller_interface::interface_configuration_type::NONE}; } -controller_interface::CallbackReturn SpotForwardController::on_activate( +controller_interface::CallbackReturn SpotJointController::on_activate( const rclcpp_lifecycle::State& /*previous_state*/) { std::vector> ordered_interfaces; if (!controller_interface::get_ordered_interfaces(command_interfaces_, command_interface_types_, std::string(""), @@ -86,24 +86,21 @@ controller_interface::CallbackReturn SpotForwardController::on_activate( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn SpotForwardController::on_deactivate( +controller_interface::CallbackReturn SpotJointController::on_deactivate( const rclcpp_lifecycle::State& /*previous_state*/) { // reset command buffer rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type SpotForwardController::update(const rclcpp::Time& /*time*/, - const rclcpp::Duration& /*period*/) { +controller_interface::return_type SpotJointController::update(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) { auto joint_commands = rt_command_ptr_.readFromRT(); // no command received yet if (!joint_commands || !(*joint_commands)) { return controller_interface::return_type::OK; } - // command_interfaces_.at(i).get_name() is full name i.e. arm_sh0/position - // .get_interface_name() is just interface i.e. position - // .get_prefix_name() is the joint name i.e. arm_sh0 const auto& joint_names = (*joint_commands)->name; const auto& njoints_to_command = joint_names.size(); @@ -115,22 +112,21 @@ controller_interface::return_type SpotForwardController::update(const rclcpp::Ti const bool using_k_qd_p = (*joint_commands)->k_qd_p.size() == njoints_to_command; for (size_t i = 0; i < command_interfaces_.size(); i++) { - // idea: for every element in the command interface: - // first tell if the name is in the name field of the message - // if it is, set the interfaces accordingly + // for every command interface, we wnat to tell if the received JointCommand contains an update for it. + // First get the joint name (i.e. arm_sh0) and the interface name of this command interface (i.e. position) const auto& joint_name = command_interfaces_.at(i).get_prefix_name(); const auto& interface_name = command_interfaces_.at(i).get_interface_name(); - // check if joint_name in name field of message + // check if we want to command an interface on this joint by seeing if it's in the JointCommand message. const auto& it = std::find(joint_names.begin(), joint_names.end(), joint_name); if (it == joint_names.end()) { - // it wasn't here, keep looping - // TODO(katie) should we set a value from state interfaces here... + // it wasn't in the JointCommand message, on to the next. continue; } // get the index that the name is at in the command const auto command_index = std::distance(joint_names.begin(), it); - // check if interface name is there and update accordingly - // todo(katie) (micro optimization) -- could break early none of using_x fields is true. + // check if interface name is there and update accordingly. + // it's possible none of these are true (i.e. we are only using_position, but this specific interface is effort) + // then we just keep going onto the next interface to check again. if (interface_name == "position" && using_position) { command_interfaces_.at(i).set_value((*joint_commands)->position.at(command_index)); } else if (interface_name == "velocity" && using_velocity) { @@ -151,4 +147,4 @@ controller_interface::return_type SpotForwardController::update(const rclcpp::Ti #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(spot_controllers::SpotForwardController, controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS(spot_controllers::SpotJointController, controller_interface::ControllerInterface) diff --git a/spot_ros2_control/README.md b/spot_ros2_control/README.md index 3e28cec3c..25d935612 100644 --- a/spot_ros2_control/README.md +++ b/spot_ros2_control/README.md @@ -70,8 +70,8 @@ ros2 launch spot_ros2_control wiggle_arm.launch.py ``` Add the launch argument `spot_name:=` if the ros2 control stack was launched in a namespace. -An alternate example using the `spot_forward_controller` is provided to demonstrate how to stream position and gains at the same time. -First launch `spot_ros2_control.launch.py` with the launch argument `robot_controller:=spot_forward_controller`. +An alternate example using the `spot_joint_controller` is provided to demonstrate how to stream position and gains at the same time. +First launch `spot_ros2_control.launch.py` with the launch argument `robot_controller:=spot_joint_controller`. Next, run ```bash ros2 run spot_ros2_control set_grippper_gains diff --git a/spot_ros2_control/config/spot_default_controllers_with_arm.yaml b/spot_ros2_control/config/spot_default_controllers_with_arm.yaml index 2b6a5ff00..593c02036 100644 --- a/spot_ros2_control/config/spot_default_controllers_with_arm.yaml +++ b/spot_ros2_control/config/spot_default_controllers_with_arm.yaml @@ -14,8 +14,8 @@ controller_manager: forward_state_controller: type: spot_controllers/ForwardStateController - spot_forward_controller: - type: spot_controllers/SpotForwardController + spot_joint_controller: + type: spot_controllers/SpotJointController forward_position_controller: @@ -70,7 +70,7 @@ forward_state_controller: - effort -spot_forward_controller: +spot_joint_controller: ros__parameters: joints: - front_left_hip_x diff --git a/spot_ros2_control/config/spot_default_controllers_without_arm.yaml b/spot_ros2_control/config/spot_default_controllers_without_arm.yaml index 47466c09a..1e756a882 100644 --- a/spot_ros2_control/config/spot_default_controllers_without_arm.yaml +++ b/spot_ros2_control/config/spot_default_controllers_without_arm.yaml @@ -14,8 +14,8 @@ controller_manager: forward_state_controller: type: spot_controllers/ForwardStateController - spot_forward_controller: - type: spot_controllers/SpotForwardController + spot_joint_controller: + type: spot_controllers/SpotJointController forward_position_controller: @@ -55,7 +55,7 @@ forward_state_controller: - velocity - effort -spot_forward_controller: +spot_joint_controller: ros__parameters: joints: - front_left_hip_x diff --git a/spot_ros2_control/examples/set_gripper_gains.py b/spot_ros2_control/examples/set_gripper_gains.py index e85fde05b..59d88ba7d 100644 --- a/spot_ros2_control/examples/set_gripper_gains.py +++ b/spot_ros2_control/examples/set_gripper_gains.py @@ -26,7 +26,7 @@ def __init__(self, node: Node, robot_name: Optional[str] = None) -> None: self._logger = self._node.get_logger() self._robot_name = robot_name self._command_pub = self._node.create_publisher( - JointCommand, namespace_with(self._robot_name, "spot_forward_controller/joint_commands"), 10 + JointCommand, namespace_with(self._robot_name, "spot_joint_controller/joint_commands"), 10 ) self._joint_command = JointCommand() self._joint_command.name = [namespace_with(self._robot_name, GRIPPER_JOINT_NAME)] diff --git a/spot_ros2_control/launch/spot_ros2_control.launch.py b/spot_ros2_control/launch/spot_ros2_control.launch.py index 44d7f57ba..f517b85ca 100644 --- a/spot_ros2_control/launch/spot_ros2_control.launch.py +++ b/spot_ros2_control/launch/spot_ros2_control.launch.py @@ -53,7 +53,7 @@ def create_controllers_config(spot_name: str, has_arm: bool) -> str: config = yaml.safe_load(template_file) config[f"{spot_name}/controller_manager"] = config["controller_manager"] del config["controller_manager"] - keys_to_namespace = ["forward_position_controller", "forward_state_controller", "spot_forward_controller"] + keys_to_namespace = ["forward_position_controller", "forward_state_controller", "spot_joint_controller"] for key in keys_to_namespace: key_joints = config[key]["ros__parameters"]["joints"]