Skip to content

Commit

Permalink
[JTC] Fix tests when state offset is used (#797)
Browse files Browse the repository at this point in the history 
* Simplify initialization of states
* Rename read_state_from_hardware method
* Don't set state_desired in on_activate
---------

Co-authored-by: Dr. Denis <[email protected]>
  • Loading branch information
@christophfroehlich @destogl
christophfroehlich and destogl authored Nov 15, 2023
1 parent f1a5397 commit b13ae5b
Show file tree
Hide file tree
Showing 4 changed files with 135 additions and 82 deletions.
8 changes: 7 additions & 1 deletion ...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
View file
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
View file
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
View file
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
72 changes: 45 additions & 27 deletions joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
View file
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

0 comments on commit b13ae5b

Please sign in to comment.