From 5e7a8e18a2e73acae432f642f7d1ce1a034d0943 Mon Sep 17 00:00:00 2001 From: Katie Hughes Date: Tue, 25 Feb 2025 15:34:51 -0500 Subject: [PATCH] Maybe working --- .../src/spot_forward_controller.cpp | 56 +++++++++++++------ 1 file changed, 39 insertions(+), 17 deletions(-) diff --git a/spot_controllers/src/spot_forward_controller.cpp b/spot_controllers/src/spot_forward_controller.cpp index 380a1653b..950909b21 100644 --- a/spot_controllers/src/spot_forward_controller.cpp +++ b/spot_controllers/src/spot_forward_controller.cpp @@ -63,8 +63,9 @@ controller_interface::InterfaceConfiguration SpotForwardController::command_inte controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; for (const auto& joint : params_.joints) { - const auto interface_name = joint + "/" + "position"; - command_interfaces_config.names.push_back(interface_name); + // these are the interfaces that get claimed + command_interfaces_config.names.push_back(joint + "/" + "position"); + command_interfaces_config.names.push_back(joint + "/" + "velocity"); } return command_interfaces_config; @@ -111,21 +112,42 @@ controller_interface::return_type SpotForwardController::update(const rclcpp::Ti if (!joint_commands || !(*joint_commands)) { return controller_interface::return_type::OK; } - - // fix this for new command type - // if ((*joint_commands)->data.size() != command_interfaces_.size()) - // { - // RCLCPP_ERROR_THROTTLE( - // get_node()->get_logger(), *(get_node()->get_clock()), 1000, - // "command size (%zu) does not match number of interfaces (%zu)", - // (*joint_commands)->data.size(), command_interfaces_.size()); - // return controller_interface::return_type::ERROR; - // } - - // for (auto index = 0ul; index < command_interfaces_.size(); ++index) - // { - // command_interfaces_[index].set_value((*joint_commands)->data[index]); - // } + // command_interfaces_.at(i).get_name() is full name i.e. arm_sh0/position + // .get_interface_name() is just interface i.e. position + // .get_prefix_name() is the joint name i.e. arm_sh0 + + const auto& joint_names = (*joint_commands)->name; + const auto& njoints_to_command = joint_names.size(); + + const bool using_position = (*joint_commands)->position.size() == njoints_to_command; + const bool using_velocity = (*joint_commands)->velocity.size() == njoints_to_command; + const bool using_effort = (*joint_commands)->effort.size() == njoints_to_command; + + for (size_t i = 0; i < command_interfaces_.size(); i++) { + // idea: for every element in the command interface: + // first tell if the name is in the name field of the message + // if it is, set the interfaces accordingly + const auto& joint_name = command_interfaces_.at(i).get_prefix_name(); + const auto& interface_name = command_interfaces_.at(i).get_interface_name(); + // check if joint_name in name field of message + const auto& it = std::find(joint_names.begin(), joint_names.end(), joint_name); + if (it == joint_names.end()) { + // it wasn't here, keep looping + continue; + } + // get the index that the name is at in the command + const auto command_index = std::distance(joint_names.begin(), it); + // check if interface name is there and update accordingly + if (interface_name == "position" && using_position) { + command_interfaces_.at(i).set_value((*joint_commands)->position.at(command_index)); + } + if (interface_name == "velocity" && using_velocity) { + command_interfaces_.at(i).set_value((*joint_commands)->position.at(command_index)); + } + if (interface_name == "effort" && using_effort) { + command_interfaces_.at(i).set_value((*joint_commands)->effort.at(command_index)); + } + } return controller_interface::return_type::OK; }