From 37de15b6061734bbd248461ac1ab82abde57cb08 Mon Sep 17 00:00:00 2001 From: Katie Hughes <157421702+khughes-bdai@users.noreply.github.com> Date: Tue, 25 Feb 2025 12:48:55 -0500 Subject: [PATCH] [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 https://github.com/bdaiinstitute/spot_description/pull/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_description | 2 +- .../spot_constants.hpp | 20 --- .../spot_hardware_interface.hpp | 38 +++-- .../src/spot_hardware_interface.cpp | 151 +++++++++--------- 4 files changed, 96 insertions(+), 115 deletions(-) diff --git a/spot_description b/spot_description index 37ecc6a55..19e9653b0 160000 --- a/spot_description +++ b/spot_description @@ -1 +1 @@ -Subproject commit 37ecc6a55dd8bc874fa4e80376d838fec33a167b +Subproject commit 19e9653b0be81091120827a4676b14df957d3f3e diff --git a/spot_hardware_interface/include/spot_hardware_interface/spot_constants.hpp b/spot_hardware_interface/include/spot_hardware_interface/spot_constants.hpp index 92169fe99..e678eb566 100644 --- a/spot_hardware_interface/include/spot_hardware_interface/spot_constants.hpp +++ b/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 diff --git a/spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp b/spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp index 360eeece5..83b827add 100644 --- a/spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp +++ b/spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp @@ -62,6 +62,19 @@ struct JointStates { std::vector 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 position; // in rad + std::vector velocity; // in rad/s + std::vector load; // in Nm + std::vector k_q_p; + std::vector 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 parse_gains_parameter(const std::string gains_string, const std::vector& 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 hw_commands_; diff --git a/spot_hardware_interface/src/spot_hardware_interface.cpp b/spot_hardware_interface/src/spot_hardware_interface.cpp index 62c61b8cc..05103a4ec 100644 --- a/spot_hardware_interface/src/spot_hardware_interface.cpp +++ b/spot_hardware_interface/src/spot_hardware_interface.cpp @@ -51,23 +51,6 @@ void StateStreamingHandler::get_joint_states(JointStates& joint_states) { joint_states.load.assign(current_load_.begin(), current_load_.end()); } -std::vector SpotHardware::parse_gains_parameter(const std::string gains_string, - const std::vector& default_gains, - const std::string gain_name) { - std::istringstream gains_stream(gains_string); - std::vector gains((std::istream_iterator(gains_stream)), (std::istream_iterator())); - 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; @@ -77,78 +60,67 @@ hardware_interface::CallbackReturn SpotHardware::on_init(const hardware_interfac username_ = info_.hardware_parameters["username"]; password_ = info_.hardware_parameters["password"]; - hw_states_.resize(info_.joints.size() * interfaces_per_joint_, std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size() * interfaces_per_joint_, std::numeric_limits::quiet_NaN()); + hw_states_.resize(info_.joints.size() * state_interfaces_per_joint_, std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size() * command_interfaces_per_joint_, std::numeric_limits::quiet_NaN()); - 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 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"); + njoints_ = hw_states_.size() / state_interfaces_per_joint_; 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_) { + // First check command interfaces + if (joint.command_interfaces.size() != command_interfaces_per_joint_) { RCLCPP_FATAL(rclcpp::get_logger("SpotHardware"), "Joint '%s' has %zu command interfaces found. 3 expected.", joint.name.c_str(), joint.command_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } - + // 1. check position if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL(rclcpp::get_logger("SpotHardware"), "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } - + // 2. check velocity if (joint.command_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) { RCLCPP_FATAL(rclcpp::get_logger("SpotHardware"), "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); return hardware_interface::CallbackReturn::ERROR; } - + // 3. check effort if (joint.command_interfaces[2].name != hardware_interface::HW_IF_EFFORT) { RCLCPP_FATAL(rclcpp::get_logger("SpotHardware"), "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_EFFORT); return hardware_interface::CallbackReturn::ERROR; } - - if (joint.state_interfaces.size() != interfaces_per_joint_) { + // 4. check k_q_p + if (joint.command_interfaces[3].name != HW_IF_K_Q_P) { + RCLCPP_FATAL(rclcpp::get_logger("SpotHardware"), "Joint '%s' have %s command interfaces found. '%s' expected.", + joint.name.c_str(), joint.command_interfaces[0].name.c_str(), HW_IF_K_Q_P.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + // 5. check k_qd_p + if (joint.command_interfaces[4].name != HW_IF_K_QD_P) { + RCLCPP_FATAL(rclcpp::get_logger("SpotHardware"), "Joint '%s' have %s command interfaces found. '%s' expected.", + joint.name.c_str(), joint.command_interfaces[0].name.c_str(), HW_IF_K_QD_P.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + // Second, check state interfaces + if (joint.state_interfaces.size() != state_interfaces_per_joint_) { RCLCPP_FATAL(rclcpp::get_logger("SpotHardware"), "Joint '%s' has %zu state interface. 3 expected.", joint.name.c_str(), joint.state_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } - + // 1. check position if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL(rclcpp::get_logger("SpotHardware"), "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } - + // 2. check velocity if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) { RCLCPP_FATAL(rclcpp::get_logger("SpotHardware"), "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); return hardware_interface::CallbackReturn::ERROR; } - + // 3. check effort if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT) { RCLCPP_FATAL(rclcpp::get_logger("SpotHardware"), "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_EFFORT); @@ -202,9 +174,11 @@ hardware_interface::CallbackReturn SpotHardware::on_activate(const rclcpp_lifecy } // Set up command_states struct, initialized to zeros - command_states_.position = std::vector(info_.joints.size(), 0.0); - command_states_.velocity = std::vector(info_.joints.size(), 0.0); - command_states_.load = std::vector(info_.joints.size(), 0.0); + joint_commands_.position = std::vector(info_.joints.size(), 0.0); + joint_commands_.velocity = std::vector(info_.joints.size(), 0.0); + joint_commands_.load = std::vector(info_.joints.size(), 0.0); + joint_commands_.k_q_p = std::vector(info_.joints.size(), 0.0); + joint_commands_.k_qd_p = std::vector(info_.joints.size(), 0.0); return hardware_interface::CallbackReturn::SUCCESS; } @@ -214,11 +188,11 @@ std::vector SpotHardware::export_state_inter for (size_t i = 0; i < info_.joints.size(); i++) { const auto& joint = info_.joints.at(i); state_interfaces.emplace_back(hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_POSITION, - &hw_states_[interfaces_per_joint_ * i])); + &hw_states_[state_interfaces_per_joint_ * i])); state_interfaces.emplace_back(hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_VELOCITY, - &hw_states_[interfaces_per_joint_ * i + 1])); + &hw_states_[state_interfaces_per_joint_ * i + 1])); state_interfaces.emplace_back(hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_EFFORT, - &hw_states_[interfaces_per_joint_ * i + 2])); + &hw_states_[state_interfaces_per_joint_ * i + 2])); } return state_interfaces; } @@ -227,12 +201,16 @@ std::vector SpotHardware::export_command_i std::vector command_interfaces; for (uint i = 0; i < info_.joints.size(); i++) { const auto& joint = info_.joints.at(i); - command_interfaces.emplace_back(hardware_interface::CommandInterface(joint.name, hardware_interface::HW_IF_POSITION, - &hw_commands_[interfaces_per_joint_ * i])); - command_interfaces.emplace_back(hardware_interface::CommandInterface(joint.name, hardware_interface::HW_IF_VELOCITY, - &hw_commands_[interfaces_per_joint_ * i + 1])); - command_interfaces.emplace_back(hardware_interface::CommandInterface(joint.name, hardware_interface::HW_IF_EFFORT, - &hw_commands_[interfaces_per_joint_ * i + 2])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint.name, hardware_interface::HW_IF_POSITION, &hw_commands_[command_interfaces_per_joint_ * i])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint.name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[command_interfaces_per_joint_ * i + 1])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint.name, hardware_interface::HW_IF_EFFORT, &hw_commands_[command_interfaces_per_joint_ * i + 2])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint.name, HW_IF_K_Q_P, &hw_commands_[command_interfaces_per_joint_ * i + 3])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint.name, HW_IF_K_QD_P, &hw_commands_[command_interfaces_per_joint_ * i + 4])); } return command_interfaces; } @@ -269,9 +247,9 @@ hardware_interface::return_type SpotHardware::read(const rclcpp::Time& /*time*/, } // Ensure that the states received from the Spot SDK will fit into the hw_states_ vector const auto states_size = hw_states_.size(); - if (interfaces_per_joint_ * joint_pos.size() != states_size || - interfaces_per_joint_ * joint_vel.size() != states_size || - interfaces_per_joint_ * joint_load.size() != states_size) { + if (state_interfaces_per_joint_ * joint_pos.size() != states_size || + state_interfaces_per_joint_ * joint_vel.size() != states_size || + state_interfaces_per_joint_ * joint_load.size() != states_size) { RCLCPP_FATAL( rclcpp::get_logger("SpotHardware"), "The number of joints and interfaces does not match with the outputted joint states from the Spot SDK!"); @@ -279,14 +257,23 @@ hardware_interface::return_type SpotHardware::read(const rclcpp::Time& /*time*/, } // Read values into joint states for (size_t i = 0; i < joint_pos.size(); ++i) { - hw_states_.at(i * interfaces_per_joint_) = joint_pos.at(i); - hw_states_.at(i * interfaces_per_joint_ + 1) = joint_vel.at(i); - hw_states_.at(i * interfaces_per_joint_ + 2) = joint_load.at(i); + hw_states_.at(i * state_interfaces_per_joint_) = joint_pos.at(i); + hw_states_.at(i * state_interfaces_per_joint_ + 1) = joint_vel.at(i); + hw_states_.at(i * state_interfaces_per_joint_ + 2) = joint_load.at(i); } - // Set initial command values to current state + // Fill in the initial command values if (!init_state_) { - hw_commands_ = hw_states_; + for (size_t i = 0; i < njoints_; i++) { + // copy over current position, velocity, and load from state to current command + hw_commands_[command_interfaces_per_joint_ * i] = hw_states_[state_interfaces_per_joint_ * i]; + hw_commands_[command_interfaces_per_joint_ * i + 1] = hw_states_[state_interfaces_per_joint_ * i + 1]; + hw_commands_[command_interfaces_per_joint_ * i + 2] = hw_states_[state_interfaces_per_joint_ * i + 2]; + // Fill in k_q_p and k_qd_p gains from the initial_value field from the URDF + const auto& joint = info_.joints.at(i); + hw_commands_[command_interfaces_per_joint_ * i + 3] = std::stof(joint.command_interfaces.at(3).initial_value); + hw_commands_[command_interfaces_per_joint_ * i + 4] = std::stof(joint.command_interfaces.at(4).initial_value); + } init_state_ = true; } @@ -301,11 +288,13 @@ hardware_interface::return_type SpotHardware::write(const rclcpp::Time& /*time*/ } for (std::size_t i = 0; i < info_.joints.size(); ++i) { - command_states_.position.at(i) = hw_commands_[interfaces_per_joint_ * i]; - command_states_.velocity.at(i) = hw_commands_[interfaces_per_joint_ * i + 1]; - command_states_.load.at(i) = hw_commands_[interfaces_per_joint_ * i + 2]; + joint_commands_.position.at(i) = hw_commands_[command_interfaces_per_joint_ * i]; + joint_commands_.velocity.at(i) = hw_commands_[command_interfaces_per_joint_ * i + 1]; + joint_commands_.load.at(i) = hw_commands_[command_interfaces_per_joint_ * i + 2]; + joint_commands_.k_q_p.at(i) = hw_commands_[command_interfaces_per_joint_ * i + 3]; + joint_commands_.k_qd_p.at(i) = hw_commands_[command_interfaces_per_joint_ * i + 4]; } - send_command(command_states_); + send_command(joint_commands_); return hardware_interface::return_type::OK; } @@ -535,10 +524,12 @@ void SpotHardware::stop_command_stream() { command_stream_started_ = false; } -void SpotHardware::send_command(const JointStates& joint_commands) { +void SpotHardware::send_command(const JointCommands& joint_commands) { const std::vector& position = joint_commands.position; const std::vector& velocity = joint_commands.velocity; const std::vector& load = joint_commands.load; + const std::vector& k_q_p = joint_commands.k_q_p; + const std::vector& k_qd_p = joint_commands.k_qd_p; // build protobuf auto* joint_cmd = joint_request_.mutable_joint_command(); @@ -549,6 +540,10 @@ void SpotHardware::send_command(const JointStates& joint_commands) { joint_cmd->mutable_velocity()->Add(velocity.begin(), velocity.end()); joint_cmd->mutable_load()->Clear(); joint_cmd->mutable_load()->Add(load.begin(), load.end()); + joint_cmd->mutable_gains()->mutable_k_q_p()->Clear(); + 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(k_qd_p.begin(), k_qd_p.end()); if (endpoint_ == nullptr) { auto endpoint_result = robot_->StartTimeSyncAndGetEndpoint();