Skip to content

Commit

Permalink
first commit
Browse files Browse the repository at this point in the history
  • Loading branch information
Katie Hughes committed Feb 21, 2025
1 parent 702c2fa commit a80e374
Show file tree
Hide file tree
Showing 6 changed files with 134 additions and 84 deletions.
2 changes: 1 addition & 1 deletion spot_description
Submodule spot_description updated 2 files
+20 −14 spot_description/urdf/spot.ros2_control.xacro
+7 −5 spot_description/urdf/spot.urdf.xacro
27 changes: 22 additions & 5 deletions spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
/**
Expand Down Expand Up @@ -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
Expand All @@ -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_;
Expand Down Expand Up @@ -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<double> hw_commands_;
Expand Down
88 changes: 56 additions & 32 deletions spot_hardware_interface/src/spot_hardware_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double>::quiet_NaN());
hw_commands_.resize(info_.joints.size() * interfaces_per_joint_, std::numeric_limits<double>::quiet_NaN());
hw_states_.resize(info_.joints.size() * state_interfaces_per_joint_, std::numeric_limits<double>::quiet_NaN());
hw_commands_.resize(info_.joints.size() * command_interfaces_per_joint_, std::numeric_limits<double>::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<float> default_k_q_p, default_k_qd_p;
Expand All @@ -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);
Expand Down Expand Up @@ -214,11 +226,11 @@ std::vector<hardware_interface::StateInterface> 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;
}
Expand All @@ -227,12 +239,16 @@ std::vector<hardware_interface::CommandInterface> SpotHardware::export_command_i
std::vector<hardware_interface::CommandInterface> 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;
}
Expand Down Expand Up @@ -269,19 +285,19 @@ 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!");
return hardware_interface::return_type::ERROR;
}
// 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
Expand All @@ -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_);

Expand Down Expand Up @@ -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<float>& position = joint_commands.position;
const std::vector<float>& velocity = joint_commands.velocity;
const std::vector<float>& load = joint_commands.load;
const std::vector<float>& k_q_p = joint_commands.k_q_p;
const std::vector<float>& k_qd_p = joint_commands.k_qd_p;

// build protobuf
auto* joint_cmd = joint_request_.mutable_joint_command();
Expand All @@ -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();
Expand Down
1 change: 1 addition & 0 deletions spot_msgs/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
12 changes: 12 additions & 0 deletions spot_msgs/msg/JointCommand.msg
View file
Original file line number Diff line number Diff line change
@@ -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
88 changes: 42 additions & 46 deletions spot_ros2_control/launch/spot_ros2_control.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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"
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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():
Expand Down

0 comments on commit a80e374

Please sign in to comment.