Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

state interface fix #186

Merged
merged 9 commits into from
May 31, 2024
Merged

state interface fix #186

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
b1da9a2
Changes made to the local branch for FRI3 compatibility
OmidRezayof May 29, 2024
187da1c
Merge pull request #185 from OmidRezayof/FRI3
mhubii May 30, 2024
a673669
Merge branch 'humble' into dev-humble-state-interface-fix
mhubii May 31, 2024
798f608
simplified versioning
mhubii May 31, 2024
9baf90b
added 2.7 build
mhubii May 31, 2024
62835a7
incorrect controller
mhubii May 31, 2024
5165336
added 2.7 repos yaml
mhubii May 31, 2024
65d6b4c
simplified directive
mhubii May 31, 2024
e9e8271
runtime check for fri client sdk version
mhubii May 31, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion .github/workflows/build.yml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ jobs:
strategy:
matrix:
os: [ubuntu-latest]
fri_version: [1.11, 1.14, 1.15, 1.16, 2.5]
fri_version: [1.11, 1.14, 1.15, 1.16, 2.5, 2.7]
steps:
- uses: ros-tooling/[email protected]
- uses: ros-tooling/[email protected]
Expand Down
2 changes: 1 addition & 1 deletion lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ struct EnumMaps {
case KUKA::FRI::EClientCommandMode::POSITION:
return "POSITION";
#endif
#if FRICLIENT_VERSION_MAJOR == 2
#if FRICLIENT_VERSION_MAJOR >= 2
case KUKA::FRI::EClientCommandMode::JOINT_POSITION:
return "JOINT_POSITION";
case KUKA::FRI::EClientCommandMode::CARTESIAN_POSE:
Expand Down
2 changes: 1 addition & 1 deletion lbr_fri_ros2/src/async_client.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ AsyncClient::AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mod
#if FRICLIENT_VERSION_MAJOR == 1
case KUKA::FRI::EClientCommandMode::POSITION:
#endif
#if FRICLIENT_VERSION_MAJOR == 2
#if FRICLIENT_VERSION_MAJOR >= 2
case KUKA::FRI::EClientCommandMode::JOINT_POSITION:
#endif
{
Expand Down
2 changes: 1 addition & 1 deletion lbr_fri_ros2/src/interfaces/position_command.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command
throw std::runtime_error(err);
}
#endif
#if FRICLIENT_VERSION_MAJOR == 2
#if FRICLIENT_VERSION_MAJOR >= 2
if (state.client_command_mode != KUKA::FRI::EClientCommandMode::JOINT_POSITION) {
std::string err =
"Expected robot in " +
Expand Down
4 changes: 2 additions & 2 deletions lbr_fri_ros2/test/test_command_interfaces.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ class TestCommandInterfaces : public ::testing::Test {
#if FRICLIENT_VERSION_MAJOR == 1
case KUKA::FRI::EClientCommandMode::POSITION:
#endif
#if FRICLIENT_VERSION_MAJOR == 2
#if FRICLIENT_VERSION_MAJOR >= 2
case KUKA::FRI::EClientCommandMode::JOINT_POSITION:
#endif
{
Expand Down Expand Up @@ -146,7 +146,7 @@ TEST_F(TestCommandInterfaces, TestPositionCommandInterface) {
#if FRICLIENT_VERSION_MAJOR == 1
set_up(KUKA::FRI::EClientCommandMode::POSITION);
#endif
#if FRICLIENT_VERSION_MAJOR == 2
#if FRICLIENT_VERSION_MAJOR >= 2
set_up(KUKA::FRI::EClientCommandMode::JOINT_POSITION);
#endif
test_simple();
Expand Down
13 changes: 13 additions & 0 deletions lbr_fri_ros2_stack/repos-fri-2.7.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
repositories:
fri:
type: git
url: https://github.com/lbr-stack/fri
version: fri-2.7
lbr_fri_idl:
type: git
url: https://github.com/lbr-stack/lbr_fri_idl
version: fri-2
lbr_fri_ros2_stack:
type: git
url: https://github.com/lbr-stack/lbr_fri_ros2_stack
version: humble
6 changes: 5 additions & 1 deletion lbr_ros2_control/config/lbr_system_interface.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@
<xacro:unless value="${sim}">
<hardware>
<plugin>lbr_ros2_control::SystemInterface</plugin>
<param name="fri_client_sdk_major_version">${system_parameters['hardware']['fri_client_sdk']['major_version']}</param>
<param name="fri_client_sdk_minor_version">${system_parameters['hardware']['fri_client_sdk']['minor_version']}</param>
<param name="client_command_mode">${system_parameters['hardware']['client_command_mode']}</param>
<param name="port_id">${system_parameters['hardware']['port_id']}</param>
<param name="remote_host">${system_parameters['hardware']['remote_host']}</param>
Expand Down Expand Up @@ -103,7 +105,9 @@
<state_interface name="velocity" />
<state_interface name="effort" />
<xacro:unless value="${sim}">
<state_interface name="commanded_joint_position" />
<xacro:if value="${system_parameters['hardware']['fri_client_sdk']['major_version'] == 1}">
<state_interface name="commanded_joint_position" />
</xacro:if>
<state_interface name="commanded_torque" />
<state_interface name="external_torque" />
<state_interface name="ipo_joint_position" />
Expand Down
5 changes: 4 additions & 1 deletion lbr_ros2_control/config/lbr_system_parameters.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,8 @@
# these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface
hardware:
fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro
major_version: 1
minor_version: 15
client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench]
port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups
remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection
Expand All @@ -11,7 +14,7 @@ hardware:
pid_i_min: 0.0 # min integral value for the joint position command
pid_antiwindup: false # enable antiwindup for the joint position command
command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop]
external_torque_cutoff_frequency: 10 # low-pass filter for the external joint torque measurements [Hz]
external_torque_cutoff_frequency: 10 # low-pass filter for the external joint torque measurements [Hz]
measured_torque_cutoff_frequency: 10 # low-pass filter for the joint torque measurements [Hz]
open_loop: true # KUKA works the best in open_loop control mode

Expand Down
9 changes: 8 additions & 1 deletion lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,10 +31,12 @@

namespace lbr_ros2_control {
struct SystemInterfaceParameters {
uint8_t fri_client_sdk_major_version{1};
uint8_t fri_client_sdk_minor_version{15};
#if FRICLIENT_VERSION_MAJOR == 1
KUKA::FRI::EClientCommandMode client_command_mode{KUKA::FRI::EClientCommandMode::POSITION};
#endif
#if FRICLIENT_VERSION_MAJOR == 2
#if FRICLIENT_VERSION_MAJOR >= 2
KUKA::FRI::EClientCommandMode client_command_mode{KUKA::FRI::EClientCommandMode::JOINT_POSITION};
#endif
int32_t port_id{30200};
Expand Down Expand Up @@ -68,7 +70,12 @@ class SystemInterface : public hardware_interface::SystemInterface {
protected:
static constexpr char LOGGER_NAME[] = "lbr_ros2_control::SystemInterface";

#if FRICLIENT_VERSION_MAJOR == 1
static constexpr uint8_t LBR_FRI_STATE_INTERFACE_SIZE = 7;
#endif
#if FRICLIENT_VERSION_MAJOR >= 2
static constexpr uint8_t LBR_FRI_STATE_INTERFACE_SIZE = 6;
#endif
static constexpr uint8_t LBR_FRI_COMMAND_INTERFACE_SIZE = 2;
static constexpr uint8_t LBR_FRI_SENSORS = 2;
static constexpr uint8_t AUXILIARY_SENSOR_SIZE = 12;
Expand Down
16 changes: 15 additions & 1 deletion lbr_ros2_control/src/system_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -320,12 +320,26 @@ hardware_interface::return_type SystemInterface::write(const rclcpp::Time & /*ti

bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo &system_info) {
try {
parameters_.fri_client_sdk_major_version =
std::stoul(system_info.hardware_parameters.at("fri_client_sdk_major_version"));
parameters_.fri_client_sdk_minor_version =
std::stoul(system_info.hardware_parameters.at("fri_client_sdk_minor_version"));
if (parameters_.fri_client_sdk_major_version != FRICLIENT_VERSION_MAJOR) {
RCLCPP_ERROR_STREAM(
rclcpp::get_logger(LOGGER_NAME),
lbr_fri_ros2::ColorScheme::ERROR
<< "Expected FRI client SDK version '" << FRICLIENT_VERSION_MAJOR << "', got '"
<< std::to_string(parameters_.fri_client_sdk_major_version)
<< "'. Update lbr_system_parameters.yaml or compile against correct FRI version."
<< lbr_fri_ros2::ColorScheme::ENDC);
return false;
}
std::string client_command_mode = system_info.hardware_parameters.at("client_command_mode");
if (client_command_mode == "position") {
#if FRICLIENT_VERSION_MAJOR == 1
parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::POSITION;
#endif
#if FRICLIENT_VERSION_MAJOR == 2
#if FRICLIENT_VERSION_MAJOR >= 2
parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::JOINT_POSITION;
#endif
} else if (client_command_mode == "torque") {
Expand Down
Loading