Skip to content

Commit

Permalink
make controllers compilable
Browse files Browse the repository at this point in the history
  • Loading branch information
mamueluth committed Aug 30, 2024
1 parent c7c2f52 commit d1086a7
Show file tree
Hide file tree
Showing 4 changed files with 74 additions and 52 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,7 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
protected:
std::vector<hardware_interface::InterfaceDescription> export_state_interface_descriptions()
override;
std::vector<hardware_interface::CommandInterface> export_reference_interface_descriptions()
std::vector<hardware_interface::InterfaceDescription> export_reference_interface_descriptions()
override;

controller_interface::return_type update_reference_from_subscribers(
Expand Down Expand Up @@ -124,8 +124,8 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
// internal reference values
const std::vector<std::string> allowed_reference_interfaces_types_ = {
hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY};
std::vector<std::reference_wrapper<double>> position_reference_;
std::vector<std::reference_wrapper<double>> velocity_reference_;
std::vector<std::string> position_reference_names_;
std::vector<std::string> velocity_reference_names_;

// Admittance rule and dependent variables;
std::unique_ptr<admittance_controller::AdmittanceRule> admittance_;
Expand Down
68 changes: 40 additions & 28 deletions admittance_controller/src/admittance_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -97,43 +97,47 @@ controller_interface::InterfaceConfiguration AdmittanceController::state_interfa
controller_interface::interface_configuration_type::INDIVIDUAL, state_interfaces_config_names};
}

std::vector<hardware_interface::CommandInterface>
AdmittanceController::on_export_reference_interfaces()
std::vector<hardware_interface::InterfaceDescription>
AdmittanceController::export_state_interface_descriptions()
{
return {};
}

std::vector<hardware_interface::InterfaceDescription>
AdmittanceController::export_reference_interface_descriptions()
{
// create CommandInterface interfaces that other controllers will be able to chain with
if (!admittance_)
{
return {};
}

std::vector<hardware_interface::CommandInterface> chainable_command_interfaces;
std::vector<hardware_interface::InterfaceDescription> chainable_command_interfaces;
const auto num_chainable_interfaces =
admittance_->parameters_.chainable_command_interfaces.size() *
admittance_->parameters_.joints.size();

// allocate dynamic memory
chainable_command_interfaces.reserve(num_chainable_interfaces);
reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits<double>::quiet_NaN());
position_reference_ = {};
velocity_reference_ = {};
position_reference_names_ = {};
velocity_reference_names_ = {};

// assign reference interfaces
auto index = 0ul;
const auto prefix = std::string(get_node()->get_name());
// create reference interfaces
for (const auto & interface : allowed_reference_interfaces_types_)
{
for (const auto & joint : admittance_->parameters_.joints)
{
const auto itf_name = joint + "/" + interface;
chainable_command_interfaces.emplace_back(
prefix, hardware_interface::InterfaceInfo(itf_name, "double"));
// add reference interface names in order to position or velocity
if (hardware_interface::HW_IF_POSITION == interface)
position_reference_.emplace_back(reference_interfaces_[index]);
position_reference_names_.push_back(chainable_command_interfaces.back().get_name());
else if (hardware_interface::HW_IF_VELOCITY == interface)
{
velocity_reference_.emplace_back(reference_interfaces_[index]);
velocity_reference_names_.push_back(chainable_command_interfaces.back().get_name());
}
const auto full_name = joint + "/" + interface;
chainable_command_interfaces.emplace_back(hardware_interface::CommandInterface(
std::string(get_node()->get_name()), full_name, reference_interfaces_.data() + index));

index++;
}
}

Expand Down Expand Up @@ -370,11 +374,13 @@ controller_interface::return_type AdmittanceController::update_reference_from_su
{
for (size_t i = 0; i < joint_command_msg_->positions.size(); ++i)
{
position_reference_[i].get() = joint_command_msg_->positions[i];
reference_interfaces_.at(position_reference_names_[i])
->set_value(joint_command_msg_->positions[i]);
}
for (size_t i = 0; i < joint_command_msg_->velocities.size(); ++i)
{
velocity_reference_[i].get() = joint_command_msg_->velocities[i];
reference_interfaces_.at(velocity_reference_names_[i])
->set_value(joint_command_msg_->velocities[i]);
}
}

Expand Down Expand Up @@ -424,8 +430,10 @@ controller_interface::CallbackReturn AdmittanceController::on_deactivate(
// reset to prevent stale references
for (size_t i = 0; i < num_joints_; i++)
{
position_reference_[i].get() = std::numeric_limits<double>::quiet_NaN();
velocity_reference_[i].get() = std::numeric_limits<double>::quiet_NaN();
reference_interfaces_.at(position_reference_names_[i])
->set_value(std::numeric_limits<double>::quiet_NaN());
reference_interfaces_.at(velocity_reference_names_[i])
->set_value(std::numeric_limits<double>::quiet_NaN());
}

for (size_t index = 0; index < allowed_interface_types_.size(); ++index)
Expand Down Expand Up @@ -473,19 +481,19 @@ void AdmittanceController::read_state_from_hardware(
if (has_position_state_interface_)
{
state_current.positions[joint_ind] =
state_interfaces_[pos_ind * num_joints_ + joint_ind].get_value();
state_interfaces_[pos_ind * num_joints_ + joint_ind].get_value<double>();
nan_position |= std::isnan(state_current.positions[joint_ind]);
}
else if (has_velocity_state_interface_)
{
state_current.velocities[joint_ind] =
state_interfaces_[vel_ind * num_joints_ + joint_ind].get_value();
state_interfaces_[vel_ind * num_joints_ + joint_ind].get_value<double>();
nan_velocity |= std::isnan(state_current.velocities[joint_ind]);
}
else if (has_acceleration_state_interface_)
{
state_current.accelerations[joint_ind] =
state_interfaces_[acc_ind * num_joints_ + joint_ind].get_value();
state_interfaces_[acc_ind * num_joints_ + joint_ind].get_value<double>();
nan_acceleration |= std::isnan(state_current.accelerations[joint_ind]);
}
}
Expand Down Expand Up @@ -551,18 +559,22 @@ void AdmittanceController::read_state_reference_interfaces(
for (size_t i = 0; i < num_joints_; ++i)
{
// update position
if (std::isnan(position_reference_[i]))
if (std::isnan(reference_interfaces_.at(position_reference_names_[i])->get_value<double>()))
{
position_reference_[i].get() = last_reference_.positions[i];
reference_interfaces_.at(position_reference_names_[i])
->set_value(last_reference_.positions[i]);
}
state_reference.positions[i] = position_reference_[i];
state_reference.positions[i] =
reference_interfaces_.at(position_reference_names_[i])->get_value<double>();

// update velocity
if (std::isnan(velocity_reference_[i]))
if (std::isnan(reference_interfaces_.at(velocity_reference_names_[i])->get_value<double>()))
{
velocity_reference_[i].get() = last_reference_.velocities[i];
reference_interfaces_.at(velocity_reference_names_[i])
->set_value(last_reference_.velocities[i]);
}
state_reference.velocities[i] = velocity_reference_[i];
state_reference.velocities[i] =
reference_interfaces_.at(velocity_reference_names_[i])->get_value<double>();
}

last_reference_.positions = state_reference.positions;
Expand Down
16 changes: 10 additions & 6 deletions admittance_controller/test/test_admittance_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -154,20 +154,22 @@ TEST_F(AdmittanceControllerTest, check_interfaces)
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);

auto command_interfaces = controller_->command_interface_configuration();
ASSERT_EQ(command_interfaces.names.size(), joint_command_values_.size());
ASSERT_EQ(command_interfaces.names.size(), initial_joint_command_values_.size());
EXPECT_EQ(
command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL);

ASSERT_EQ(
controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size());

auto state_interfaces = controller_->state_interface_configuration();
ASSERT_EQ(state_interfaces.names.size(), joint_state_values_.size() + fts_state_values_.size());
ASSERT_EQ(
state_interfaces.names.size(),
initial_joint_state_values_.size() + initial_fts_state_values_.size());
EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL);

ASSERT_EQ(
controller_->state_interfaces_.size(),
state_interface_types_.size() * joint_names_.size() + fts_state_values_.size());
state_interface_types_.size() * joint_names_.size() + initial_fts_state_values_.size());
}

TEST_F(AdmittanceControllerTest, activate_success)
Expand Down Expand Up @@ -264,9 +266,10 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status)
controller_interface::return_type::OK);

// After first update state, commanded position should be near the start state
for (auto i = 0ul; i < joint_state_values_.size(); i++)
for (auto i = 0ul; i < state_itfs_.size(); i++)
{
ASSERT_NEAR(joint_state_values_[i], joint_command_values_[i], COMMON_THRESHOLD);
ASSERT_NEAR(
state_itfs_[i].get_value<double>(), command_itfs_[i].get_value<double>(), COMMON_THRESHOLD);
}

ControllerStateMsg msg;
Expand All @@ -282,7 +285,8 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status)
controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::OK);

EXPECT_NEAR(joint_command_values_[0], joint_state_values_[0], COMMON_THRESHOLD);
EXPECT_NEAR(
command_itfs_[0].get_value<double>(), state_itfs_[0].get_value<double>(), COMMON_THRESHOLD);

subscribe_and_get_messages(msg);
}
Expand Down
36 changes: 21 additions & 15 deletions admittance_controller/test/test_admittance_controller.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -174,28 +174,32 @@ class AdmittanceControllerTest : public ::testing::Test
void assign_interfaces()
{
std::vector<hardware_interface::LoanedCommandInterface> command_ifs;
command_itfs_.reserve(joint_command_values_.size());
command_ifs.reserve(joint_command_values_.size());
command_itfs_.reserve(initial_joint_command_values_.size());
command_ifs.reserve(initial_joint_command_values_.size());

for (auto i = 0u; i < joint_command_values_.size(); ++i)
for (auto i = 0u; i < initial_joint_command_values_.size(); ++i)
{
command_itfs_.emplace_back(hardware_interface::CommandInterface(
joint_names_[i], command_interface_types_[0], &joint_command_values_[i]));
command_itfs_.emplace_back(hardware_interface::InterfaceDescription(
joint_names_[i], hardware_interface::InterfaceInfo(
command_interface_types_[0], "double",
std::to_string(initial_joint_command_values_[i]))));
command_ifs.emplace_back(command_itfs_.back());
}

auto sc_fts = semantic_components::ForceTorqueSensor(ft_sensor_name_);
fts_state_names_ = sc_fts.get_state_interface_names();
std::vector<hardware_interface::LoanedStateInterface> state_ifs;

const size_t num_state_ifs = joint_state_values_.size() + fts_state_names_.size();
const size_t num_state_ifs = initial_joint_state_values_.size() + fts_state_names_.size();
state_itfs_.reserve(num_state_ifs);
state_ifs.reserve(num_state_ifs);

for (auto i = 0u; i < joint_state_values_.size(); ++i)
for (auto i = 0u; i < initial_joint_state_values_.size(); ++i)
{
state_itfs_.emplace_back(hardware_interface::StateInterface(
joint_names_[i], state_interface_types_[0], &joint_state_values_[i]));
state_itfs_.emplace_back(hardware_interface::InterfaceDescription(
joint_names_[i],
hardware_interface::InterfaceInfo(
state_interface_types_[0], std::to_string(initial_joint_command_values_[i]))));
state_ifs.emplace_back(state_itfs_.back());
}

Expand All @@ -204,8 +208,10 @@ class AdmittanceControllerTest : public ::testing::Test

for (auto i = 0u; i < fts_state_names_.size(); ++i)
{
state_itfs_.emplace_back(hardware_interface::StateInterface(
ft_sensor_name_, fts_itf_names[i], &fts_state_values_[i]));
state_itfs_.emplace_back(hardware_interface::InterfaceDescription(
ft_sensor_name_,
hardware_interface::InterfaceInfo(
fts_itf_names[i], "double", std::to_string(initial_fts_state_values_[i]))));
state_ifs.emplace_back(state_itfs_.back());
}

Expand Down Expand Up @@ -344,7 +350,7 @@ class AdmittanceControllerTest : public ::testing::Test
trajectory_point.velocities.resize(num_joints, 0.0);
for (auto index = 0u; index < num_joints; ++index)
{
trajectory_point.positions.emplace_back(joint_state_values_[index]);
trajectory_point.positions.emplace_back(initial_joint_state_values_[index]);
}
joint_msg = trajectory_point;

Expand Down Expand Up @@ -380,9 +386,9 @@ class AdmittanceControllerTest : public ::testing::Test
2.828427, 2.828427, 2.828427};
std::array<double, 6> admittance_stiffness_ = {214.1, 214.2, 214.3, 214.4, 214.5, 214.6};

std::array<double, 6> joint_command_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
std::array<double, 6> joint_state_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6};
std::array<double, 6> fts_state_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
const std::array<double, 6> initial_joint_command_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
const std::array<double, 6> initial_joint_state_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6};
const std::array<double, 6> initial_fts_state_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
std::vector<std::string> fts_state_names_;

std::vector<hardware_interface::StateInterface> state_itfs_;
Expand Down

0 comments on commit d1086a7

Please sign in to comment.