change name to spot joint controller

spot_controllers/CMakeLists.txt

@@ -39,16 +39,16 @@ 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
 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

spot_controllers/include/spot_controllers/spot_forward_controller.hpp → 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 <string>
 #include <vector>
 
-#include <spot_controllers/spot_forward_controller_parameters.hpp>
+#include <spot_controllers/spot_joint_controller_parameters.hpp>
 #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<ParamListener> param_listener_;
   Params params_;

spot_controllers/include/spot_controllers/spot_forward_controller_parameters.yaml → 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: [],

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.
   </description>
   </class>
-  <class name="spot_controllers/SpotForwardController" type="spot_controllers::SpotForwardController" base_class_type="controller_interface::ControllerInterface">
+  <class name="spot_controllers/SpotJointController" type="spot_controllers::SpotJointController" base_class_type="controller_interface::ControllerInterface">
   <description>
     Forwarding controller useful for forwarding sets of joint commands (including gains) to Spot's hardware interface.
   </description>

spot_controllers/src/spot_forward_controller.cpp → 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 <memory>
 #include <string>
@@ -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<ParamListener>(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<CmdType>("~/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<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> 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<std::shared_ptr<CmdType>>(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)

spot_ros2_control/README.md

@@ -70,8 +70,8 @@ ros2 launch spot_ros2_control wiggle_arm.launch.py
 ```
 Add the launch argument `spot_name:=<namespace>` 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

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

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

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)]

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"]