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.

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

[JTC] Fix state offset tests #797

Merged
Original file line number Diff line number Diff line change
Expand Up @@ -261,8 +261,14 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
const rclcpp::Time & time, const JointTrajectoryPoint & desired_state,
const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error);

void read_state_from_hardware(JointTrajectoryPoint & state);
void read_state_from_state_interfaces(JointTrajectoryPoint & state);

/** Assign values from the command interfaces as state.
* This is only possible if command AND state interfaces exist for the same type,
* therefore needs check for both.
* @param[out] state to be filled with values from command interfaces.
* @return true if all interfaces exists and contain non-NaN values, false otherwise.
*/
bool read_state_from_command_interfaces(JointTrajectoryPoint & state);
bool read_commands_from_command_interfaces(JointTrajectoryPoint & commands);

Expand Down
19 changes: 10 additions & 9 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -175,7 +175,7 @@ controller_interface::return_type JointTrajectoryController::update(

// current state update
state_current_.time_from_start.set__sec(0);
read_state_from_hardware(state_current_);
read_state_from_state_interfaces(state_current_);

// currently carrying out a trajectory
if (has_active_trajectory())
Expand Down Expand Up @@ -397,7 +397,7 @@ controller_interface::return_type JointTrajectoryController::update(
return controller_interface::return_type::OK;
}

void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state)
void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectoryPoint & state)
{
auto assign_point_from_interface =
[&](std::vector<double> & trajectory_point_interface, const auto & joint_interface)
Expand Down Expand Up @@ -951,20 +951,21 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(

subscriber_is_active_ = true;

// Initialize current state storage if hardware state has tracking offset
read_state_from_hardware(state_current_);
read_state_from_hardware(state_desired_);
read_state_from_hardware(last_commanded_state_);
// Handle restart of controller by reading from commands if
// those are not nan
// Handle restart of controller by reading from commands if those are not NaN (a controller was
// running already)
trajectory_msgs::msg::JointTrajectoryPoint state;
resize_joint_trajectory_point(state, dof_);
if (read_state_from_command_interfaces(state))
{
state_current_ = state;
state_desired_ = state;
last_commanded_state_ = state;
}
else
{
// Initialize current state storage from hardware
read_state_from_state_interfaces(state_current_);
read_state_from_state_interfaces(last_commanded_state_);
}

// Should the controller start by holding position at the beginning of active state?
if (params_.start_with_holding)
Expand Down
118 changes: 73 additions & 45 deletions joint_trajectory_controller/test/test_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1668,21 +1668,23 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e
#endif

// Testing that values are read from state interfaces when hardware is started for the first
// time and hardware state has offset --> this is indicated by NaN values in state interfaces
// time and hardware state has offset --> this is indicated by NaN values in command interfaces
TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_controller_start)
{
rclcpp::executors::SingleThreadedExecutor executor;
// default if false so it will not be actually set parameter
rclcpp::Parameter is_open_loop_parameters("open_loop_control", true);

// set command values to NaN
for (size_t i = 0; i < 3; ++i)
{
joint_pos_[i] = std::numeric_limits<double>::quiet_NaN();
joint_vel_[i] = std::numeric_limits<double>::quiet_NaN();
joint_acc_[i] = std::numeric_limits<double>::quiet_NaN();
}
SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true);
std::vector<double> initial_pos_cmd{3, std::numeric_limits<double>::quiet_NaN()};
std::vector<double> initial_vel_cmd{3, std::numeric_limits<double>::quiet_NaN()};
std::vector<double> initial_acc_cmd{3, std::numeric_limits<double>::quiet_NaN()};

SetUpAndActivateTrajectoryController(
executor, {is_open_loop_parameters}, true, 0., 1., false, initial_pos_cmd, initial_vel_cmd,
initial_acc_cmd);

// no call of update method, so the values should be read from state interfaces
// (command interface are NaN)

auto current_state_when_offset = traj_controller_->get_current_state_when_offset();

Expand All @@ -1691,70 +1693,96 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co
EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]);

// check velocity
if (
std::find(
state_interface_types_.begin(), state_interface_types_.end(),
hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() &&
traj_controller_->has_velocity_command_interface())
if (traj_controller_->has_velocity_state_interface())
{
EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]);
EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]);
}

// check acceleration
if (
std::find(
state_interface_types_.begin(), state_interface_types_.end(),
hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() &&
traj_controller_->has_acceleration_command_interface())
if (traj_controller_->has_acceleration_state_interface())
{
EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]);
EXPECT_EQ(current_state_when_offset.accelerations[i], joint_state_acc_[i]);
}
}

executor.cancel();
}

// Testing that values are read from state interfaces when hardware is started after some values
// Testing that values are read from command interfaces when hardware is started after some values
// are set on the hardware commands
TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_controller_start)
{
rclcpp::executors::SingleThreadedExecutor executor;
// default if false so it will not be actually set parameter
rclcpp::Parameter is_open_loop_parameters("open_loop_control", true);

// set command values to NaN
// set command values to arbitrary values
std::vector<double> initial_pos_cmd, initial_vel_cmd, initial_acc_cmd;
for (size_t i = 0; i < 3; ++i)
{
joint_pos_[i] = 3.1 + static_cast<double>(i);
joint_vel_[i] = 0.25 + static_cast<double>(i);
joint_acc_[i] = 0.02 + static_cast<double>(i) / 10.0;
initial_pos_cmd.push_back(3.1 + static_cast<double>(i));
initial_vel_cmd.push_back(0.25 + static_cast<double>(i));
initial_acc_cmd.push_back(0.02 + static_cast<double>(i) / 10.0);
}
SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true);
SetUpAndActivateTrajectoryController(
executor, {is_open_loop_parameters}, true, 0., 1., false, initial_pos_cmd, initial_vel_cmd,
initial_acc_cmd);

// no call of update method, so the values should be read from command interfaces

auto current_state_when_offset = traj_controller_->get_current_state_when_offset();

for (size_t i = 0; i < 3; ++i)
{
EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]);

// check velocity
if (
std::find(
state_interface_types_.begin(), state_interface_types_.end(),
hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() &&
traj_controller_->has_velocity_command_interface())
// check position
if (traj_controller_->has_position_command_interface())
{
EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]);
// check velocity
if (traj_controller_->has_velocity_state_interface())
{
if (traj_controller_->has_velocity_command_interface())
{
// check acceleration
if (traj_controller_->has_acceleration_state_interface())
{
if (traj_controller_->has_acceleration_command_interface())
{
// should have set it to last position + velocity + acceleration command
EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]);
EXPECT_EQ(current_state_when_offset.velocities[i], initial_vel_cmd[i]);
EXPECT_EQ(current_state_when_offset.accelerations[i], initial_acc_cmd[i]);
}
else
{
// should have set it to the state interface instead
EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]);
EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]);
EXPECT_EQ(current_state_when_offset.accelerations[i], joint_state_acc_[i]);
}
}
else
{
// should have set it to last position + velocity command
EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]);
EXPECT_EQ(current_state_when_offset.velocities[i], initial_vel_cmd[i]);
}
}
else
{
// should have set it to the state interface instead
EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]);
EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]);
}
}
else
{
// should have set it to last position command
EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]);
}
}

// check acceleration
if (
std::find(
state_interface_types_.begin(), state_interface_types_.end(),
hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() &&
traj_controller_->has_acceleration_command_interface())
else
{
EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]);
// should have set it to the state interface instead
EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]);
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -220,7 +220,11 @@ class TrajectoryControllerTest : public ::testing::Test
void SetUpAndActivateTrajectoryController(
rclcpp::Executor & executor, const std::vector<rclcpp::Parameter> & parameters = {},
bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0,
bool angle_wraparound = false)
bool angle_wraparound = false,
const std::vector<double> initial_pos_joints = INITIAL_POS_JOINTS,
const std::vector<double> initial_vel_joints = INITIAL_VEL_JOINTS,
const std::vector<double> initial_acc_joints = INITIAL_ACC_JOINTS,
const std::vector<double> initial_eff_joints = INITIAL_EFF_JOINTS)
{
SetUpTrajectoryController(executor);

Expand All @@ -239,12 +243,17 @@ class TrajectoryControllerTest : public ::testing::Test

traj_controller_->get_node()->configure();

ActivateTrajectoryController(separate_cmd_and_state_values);
ActivateTrajectoryController(
separate_cmd_and_state_values, initial_pos_joints, initial_vel_joints, initial_acc_joints,
initial_eff_joints);
}

void ActivateTrajectoryController(
bool separate_cmd_and_state_values = false,
const std::vector<double> initial_pos_joints = INITIAL_POS_JOINTS)
const std::vector<double> initial_pos_joints = INITIAL_POS_JOINTS,
const std::vector<double> initial_vel_joints = INITIAL_VEL_JOINTS,
const std::vector<double> initial_acc_joints = INITIAL_ACC_JOINTS,
const std::vector<double> initial_eff_joints = INITIAL_EFF_JOINTS)
{
std::vector<hardware_interface::LoanedCommandInterface> cmd_interfaces;
std::vector<hardware_interface::LoanedStateInterface> state_interfaces;
Expand Down Expand Up @@ -280,14 +289,17 @@ class TrajectoryControllerTest : public ::testing::Test
cmd_interfaces.emplace_back(pos_cmd_interfaces_.back());
cmd_interfaces.back().set_value(initial_pos_joints[i]);
cmd_interfaces.emplace_back(vel_cmd_interfaces_.back());
cmd_interfaces.back().set_value(INITIAL_VEL_JOINTS[i]);
cmd_interfaces.back().set_value(initial_vel_joints[i]);
cmd_interfaces.emplace_back(acc_cmd_interfaces_.back());
cmd_interfaces.back().set_value(INITIAL_ACC_JOINTS[i]);
cmd_interfaces.back().set_value(initial_acc_joints[i]);
cmd_interfaces.emplace_back(eff_cmd_interfaces_.back());
cmd_interfaces.back().set_value(INITIAL_EFF_JOINTS[i]);
joint_state_pos_[i] = initial_pos_joints[i];
joint_state_vel_[i] = INITIAL_VEL_JOINTS[i];
joint_state_acc_[i] = INITIAL_ACC_JOINTS[i];
cmd_interfaces.back().set_value(initial_eff_joints[i]);
if (separate_cmd_and_state_values)
{
joint_state_pos_[i] = INITIAL_POS_JOINTS[i];
joint_state_vel_[i] = INITIAL_VEL_JOINTS[i];
joint_state_acc_[i] = INITIAL_ACC_JOINTS[i];
}
state_interfaces.emplace_back(pos_state_interfaces_.back());
state_interfaces.emplace_back(vel_state_interfaces_.back());
state_interfaces.emplace_back(acc_state_interfaces_.back());
Expand Down Expand Up @@ -489,27 +501,33 @@ class TrajectoryControllerTest : public ::testing::Test
// --> set kp > 0.0 in test
if (traj_controller_->has_velocity_command_interface())
{
EXPECT_TRUE(is_same_sign_or_zero(point.at(0) - joint_state_pos_[0], joint_vel_[0]))
<< "current error: " << point.at(0) - joint_state_pos_[0] << ", velocity command is "
<< joint_vel_[0];
EXPECT_TRUE(is_same_sign_or_zero(point.at(1) - joint_state_pos_[1], joint_vel_[1]))
<< "current error: " << point.at(1) - joint_state_pos_[1] << ", velocity command is "
<< joint_vel_[1];
EXPECT_TRUE(is_same_sign_or_zero(point.at(2) - joint_state_pos_[2], joint_vel_[2]))
<< "current error: " << point.at(2) - joint_state_pos_[2] << ", velocity command is "
<< joint_vel_[2];
EXPECT_TRUE(
is_same_sign_or_zero(point.at(0) - pos_state_interfaces_[0].get_value(), joint_vel_[0]))
<< "current error: " << point.at(0) - pos_state_interfaces_[0].get_value()
<< ", velocity command is " << joint_vel_[0];
EXPECT_TRUE(
is_same_sign_or_zero(point.at(1) - pos_state_interfaces_[1].get_value(), joint_vel_[1]))
<< "current error: " << point.at(1) - pos_state_interfaces_[1].get_value()
<< ", velocity command is " << joint_vel_[1];
EXPECT_TRUE(
is_same_sign_or_zero(point.at(2) - pos_state_interfaces_[2].get_value(), joint_vel_[2]))
<< "current error: " << point.at(2) - pos_state_interfaces_[2].get_value()
<< ", velocity command is " << joint_vel_[2];
}
if (traj_controller_->has_effort_command_interface())
{
EXPECT_TRUE(is_same_sign_or_zero(point.at(0) - joint_state_pos_[0], joint_eff_[0]))
<< "current error: " << point.at(0) - joint_state_pos_[0] << ", effort command is "
<< joint_eff_[0];
EXPECT_TRUE(is_same_sign_or_zero(point.at(1) - joint_state_pos_[1], joint_eff_[1]))
<< "current error: " << point.at(1) - joint_state_pos_[1] << ", effort command is "
<< joint_eff_[1];
EXPECT_TRUE(is_same_sign_or_zero(point.at(2) - joint_state_pos_[2], joint_eff_[2]))
<< "current error: " << point.at(2) - joint_state_pos_[2] << ", effort command is "
<< joint_eff_[2];
EXPECT_TRUE(
is_same_sign_or_zero(point.at(0) - pos_state_interfaces_[0].get_value(), joint_eff_[0]))
<< "current error: " << point.at(0) - pos_state_interfaces_[0].get_value()
<< ", effort command is " << joint_eff_[0];
EXPECT_TRUE(
is_same_sign_or_zero(point.at(1) - pos_state_interfaces_[1].get_value(), joint_eff_[1]))
<< "current error: " << point.at(1) - pos_state_interfaces_[1].get_value()
<< ", effort command is " << joint_eff_[1];
EXPECT_TRUE(
is_same_sign_or_zero(point.at(2) - pos_state_interfaces_[2].get_value(), joint_eff_[2]))
<< "current error: " << point.at(2) - pos_state_interfaces_[2].get_value()
<< ", effort command is " << joint_eff_[2];
}
}
}
Expand Down
Loading