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

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_;