From a80e374cf77f3cd66d9d1bcc939ef1ba0992740b Mon Sep 17 00:00:00 2001 From: Katie Hughes Date: Thu, 20 Feb 2025 15:23:06 -0500 Subject: [PATCH] first commit --- spot_description | 2 +- .../spot_hardware_interface.hpp | 27 ++++-- .../src/spot_hardware_interface.cpp | 88 ++++++++++++------- spot_msgs/CMakeLists.txt | 1 + spot_msgs/msg/JointCommand.msg | 12 +++ .../launch/spot_ros2_control.launch.py | 88 +++++++++---------- 6 files changed, 134 insertions(+), 84 deletions(-) create mode 100644 spot_msgs/msg/JointCommand.msg diff --git a/spot_description b/spot_description index 37ecc6a55..a7158906b 160000 --- a/spot_description +++ b/spot_description @@ -1 +1 @@ -Subproject commit 37ecc6a55dd8bc874fa4e80376d838fec33a167b +Subproject commit a7158906bc34256e1982e34ae7adfdb330440043 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..a36dd4709 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 command_states_; // Thread for reading the state of the robot. std::jthread state_thread_; @@ -235,7 +252,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..a61de2e04 100644 --- a/spot_hardware_interface/src/spot_hardware_interface.cpp +++ b/spot_hardware_interface/src/spot_hardware_interface.cpp @@ -77,10 +77,10 @@ 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_; + njoints_ = hw_states_.size() / state_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; @@ -106,49 +106,61 @@ hardware_interface::CallbackReturn SpotHardware::on_init(const hardware_interfac 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_) { + // 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); @@ -214,11 +226,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 +239,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 + 3])); } return command_interfaces; } @@ -269,9 +285,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,9 +295,9 @@ 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 @@ -301,9 +317,11 @@ 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]; + command_states_.position.at(i) = hw_commands_[command_interfaces_per_joint_ * i]; + command_states_.velocity.at(i) = hw_commands_[command_interfaces_per_joint_ * i + 1]; + command_states_.load.at(i) = hw_commands_[command_interfaces_per_joint_ * i + 2]; + command_states_.k_q_p.at(i) = hw_commands_[command_interfaces_per_joint_ * i + 3]; + command_states_.k_qd_p.at(i) = hw_commands_[command_interfaces_per_joint_ * i + 4]; } send_command(command_states_); @@ -535,10 +553,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 +569,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(); diff --git a/spot_msgs/CMakeLists.txt b/spot_msgs/CMakeLists.txt index b3d2a9ec7..311f3465f 100644 --- a/spot_msgs/CMakeLists.txt +++ b/spot_msgs/CMakeLists.txt @@ -52,6 +52,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/EStopState.msg" "msg/Feedback.msg" "msg/FootState.msg" + "msg/JointCommand.msg" "msg/Lease.msg" "msg/LeaseResource.msg" "msg/PowerState.msg" diff --git a/spot_msgs/msg/JointCommand.msg b/spot_msgs/msg/JointCommand.msg new file mode 100644 index 000000000..42ef8d61d --- /dev/null +++ b/spot_msgs/msg/JointCommand.msg @@ -0,0 +1,12 @@ +# list of the joint names to control +string[] name +# desired position command for each joint +float64[] position +# desired velocity command for each joint +float64[] velocity +# desired effort command for each joint +float64[] effort +# desired k_q_p command for each joint +float64[] k_q_p +# desired k_qd_p command for each joint +float64[] k_qd_p diff --git a/spot_ros2_control/launch/spot_ros2_control.launch.py b/spot_ros2_control/launch/spot_ros2_control.launch.py index 0d55148a4..bf5d2b7a2 100644 --- a/spot_ros2_control/launch/spot_ros2_control.launch.py +++ b/spot_ros2_control/launch/spot_ros2_control.launch.py @@ -5,9 +5,8 @@ import yaml from ament_index_python.packages import get_package_share_directory from launch import LaunchContext, LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import ( Command, FindExecutable, @@ -19,11 +18,8 @@ from synchros2.launch.actions import DeclareBooleanLaunchArgument from spot_driver.launch.spot_launch_helpers import ( - IMAGE_PUBLISHER_ARGS, declare_image_publisher_args, - get_login_parameters, get_ros_param_dict, - spot_has_arm, ) THIS_PACKAGE = "spot_ros2_control" @@ -113,8 +109,8 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: # 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] + arm = True # spot_has_arm(config_file_path=config_file, spot_name="") + username, password, hostname = "aaaaa", "bbbbb", "ccccc" # get_login_parameters(config_file)[:3] login_params = f" hostname:={hostname} username:={username} password:={password} " param_dict = get_ros_param_dict(config_file) if "k_q_p" in param_dict: @@ -202,45 +198,45 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: namespace=spot_name, ) ) - # Finally, launch extra nodes for state and image publishing if we are running on a robot. - if hardware_interface == "robot": - # launch image publishers - ld.add_action( - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [ - PathJoinSubstitution( - [FindPackageShare("spot_driver"), "launch", "spot_image_publishers.launch.py"] - ) - ] - ), - launch_arguments={ - key: LaunchConfiguration(key) for key in ["config_file", "spot_name"] + IMAGE_PUBLISHER_ARGS - }.items(), - condition=IfCondition(LaunchConfiguration("launch_image_publishers")), - ) - ) - # launch state publisher node (useful for publishing odom & other statuses) - ld.add_action( - Node( - package="spot_driver", - executable="state_publisher_node", - output="screen", - parameters=[config_file, {"spot_name": spot_name}], - namespace=spot_name, - ) - ) - # launch object sync node (for fiducials) - ld.add_action( - Node( - package="spot_driver", - executable="object_synchronizer_node", - output="screen", - parameters=[config_file, {"spot_name": spot_name}], - namespace=spot_name, - ) - ) - return + # # Finally, launch extra nodes for state and image publishing if we are running on a robot. + # if hardware_interface == "robot": + # # launch image publishers + # ld.add_action( + # IncludeLaunchDescription( + # PythonLaunchDescriptionSource( + # [ + # PathJoinSubstitution( + # [FindPackageShare("spot_driver"), "launch", "spot_image_publishers.launch.py"] + # ) + # ] + # ), + # launch_arguments={ + # key: LaunchConfiguration(key) for key in ["config_file", "spot_name"] + IMAGE_PUBLISHER_ARGS + # }.items(), + # condition=IfCondition(LaunchConfiguration("launch_image_publishers")), + # ) + # ) + # # launch state publisher node (useful for publishing odom & other statuses) + # ld.add_action( + # Node( + # package="spot_driver", + # executable="state_publisher_node", + # output="screen", + # parameters=[config_file, {"spot_name": spot_name}], + # namespace=spot_name, + # ) + # ) + # # launch object sync node (for fiducials) + # ld.add_action( + # Node( + # package="spot_driver", + # executable="object_synchronizer_node", + # output="screen", + # parameters=[config_file, {"spot_name": spot_name}], + # namespace=spot_name, + # ) + # ) + # return def generate_launch_description():