[SW-1466] ability to take joint gains in through a parameter file (#516)

## Change Overview

1. standardizes gain names in the hardware interface to `k_q_p` and `k_qd_p`, same as spot sdk (https://dev.bostondynamics.com/docs/concepts/joint_control/readme)

2. allows taking in `k_q_p` and `k_qd_p` through a a parameter file. If you have a yaml that looks like this
```
/**:
  ros__parameters:
    k_q_p: [624.0, 936.0, 286.0,  624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 1020.0, 255.0, 204.0, 102.0, 102.0, 102.0, 16.0]
    k_qd_p: [5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 10.2, 15.3, 10.2, 2.04, 2.04, 2.04, 0.32]
```
and pass it to the `spot_ros2_control.launch.py` launchfile with the arg `config_file:=<file path>`, these gains will get used in the hardware interface to stream commands.
[Because ros2 control hardware interfaces don't have "true" support for parameters, ](ros-controls/ros2_control#347 gains have to be passed in as a [string through the xacro](https://github.com/ros-controls/ros2_control/blob/cb91599f8f66aaf39b7485a2f7e131157f633474/hardware_interface/include/hardware_interface/hardware_info.hpp#L182), so this also adds a helper function to assist with this conversion back to a usable vector. 

edge case handling:
1. if either of these params isn't set (or no config file is specified) then the default gains are used. 
2. if either the gain lists defined in the parameter file is the wrong number of elements, the hardware interface will fall back to using the default gains, and a warning will be logged about this.

## Testing Done

* The wiggle arm example has some slightly jerky gripper motion using the default gains.
I changed the `k_qd_p` gain for the gripper joint in my parameter file from `0.32` to `1.32` and reran the example, and the gripper looked a lot smoother. 

* Checked edge case handling manually 

### Next steps

The function to translate the gains as a string into a usable vector in the hardware interface should get unit tested (SW-1148)

spot_description/urdf/spot.ros2_control.xacro

@@ -20,10 +20,6 @@
                 <param name="min">${-effort_max}</param>
                 <param name="max">${effort_max}</param>
             </command_interface>
-            <!-- TODO at some point, this should be a part of the command interface so we can have a 
-            one-to-one mapping to the spot sdk implementation. -->
-            <!-- <command_interface name="k_q_p" />
-            <command_interface name="k_qd_p" /> -->
         </joint>
     </xacro:macro>
 
@@ -65,7 +61,7 @@
 
     </xacro:macro>
 
-    <xacro:macro name="spot_ros2_control" params="interface_type has_arm hostname username password tf_prefix">
+    <xacro:macro name="spot_ros2_control" params="interface_type has_arm hostname username password tf_prefix k_q_p k_qd_p">
         <!-- Currently implements a simple system interface that covers all joints of the robot.
         In the future, we could make different hardware interfaces for the body, arm, etc. -->
         <ros2_control name="SpotSystem" type="system">
@@ -80,6 +76,8 @@
                 <param name="hostname">$(optenv SPOT_IP ${hostname})</param>
                 <param name="username">$(optenv BOSDYN_CLIENT_USERNAME ${username})</param>
                 <param name="password">$(optenv BOSDYN_CLIENT_PASSWORD ${password})</param>
+                <param name="k_q_p">${k_q_p}</param>
+                <param name="k_qd_p">${k_qd_p}</param>
             </xacro:if>
             </hardware>
             <!-- Add the legs -->

spot_description/urdf/spot.urdf.xacro

@@ -14,10 +14,11 @@
   <!-- Parameters for ROS 2 control -->
   <xacro:arg name="add_ros2_control_tag" default="false" />
   <xacro:arg name="hardware_interface_type" default="mock" />
-  <xacro:arg name="tf_prefix" default="" />
   <xacro:arg name="hostname" default="10.0.0.3" />
   <xacro:arg name="username" default="username" />
   <xacro:arg name="password" default="password" />
+  <xacro:arg name="k_q_p" default="" />
+  <xacro:arg name="k_qd_p" default="" />
 
   <!-- Load Spot -->
   <xacro:load_spot
@@ -32,7 +33,9 @@
                                hostname="$(arg hostname)" 
                                username="$(arg username)" 
                                password="$(arg password)" 
-                               tf_prefix="$(arg tf_prefix)" />
+                               tf_prefix="$(arg tf_prefix)" 
+                               k_q_p="$(arg k_q_p)" 
+                               k_qd_p="$(arg k_qd_p)" />
   </xacro:if>
 
 </robot>

spot_hardware_interface/include/spot_hardware_interface/spot_constants.hpp

@@ -26,22 +26,24 @@ inline constexpr int kNjointsNoArm = 12;
 /// @brief Number of joints we expect if the robot has an arm
 inline constexpr int kNjointsArm = 19;
 
-// Gain values https://github.com/boston-dynamics/spot-cpp-sdk/blob/master/cpp/examples/joint_control/constants.hpp
-// This will be handled via a parameter in the future so there is the option to change them, for now they are hardcoded
+// Default gain values obtained from
+// https://github.com/boston-dynamics/spot-cpp-sdk/blob/master/cpp/examples/joint_control/constants.hpp
 
-/// @brief Default Kp gains for robot without an arm
-inline constexpr float kDefaultKpNoArm[] = {624.0, 936.0, 286.0, 624.0, 936.0, 286.0,
-                                            624.0, 936.0, 286.0, 624.0, 936.0, 286.0};
+/// @brief Default k_q_p gains for robot without an arm
+inline constexpr float kDefaultKqpNoArm[] = {624.0, 936.0, 286.0, 624.0, 936.0, 286.0,
+                                             624.0, 936.0, 286.0, 624.0, 936.0, 286.0};
 
-/// @brief Default Kd gains for robot without an arm
-inline constexpr float kDefaultKdNoArm[] = {5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04};
+/// @brief Default k_qd_p gains for robot without an arm
+inline constexpr float kDefaultKqdpNoArm[] = {5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04};
 
-/// @brief Default Kp gains for robot with an arm
-inline constexpr float kDefaultKpArm[] = {624.0, 936.0, 286.0,  624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0,
-                                          936.0, 286.0, 1020.0, 255.0, 204.0, 102.0, 102.0, 102.0, 16.0};
+/// @brief Default k_q_p gains for robot with an arm (note that the first 12 elements that correspond to the leg joints
+/// are the same as `kDefaultKqpNoArm`)
+inline constexpr float kDefaultKqpArm[] = {624.0, 936.0, 286.0,  624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0,
+                                           936.0, 286.0, 1020.0, 255.0, 204.0, 102.0, 102.0, 102.0, 16.0};
 
-/// @brief Default Kd gains for robot with an arm
-inline constexpr float kDefaultKdArm[] = {5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20,
-                                          5.20, 2.04, 10.2, 15.3, 10.2, 2.04, 2.04, 2.04, 0.32};
+/// @brief Default k_qd_p gains for robot with an arm (note that the first 12 elements that correspond to the leg joints
+/// are the same as `kDefaultKqdpNoArm`)
+inline constexpr float kDefaultKqdpArm[] = {5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20,
+                                            5.20, 2.04, 10.2, 15.3, 10.2, 2.04, 2.04, 2.04, 0.32};
 
 }  // namespace spot_hardware_interface

spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp

@@ -20,6 +20,7 @@
 
 #include <functional>
 #include <memory>
+#include <sstream>
 #include <string>
 #include <thread>
 #include <vector>
@@ -128,6 +129,10 @@ class SpotHardware : public hardware_interface::SystemInterface {
   std::string username_;
   std::string password_;
 
+  // Stores gains to be used in the joint level command
+  std::vector<float> k_q_p_;
+  std::vector<float> k_qd_p_;
+
   // Power status
   bool powered_on_ = false;
 
@@ -157,6 +162,17 @@ class SpotHardware : public hardware_interface::SystemInterface {
 
   ::bosdyn::api::JointControlStreamRequest joint_request_;
 
+  /// @brief Gains are passed in as hardware parameters as a space separated string. This function translates this
+  /// information into the corresponding std::vector to be used in constructing the robot streaming command.
+  /// @param gain_string Space separated string of k_q_p or k_qd_p values  -- e.g., "1.0 2.0 3.0"
+  /// @param default_gains Vector of default gains to fall back to if the input is not formatted correctly.
+  /// @param gain_name Human readable name of the parameter parsed -- e.g., "k_q_p". Used in logging a warning if the
+  /// input is malformed.
+  /// @return The input gain_string formatted as an std::vector if it is the appropriate number of elements, and
+  /// default_gains if not.
+  std::vector<float> parse_gains_parameter(const std::string gains_string, const std::vector<float>& default_gains,
+                                           const std::string gain_name);
+
   // The following are functions that interact with the BD SDK to set up the robot and get the robot states.
 
   /**

spot_hardware_interface/src/spot_hardware_interface.cpp

@@ -51,6 +51,23 @@ void StateStreamingHandler::get_joint_states(JointStates& joint_states) {
   joint_states.load.assign(current_load_.begin(), current_load_.end());
 }
 
+std::vector<float> SpotHardware::parse_gains_parameter(const std::string gains_string,
+                                                       const std::vector<float>& default_gains,
+                                                       const std::string gain_name) {
+  std::istringstream gains_stream(gains_string);
+  std::vector<float> gains((std::istream_iterator<float>(gains_stream)), (std::istream_iterator<float>()));
+  const auto expected_size = default_gains.size();
+  if (gains.size() != expected_size) {
+    if (!gains.empty()) {
+      RCLCPP_WARN(rclcpp::get_logger("SpotHardware"),
+                  "%s has %ld entries, expected %ld. Check your config file! Falling back to default gains.",
+                  gain_name.c_str(), gains.size(), expected_size);
+    }
+    return default_gains;
+  }
+  return gains;
+}
+
 hardware_interface::CallbackReturn SpotHardware::on_init(const hardware_interface::HardwareInfo& info) {
   if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) {
     return hardware_interface::CallbackReturn::ERROR;
@@ -65,6 +82,29 @@ hardware_interface::CallbackReturn SpotHardware::on_init(const hardware_interfac
 
   njoints_ = hw_states_.size() / interfaces_per_joint_;
 
+  // check that the number of joints matches what we expect, and determine default k_q_p/k_qd_p from this
+  std::vector<float> default_k_q_p, default_k_qd_p;
+  if (njoints_ == kNjointsArm) {
+    default_k_q_p.assign(std::begin(kDefaultKqpArm), std::end(kDefaultKqpArm));
+    default_k_qd_p.assign(std::begin(kDefaultKqdpArm), std::end(kDefaultKqdpArm));
+  } else if (njoints_ == kNjointsNoArm) {
+    default_k_q_p.assign(std::begin(kDefaultKqpNoArm), std::end(kDefaultKqpNoArm));
+    default_k_qd_p.assign(std::begin(kDefaultKqdpNoArm), std::end(kDefaultKqdpNoArm));
+  } else {
+    RCLCPP_ERROR(rclcpp::get_logger("SpotHardware"),
+                 "Got %ld joints, expected either %d (Spot with arm) or %d (Spot without arm)!!", njoints_, kNjointsArm,
+                 kNjointsNoArm);
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+
+  // Get the user-passed in k_q_p and k_q_d values.
+  const std::string k_q_p_string = info_.hardware_parameters["k_q_p"];
+  const std::string k_qd_p_string = info_.hardware_parameters["k_qd_p"];
+
+  // Determine the gains that should be used on robot.
+  k_q_p_ = parse_gains_parameter(k_q_p_string, default_k_q_p, "k_q_p");
+  k_qd_p_ = parse_gains_parameter(k_qd_p_string, default_k_qd_p, "k_qd_p");
+
   for (const hardware_interface::ComponentInfo& joint : info_.joints) {
     // Assumes three state and three command interfaces for each joint (position, velocity, and effort).
     if (joint.command_interfaces.size() != interfaces_per_joint_) {
@@ -469,30 +509,10 @@ bool SpotHardware::start_command_stream() {
   // Fill in the parts of the joint streaming command request that are constant.
   auto* joint_cmd = joint_request_.mutable_joint_command();
 
-  std::vector<float> kp;
-  std::vector<float> kd;
-
-  // Assign k values depending on if the robot has an arm or not
-  switch (njoints_) {
-    case spot_hardware_interface::kNjointsArm:
-      kp.assign(std::begin(spot_hardware_interface::kDefaultKpArm), std::end(spot_hardware_interface::kDefaultKpArm));
-      kd.assign(std::begin(spot_hardware_interface::kDefaultKdArm), std::end(spot_hardware_interface::kDefaultKdArm));
-      break;
-    case spot_hardware_interface::kNjointsNoArm:
-      kp.assign(std::begin(spot_hardware_interface::kDefaultKpNoArm),
-                std::end(spot_hardware_interface::kDefaultKpNoArm));
-      kd.assign(std::begin(spot_hardware_interface::kDefaultKdNoArm),
-                std::end(spot_hardware_interface::kDefaultKdNoArm));
-      break;
-    default:
-      RCLCPP_ERROR(rclcpp::get_logger("SpotHardware"), "WRONG # OF JOINTS");
-      return false;
-  }
-
   joint_cmd->mutable_gains()->mutable_k_q_p()->Clear();
-  joint_cmd->mutable_gains()->mutable_k_q_p()->Add(kp.begin(), kp.end());
+  joint_cmd->mutable_gains()->mutable_k_q_p()->Add(k_q_p_.begin(), k_q_p_.end());
   joint_cmd->mutable_gains()->mutable_k_qd_p()->Clear();
-  joint_cmd->mutable_gains()->mutable_k_qd_p()->Add(kd.begin(), kd.end());
+  joint_cmd->mutable_gains()->mutable_k_qd_p()->Add(k_qd_p_.begin(), k_qd_p_.end());
 
   // Let it extrapolate the command a little
   joint_cmd->mutable_extrapolation_duration()->CopyFrom(

spot_ros2_control/README.md

@@ -24,6 +24,14 @@ You can then run the launchfile with the following command:
 ros2 launch spot_ros2_control spot_ros2_control.launch.py hardware_interface:=robot config_file:=path/to/spot_ros.yaml
 ```
 
+Joint level gains can also be specified in this same config file under the parameter names `k_q_p` and `k_qd_p`. If you do not specify these parameters, the default gains from the `spot-sdk` joint control examples are used during command streaming. The example below shows a valid configuration for a robot with an arm as each list contains 19 elements. More information on how these gains are used by Spot can be found [here](https://dev.bostondynamics.com/docs/concepts/joint_control/readme).
+```
+/**:
+  ros__parameters:
+    k_q_p: [624.0, 936.0, 286.0,  624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 1020.0, 255.0, 204.0, 102.0, 102.0, 102.0, 16.0]
+    k_qd_p: [5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 10.2, 15.3, 10.2, 2.04, 2.04, 2.04, 0.32]
+```
+
 If you wish to launch these nodes in a namespace, add the argument `spot_name:=<Robot Name>`.
 
 This hardware interface will stream the joint angles of the robot at 333 Hz onto the topic `/<Robot Name>/low_level/joint_states`.

spot_ros2_control/launch/spot_ros2_control.launch.py

@@ -21,6 +21,7 @@
     IMAGE_PUBLISHER_ARGS,
     declare_image_publisher_args,
     get_login_parameters,
+    get_ros_param_dict,
     spot_has_arm,
 )
 
@@ -104,18 +105,30 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
     spot_name: str = LaunchConfiguration("spot_name").perform(context)
     config_file: str = LaunchConfiguration("config_file").perform(context)
 
-    # If connected to a physical robot, query if it has an arm. Otherwise, use the value in mock_arm.
+    # Default parameters used in the URDF if not connected to a robot
+    arm = mock_arm
+    login_params = ""
+    gain_params = ""
+
+    # If running on robot, query if it has an arm, and parse config for login parameters and gains
     if hardware_interface == "robot":
         arm = spot_has_arm(config_file_path=config_file, spot_name="")
         username, password, hostname = get_login_parameters(config_file)[:3]
-        login_params = f" hostname:={hostname} username:={username} password:={password}"
-    else:
-        arm = mock_arm
-        login_params = ""
+        login_params = f" hostname:={hostname} username:={username} password:={password} "
+        param_dict = get_ros_param_dict(config_file)
+        if "k_q_p" in param_dict:
+            # we pass the gains to the xacro as space-separated strings as the hardware interface needs to read in all
+            # of its hardware parameters as strings, and it is easier to parse them out from the config file here.
+            # eg: k_q_p: [1, 2, 3] in the config file will get translated to the string "1 2 3" here
+            k_q_p = " ".join(map(str, param_dict["k_q_p"]))
+            gain_params += f' k_q_p:="{k_q_p}" '
+        if "k_qd_p" in param_dict:
+            k_qd_p = " ".join(map(str, param_dict["k_qd_p"]))
+            gain_params += f' k_qd_p:="{k_qd_p}" '
 
     tf_prefix = f"{spot_name}/" if spot_name else ""
 
-    # Generate the robot description based off if the robot has an arm.
+    # Generate the robot description containing the ros2 control tags and hardware interface parameters.
     robot_urdf = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
@@ -128,6 +141,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
             " hardware_interface_type:=",
             LaunchConfiguration("hardware_interface"),
             login_params,
+            gain_params,
         ]
     )
     robot_description = {"robot_description": robot_urdf}