Skip to content

Commit

Permalink
Generic_system: Use mimic info from hardware_info
Browse files Browse the repository at this point in the history
  • Loading branch information
@christophfroehlich
christophfroehlich committed Dec 28, 2023
1 parent 8ce02e6 commit 0f1cbfc
Show file tree
Hide file tree
Showing 6 changed files with 212 additions and 88 deletions.
33 changes: 33 additions & 0 deletions hardware_interface/include/hardware_interface/tools.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
// Copyright 2023 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef HARDWARE_INTERFACE__TOOLS_HPP_
#define HARDWARE_INTERFACE__TOOLS_HPP_

#include <charconv>
#include <string>

double parse_double(const std::string & text)
{
double result_value;
const auto parse_result = std::from_chars(text.data(), text.data() + text.size(), result_value);
if (parse_result.ec == std::errc())
{
return result_value;
}

return 0.0;
}

#endif // HARDWARE_INTERFACE__TOOLS_HPP_
8 changes: 0 additions & 8 deletions hardware_interface/include/mock_components/generic_system.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,14 +72,6 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste
hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY,
hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT};

struct MimicJoint
{
std::size_t joint_index;
std::size_t mimicked_joint_index;
double multiplier = 1.0;
};
std::vector<MimicJoint> mimic_joints_;

/// The size of this vector is (standard_interfaces_.size() x nr_joints)
std::vector<std::vector<double>> joint_commands_;
std::vector<std::vector<double>> joint_states_;
Expand Down
91 changes: 65 additions & 26 deletions hardware_interface/src/component_parser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@

#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/tools.hpp"

namespace
{
Expand Down Expand Up @@ -659,39 +660,77 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
}
for (auto & hw_info : hardware_info)
{
for (auto joint : hw_info.joints)
for (auto i = 0u; i < hw_info.joints.size(); ++i)
{
auto urdf_joint = model.getJoint(joint.name);
if (!urdf_joint)
const auto & joint = hw_info.joints.at(i);

// Search for mimic joints defined in ros2_control tag (deprecated)
if (joint.parameters.find("mimic") != joint.parameters.cend())
{
throw std::runtime_error("Joint " + joint.name + " not found in URDF");
std::cerr << "Warning: Mimic joints defined in ros2_control tag are deprecated. "
<< "Please define mimic joints in URDF." << std::endl;
const auto mimicked_joint_it = std::find_if(
hw_info.joints.begin(), hw_info.joints.end(),
[&mimicked_joint =
joint.parameters.at("mimic")](const hardware_interface::ComponentInfo & joint_info)
{ return joint_info.name == mimicked_joint; });
if (mimicked_joint_it == hw_info.joints.cend())
{
throw std::runtime_error(
std::string("Mimicked joint '") + joint.parameters.at("mimic") + "' not found");
}
hardware_interface::MimicJoint mimic_joint;
mimic_joint.joint_index = i;
mimic_joint.multiplier = 1.0;
mimic_joint.offset = 0.0;
mimic_joint.mimicked_joint_index = std::distance(hw_info.joints.begin(), mimicked_joint_it);
auto param_it = joint.parameters.find("multiplier");
if (param_it != joint.parameters.end())
{
mimic_joint.multiplier = parse_double(joint.parameters.at("multiplier"));
}
param_it = joint.parameters.find("offset");
if (param_it != joint.parameters.end())
{
mimic_joint.offset = parse_double(joint.parameters.at("offset"));
}
hw_info.mimic_joints.push_back(mimic_joint);
hw_info.joints.at(i).is_mimic = true;
}
if (joint.is_mimic)
else
{
if (urdf_joint->mimic)
auto urdf_joint = model.getJoint(joint.name);
if (!urdf_joint)
{
auto find_joint = [&hw_info](const std::string & name)
{
auto it = std::find_if(
hw_info.joints.begin(), hw_info.joints.end(),
[&name](const auto & j) { return j.name == name; });
if (it == hw_info.joints.end())
{
throw std::runtime_error("Joint `" + name + "` not found in hw_info.joints");
}
return std::distance(hw_info.joints.begin(), it);
};

MimicJoint mimic_joint;
mimic_joint.joint_index = find_joint(joint.name);
mimic_joint.mimicked_joint_index = find_joint(urdf_joint->mimic->joint_name);
mimic_joint.multiplier = urdf_joint->mimic->multiplier;
mimic_joint.offset = urdf_joint->mimic->offset;
hw_info.mimic_joints.push_back(mimic_joint);
throw std::runtime_error("Joint " + joint.name + " not found in URDF");
}
else
if (joint.is_mimic)
{
throw std::runtime_error("Joint `" + joint.name + "` has no mimic information in URDF");
if (urdf_joint->mimic)
{
auto find_joint = [&hw_info](const std::string & name)
{
auto it = std::find_if(
hw_info.joints.begin(), hw_info.joints.end(),
[&name](const auto & j) { return j.name == name; });
if (it == hw_info.joints.end())
{
throw std::runtime_error("Joint `" + name + "` not found in hw_info.joints");
}
return std::distance(hw_info.joints.begin(), it);
};

MimicJoint mimic_joint;
mimic_joint.joint_index = i;
mimic_joint.mimicked_joint_index = find_joint(urdf_joint->mimic->joint_name);
mimic_joint.multiplier = urdf_joint->mimic->multiplier;
mimic_joint.offset = urdf_joint->mimic->offset;
hw_info.mimic_joints.push_back(mimic_joint);
}
else
{
throw std::runtime_error("Joint `" + joint.name + "` has no mimic information in URDF");
}
}
}
}
Expand Down
44 changes: 3 additions & 41 deletions hardware_interface/src/mock_components/generic_system.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,6 @@
#include "mock_components/generic_system.hpp"

#include <algorithm>
#include <charconv>
#include <cmath>
#include <iterator>
#include <limits>
Expand All @@ -26,22 +25,12 @@
#include <vector>

#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/tools.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rcutils/logging_macros.h"

namespace mock_components
{
double parse_double(const std::string & text)
{
double result_value;
const auto parse_result = std::from_chars(text.data(), text.data() + text.size(), result_value);
if (parse_result.ec == std::errc())
{
return result_value;
}

return 0.0;
}

CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & info)
{
Expand Down Expand Up @@ -147,34 +136,6 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i
}
}

// Search for mimic joints
for (auto i = 0u; i < info_.joints.size(); ++i)
{
const auto & joint = info_.joints.at(i);
if (joint.parameters.find("mimic") != joint.parameters.cend())
{
const auto mimicked_joint_it = std::find_if(
info_.joints.begin(), info_.joints.end(),
[&mimicked_joint =
joint.parameters.at("mimic")](const hardware_interface::ComponentInfo & joint_info)
{ return joint_info.name == mimicked_joint; });
if (mimicked_joint_it == info_.joints.cend())
{
throw std::runtime_error(
std::string("Mimicked joint '") + joint.parameters.at("mimic") + "' not found");
}
MimicJoint mimic_joint;
mimic_joint.joint_index = i;
mimic_joint.mimicked_joint_index = std::distance(info_.joints.begin(), mimicked_joint_it);
auto param_it = joint.parameters.find("multiplier");
if (param_it != joint.parameters.end())
{
mimic_joint.multiplier = parse_double(joint.parameters.at("multiplier"));
}
mimic_joints_.push_back(mimic_joint);
}
}

// search for non-standard joint interfaces
for (const auto & joint : info_.joints)
{
Expand Down Expand Up @@ -604,11 +565,12 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
mirror_command_to_state(joint_states_, joint_commands_, 1);
}

for (const auto & mimic_joint : mimic_joints_)
for (const auto & mimic_joint : info_.mimic_joints)
{
for (auto i = 0u; i < joint_states_.size(); ++i)
{
joint_states_[i][mimic_joint.joint_index] =
mimic_joint.offset +
mimic_joint.multiplier * joint_states_[i][mimic_joint.mimicked_joint_index];
}
}
Expand Down
22 changes: 9 additions & 13 deletions hardware_interface/test/mock_components/test_generic_system.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -251,11 +251,7 @@ class TestGenericSystem : public ::testing::Test
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<param name="mimic">joint1</param>
<param name="multiplier">-2</param>
<command_interface name="position"/>
<command_interface name="velocity"/>
<joint name="joint2" mimic="true">
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
Expand Down Expand Up @@ -1208,11 +1204,9 @@ void TestGenericSystem::test_generic_system_with_mimic_joint(
EXPECT_TRUE(rm.state_interface_exists("joint2/position"));
EXPECT_TRUE(rm.state_interface_exists("joint2/velocity"));

ASSERT_EQ(4u, rm.command_interface_keys().size());
ASSERT_EQ(2u, rm.command_interface_keys().size());
EXPECT_TRUE(rm.command_interface_exists("joint1/position"));
EXPECT_TRUE(rm.command_interface_exists("joint1/velocity"));
EXPECT_TRUE(rm.command_interface_exists("joint2/position"));
EXPECT_TRUE(rm.command_interface_exists("joint2/velocity"));

// Check initial values
hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position");
Expand Down Expand Up @@ -1262,7 +1256,7 @@ void TestGenericSystem::test_generic_system_with_mimic_joint(

TEST_F(TestGenericSystem, hardware_system_2dof_with_mimic_joint)
{
auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_mimic_joint_ +
auto urdf = ros2_control_test_assets::urdf_head_mimic + hardware_system_2dof_with_mimic_joint_ +
ros2_control_test_assets::urdf_tail;

test_generic_system_with_mimic_joint(urdf, "HardwareSystem2dofWithMimicJoint");
Expand Down Expand Up @@ -1923,10 +1917,11 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active)

TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tags)
{
auto check_prepare_command_mode_switch = [&](const std::string & urdf)
auto check_prepare_command_mode_switch =
[&](
const std::string & urdf, const std::string & urdf_head = ros2_control_test_assets::urdf_head)
{
TestableResourceManager rm(
ros2_control_test_assets::urdf_head + urdf + ros2_control_test_assets::urdf_tail);
TestableResourceManager rm(urdf_head + urdf + ros2_control_test_assets::urdf_tail);
auto start_interfaces = rm.command_interface_keys();
std::vector<std::string> stop_interfaces;
return rm.prepare_command_mode_switch(start_interfaces, stop_interfaces);
Expand All @@ -1940,7 +1935,8 @@ TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tag
ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_mock_command_));
ASSERT_TRUE(
check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_mock_command_True_));
ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_mimic_joint_));
ASSERT_TRUE(check_prepare_command_mode_switch(
hardware_system_2dof_with_mimic_joint_, ros2_control_test_assets::urdf_head_mimic));
ASSERT_TRUE(
check_prepare_command_mode_switch(hardware_system_2dof_standard_interfaces_with_offset_));
ASSERT_TRUE(check_prepare_command_mode_switch(
Expand Down
Loading

0 comments on commit 0f1cbfc

Please sign in to comment.