From e3992ebaf183eae02b4f08b086b7baf9c8f7e400 Mon Sep 17 00:00:00 2001 From: Katie Hughes <157421702+khughes-bdai@users.noreply.github.com> Date: Tue, 29 Oct 2024 09:41:34 -0400 Subject: [PATCH] [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:=`, these gains will get used in the hardware interface to stream commands. [Because ros2 control hardware interfaces don't have "true" support for parameters, ](https://github.com/ros-controls/ros2_control/issues/347)the 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 | 8 +-- spot_description/urdf/spot.urdf.xacro | 7 +- .../spot_constants.hpp | 28 ++++---- .../spot_hardware_interface.hpp | 16 +++++ .../src/spot_hardware_interface.cpp | 64 ++++++++++++------- spot_ros2_control/README.md | 8 +++ .../launch/spot_ros2_control.launch.py | 26 ++++++-- 7 files changed, 109 insertions(+), 48 deletions(-) diff --git a/spot_description/urdf/spot.ros2_control.xacro b/spot_description/urdf/spot.ros2_control.xacro index 216219c2..2e8d6af7 100644 --- a/spot_description/urdf/spot.ros2_control.xacro +++ b/spot_description/urdf/spot.ros2_control.xacro @@ -20,10 +20,6 @@ ${-effort_max} ${effort_max} - - @@ -65,7 +61,7 @@ - + @@ -80,6 +76,8 @@ $(optenv SPOT_IP ${hostname}) $(optenv BOSDYN_CLIENT_USERNAME ${username}) $(optenv BOSDYN_CLIENT_PASSWORD ${password}) + ${k_q_p} + ${k_qd_p} diff --git a/spot_description/urdf/spot.urdf.xacro b/spot_description/urdf/spot.urdf.xacro index ed0ad8fa..5fb80992 100644 --- a/spot_description/urdf/spot.urdf.xacro +++ b/spot_description/urdf/spot.urdf.xacro @@ -14,10 +14,11 @@ - + + + tf_prefix="$(arg tf_prefix)" + k_q_p="$(arg k_q_p)" + k_qd_p="$(arg k_qd_p)" /> 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 5e1b90f5..92169fe9 100644 --- a/spot_hardware_interface/include/spot_hardware_interface/spot_constants.hpp +++ b/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 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 6731ffcb..360eeece 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 @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -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 k_q_p_; + std::vector 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 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. /** diff --git a/spot_hardware_interface/src/spot_hardware_interface.cpp b/spot_hardware_interface/src/spot_hardware_interface.cpp index e9d7fee9..99bc950a 100644 --- a/spot_hardware_interface/src/spot_hardware_interface.cpp +++ b/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 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; @@ -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 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 kp; - std::vector 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( diff --git a/spot_ros2_control/README.md b/spot_ros2_control/README.md index 99f39f0a..70604be6 100644 --- a/spot_ros2_control/README.md +++ b/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:=`. This hardware interface will stream the joint angles of the robot at 333 Hz onto the topic `//low_level/joint_states`. diff --git a/spot_ros2_control/launch/spot_ros2_control.launch.py b/spot_ros2_control/launch/spot_ros2_control.launch.py index 0d68251c..b5352b78 100644 --- a/spot_ros2_control/launch/spot_ros2_control.launch.py +++ b/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}