branch for custom controller

spot_controllers/CMakeLists.txt

@@ -19,6 +19,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   pluginlib
   rclcpp
   rclcpp_lifecycle
+  spot_msgs
 )
 foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${Dependency} REQUIRED)
@@ -37,11 +38,17 @@ generate_parameter_library(
   include/spot_controllers/forward_state_controller_parameters.yaml
 )
 
+generate_parameter_library(
+  spot_forward_controller_parameters
+  include/spot_controllers/spot_forward_controller_parameters.yaml
+)
+
 # Add the hardware interface
 add_library(
   spot_controllers
   SHARED
   src/forward_state_controller.cpp
+  src/spot_forward_controller.cpp
 )
 target_compile_features(spot_controllers PUBLIC cxx_std_20)
 target_include_directories(spot_controllers PUBLIC
@@ -50,7 +57,9 @@ target_include_directories(spot_controllers PUBLIC
 )
 target_link_libraries(
   spot_controllers PUBLIC 
-  forward_state_controller_parameters forward_command_controller::forward_command_controller
+  forward_state_controller_parameters
+  spot_forward_controller_parameters
+  forward_command_controller::forward_command_controller
 )
 ament_target_dependencies(
   spot_controllers PUBLIC
@@ -69,7 +78,7 @@ install(
   DESTINATION include/${PROJECT_NAME}
 )
 
-install(TARGETS spot_controllers forward_state_controller_parameters
+install(TARGETS spot_controllers forward_state_controller_parameters spot_forward_controller_parameters
   EXPORT export_spot_controllers
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib

spot_controllers/include/spot_controllers/spot_forward_controller.hpp

@@ -0,0 +1,79 @@
+// Copyright 2021 Stogl Robotics Consulting UG (haftungsbescrhänkt)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <spot_controllers/spot_forward_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.
+ *
+ * This class forwards the command signal down to a set of joints or interfaces.
+ *
+ * Subscribes to:
+ * - \b joint_commands (spot_msgs::msg::JointCommand) : The commands to apply.
+ */
+class SpotForwardController : public controller_interface::ControllerInterface {
+ public:
+  SpotForwardController();
+
+  ~SpotForwardController() = default;
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::CallbackReturn on_init() override;
+
+  controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+
+  controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+
+  controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+
+  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;
+
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+
+  std::vector<std::string> joint_names_;
+  std::string interface_name_;
+
+  std::vector<std::string> command_interface_types_;
+
+  realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>> rt_command_ptr_;
+  rclcpp::Subscription<CmdType>::SharedPtr joints_command_subscriber_;
+};
+
+}  // namespace spot_controllers

spot_controllers/include/spot_controllers/spot_forward_controller_parameters.yaml

@@ -0,0 +1,16 @@
+spot_forward_controller:
+  joints: {
+    type: string_array,
+    default_value: [],
+    description: "Names of the joint to control",
+  }
+  joint_prefix: {
+    type: string,
+    default_value: "",
+    description: "Optional prefix to apply to joint names"
+  }
+  interface_names: {
+    type: string_array,
+    default_value: [],
+    description: "Names of the interfaces to command",
+  }

spot_controllers/package.xml

@@ -17,6 +17,7 @@
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>
+  <depend>spot_msgs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

spot_controllers/spot_controllers.xml

@@ -4,4 +4,9 @@
     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">
+  <description>
+    Forwarding controller useful for forwarding sets of joint commands (including gains) to Spot's hardware interface.
+  </description>
+  </class>
 </library>

spot_controllers/src/spot_forward_controller.cpp

@@ -0,0 +1,154 @@
+// 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");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "spot_controllers/spot_forward_controller.hpp"
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "controller_interface/helpers.hpp"
+#include "hardware_interface/loaned_command_interface.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/qos.hpp"
+
+namespace spot_controllers {
+SpotForwardController::SpotForwardController()
+    : controller_interface::ControllerInterface(), rt_command_ptr_(nullptr), joints_command_subscriber_(nullptr) {}
+
+controller_interface::CallbackReturn SpotForwardController::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(
+    const rclcpp_lifecycle::State& /*previous_state*/) {
+  joints_command_subscriber_ = get_node()->create_subscription<CmdType>("~/joint_commands", rclcpp::SystemDefaultsQoS(),
+                                                                        [this](const CmdType::SharedPtr msg) {
+                                                                          rt_command_ptr_.writeFromNonRT(msg);
+                                                                        });
+
+  RCLCPP_INFO(get_node()->get_logger(), "configure successful");
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::InterfaceConfiguration SpotForwardController::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
+    command_interfaces_config.names.push_back(joint + "/" + "position");
+    command_interfaces_config.names.push_back(joint + "/" + "velocity");
+    command_interfaces_config.names.push_back(joint + "/" + "effort");
+    command_interfaces_config.names.push_back(joint + "/" + "k_q_p");
+    command_interfaces_config.names.push_back(joint + "/" + "k_qd_p");
+  }
+
+  return command_interfaces_config;
+}
+
+controller_interface::InterfaceConfiguration SpotForwardController::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(
+    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(""),
+                                                    ordered_interfaces) ||
+      command_interface_types_.size() != ordered_interfaces.size()) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Expected %zu command interfaces, got %zu", command_interface_types_.size(),
+                 ordered_interfaces.size());
+    return controller_interface::CallbackReturn::ERROR;
+  }
+
+  // reset command buffer if a command came through callback when controller was inactive
+  rt_command_ptr_ = realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>>(nullptr);
+
+  RCLCPP_INFO(get_node()->get_logger(), "activate successful");
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn SpotForwardController::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*/) {
+  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();
+
+  const bool using_position = (*joint_commands)->position.size() == njoints_to_command;
+  const bool using_velocity = (*joint_commands)->velocity.size() == njoints_to_command;
+  const bool using_effort = (*joint_commands)->effort.size() == njoints_to_command;
+  const bool using_k_q_p = (*joint_commands)->k_q_p.size() == njoints_to_command;
+  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
+    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
+    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...
+      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.
+    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) {
+      command_interfaces_.at(i).set_value((*joint_commands)->velocity.at(command_index));
+    } else if (interface_name == "effort" && using_effort) {
+      command_interfaces_.at(i).set_value((*joint_commands)->effort.at(command_index));
+    } else if (interface_name == "k_q_p" && using_k_q_p) {
+      command_interfaces_.at(i).set_value((*joint_commands)->k_q_p.at(command_index));
+    } else if (interface_name == "k_qd_p" && using_k_qd_p) {
+      command_interfaces_.at(i).set_value((*joint_commands)->k_qd_p.at(command_index));
+    }
+  }
+
+  return controller_interface::return_type::OK;
+}
+
+}  // namespace spot_controllers
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(spot_controllers::SpotForwardController, controller_interface::ControllerInterface)

spot_msgs/CMakeLists.txt

@@ -52,6 +52,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/EStopState.msg"
   "msg/Feedback.msg"
   "msg/FootState.msg"
+  "msg/JointCommand.msg"
   "msg/Lease.msg"
   "msg/LeaseResource.msg"
   "msg/PowerState.msg"

spot_msgs/msg/JointCommand.msg

@@ -0,0 +1,12 @@
+# list of the joint names to control
+string[] name
+# desired position command for each joint
+float64[] position
+# desired velocity command for each joint
+float64[] velocity
+# desired effort command for each joint
+float64[] effort
+# desired k_q_p command for each joint
+float64[] k_q_p
+# desired k_qd_p command for each joint
+float64[] k_qd_p

spot_ros2_control/config/spot_default_controllers_with_arm.yaml

@@ -14,6 +14,9 @@ controller_manager:
     forward_state_controller:
       type: spot_controllers/ForwardStateController
 
+    spot_forward_controller:
+      type: spot_controllers/SpotForwardController
+
 
 forward_position_controller:
   ros__parameters:
@@ -65,3 +68,27 @@ forward_state_controller:
       - position
       - velocity
       - effort
+
+
+spot_forward_controller:
+  ros__parameters:
+    joints:
+      - front_left_hip_x
+      - front_left_hip_y
+      - front_left_knee
+      - front_right_hip_x
+      - front_right_hip_y
+      - front_right_knee
+      - rear_left_hip_x
+      - rear_left_hip_y
+      - rear_left_knee
+      - rear_right_hip_x
+      - rear_right_hip_y
+      - rear_right_knee
+      - arm_sh0
+      - arm_sh1
+      - arm_el0
+      - arm_el1
+      - arm_wr0
+      - arm_wr1
+      - arm_f1x