[SW-2049] ability to stream joint gains through hardware interface (#588

)

## Change Overview

Adds k_q_p and k_qd_p to the command interface of Spot's hardware interface.These get set to an initial value via the `initial_value` field from the URDF, so existing logic here in the hardware interface to parse this input has been removed in favor of this new method. Existing controllers will not need to change anything, as they simply don't claim the interface. 

The following PR in the stack will set up a controller to allow users to easily change this at runtime.

This PR also depends on the changes from bdaiinstitute/spot_description#3 to expose these new command interfaces in the URDF. 

## Testing Done
The default behavior from the user interface does not change with this PR.

- [x] Tested wiggle arm example successfully on robot with default gains
- [x] Tested wiggle arm example with different initial gains, verified that robot behavior adjusts accordingly
\\

spot_hardware_interface/include/spot_hardware_interface/spot_constants.hpp

@@ -26,24 +26,4 @@ inline constexpr int kNjointsNoArm = 12;
 /// @brief Number of joints we expect if the robot has an arm
 inline constexpr int kNjointsArm = 19;
 
-// Default gain values obtained from
-// https://github.com/boston-dynamics/spot-cpp-sdk/blob/master/cpp/examples/joint_control/constants.hpp
-
-/// @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 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 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 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

@@ -62,6 +62,19 @@ struct JointStates {
   std::vector<float> load;      // in Nm
 };
 
+struct JointCommands {
+  // This struct is used to hold a set of joint commands of the robot.
+  // The first 12 entries will be the leg commands in the following order:
+  // FL hip x, FL hip y, FL knee, FR hip x, FR hip y, FR knee, RL hip x, RL hip y, RL knee, RR hip x, RR hip y, RR knee
+  // And, if the robot has an arm, the 7 arm commands follow in this order:
+  // sh0, sh1, el0, el1, wr0, wr1, f1x
+  std::vector<float> position;  // in rad
+  std::vector<float> velocity;  // in rad/s
+  std::vector<float> load;      // in Nm
+  std::vector<float> k_q_p;
+  std::vector<float> k_qd_p;
+};
+
 class StateStreamingHandler {
  public:
   /**
@@ -119,9 +132,13 @@ class SpotHardware : public hardware_interface::SystemInterface {
   hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override;
 
  private:
-  // TODO(khughes): Hard coding this for now, but there should be a cleaner way to do this.
-  // The 3 interfaces are position, velocity, and effort.
-  static constexpr size_t interfaces_per_joint_ = 3;
+  // constants used for expanding the available command interfaces
+  const std::string HW_IF_K_Q_P = "k_q_p";
+  const std::string HW_IF_K_QD_P = "k_qd_p";
+  // The 3 command interfaces are position, velocity, effort, k_q_p, and k_qd_p.
+  static constexpr size_t command_interfaces_per_joint_ = 5;
+  // The 3 state interfaces are position, velocity, and effort.
+  static constexpr size_t state_interfaces_per_joint_ = 3;
   size_t njoints_;
 
   // Login info
@@ -146,7 +163,7 @@ class SpotHardware : public hardware_interface::SystemInterface {
   // Holds joint states of the robot received from the BD SDK
   JointStates joint_states_;
   // Holds joint commands for the robot to send to BD SDK
-  JointStates command_states_;
+  JointCommands joint_commands_;
 
   // Thread for reading the state of the robot.
   std::jthread state_thread_;
@@ -162,17 +179,6 @@ 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.
 
   /**
@@ -235,7 +241,7 @@ class SpotHardware : public hardware_interface::SystemInterface {
    * @brief Send a joint command to the robot.
    * @param joint_commands contains position, velocity, and load
    */
-  void send_command(const JointStates& joint_commands);
+  void send_command(const JointCommands& joint_commands);
 
   // Vectors for storing the commands and states for the robot.
   std::vector<double> hw_commands_;