Skip to content

Commit

Permalink
Go back to old order of tests for diff view
Browse files Browse the repository at this point in the history
  • Loading branch information
@christophfroehlich
christophfroehlich committed Oct 10, 2023
1 parent ab96ef1 commit 877486e
Showing 1 changed file with 127 additions and 127 deletions.
254 changes: 127 additions & 127 deletions joint_trajectory_controller/test/test_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -453,6 +453,133 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup)
executor.cancel();
}

// Floating-point value comparison threshold
const double EPS = 1e-6;
/**
* @brief check if position error of revolute joints are angle_wraparound if not configured so
*/
TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparound)
{
rclcpp::executors::MultiThreadedExecutor executor;
constexpr double k_p = 10.0;
std::vector<rclcpp::Parameter> params = {
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
bool angle_wraparound = false;
SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, angle_wraparound);
subscribeToState();

size_t n_joints = joint_names_.size();

// send msg
constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
// *INDENT-OFF*
std::vector<std::vector<double>> points{
{{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
std::vector<std::vector<double>> points_velocities{
{{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
// *INDENT-ON*
publish(time_from_start, points, rclcpp::Time(), {}, {});
traj_controller_->wait_for_trajectory(executor);

// first update
updateController(rclcpp::Duration(FIRST_POINT_TIME));

// Spin to receive latest state
executor.spin_some();
auto state_msg = getState();
ASSERT_TRUE(state_msg);

const auto allowed_delta = 0.1;

// no update of state_interface
EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS);

// has the msg the correct vector sizes?
EXPECT_EQ(n_joints, state_msg->reference.positions.size());
EXPECT_EQ(n_joints, state_msg->feedback.positions.size());
EXPECT_EQ(n_joints, state_msg->error.positions.size());

// are the correct reference positions used?
EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta);
EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta);
EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta);

// no normalization of position error
EXPECT_NEAR(
state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS);
EXPECT_NEAR(
state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS);
EXPECT_NEAR(
state_msg->error.positions[2], state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2], EPS);

if (traj_controller_->has_position_command_interface())
{
// check command interface
EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta);
EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta);
EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta);
// double check state msg
EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta);
EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta);
EXPECT_NEAR(points[0][2], state_msg->output.positions[2], allowed_delta);
}

if (traj_controller_->has_velocity_command_interface())
{
// use_closed_loop_pid_adapter_
if (traj_controller_->use_closed_loop_pid_adapter())
{
// we expect u = k_p * (s_d-s)
EXPECT_NEAR(
k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
k_p * allowed_delta);
EXPECT_NEAR(
k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
k_p * allowed_delta);
// no normalization of position error
EXPECT_NEAR(
k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2],
k_p * allowed_delta);
}
else
{
// check command interface
EXPECT_LT(0.0, joint_vel_[0]);
EXPECT_LT(0.0, joint_vel_[1]);
EXPECT_LT(0.0, joint_vel_[2]);
// double check state msg
EXPECT_LT(0.0, state_msg->output.velocities[0]);
EXPECT_LT(0.0, state_msg->output.velocities[1]);
EXPECT_LT(0.0, state_msg->output.velocities[2]);
}
}

if (traj_controller_->has_effort_command_interface())
{
// with effort command interface, use_closed_loop_pid_adapter is always true

// check command interface
// we expect u = k_p * (s_d-s) for joint0, joint1, joint2
EXPECT_NEAR(
k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0],
k_p * allowed_delta);
EXPECT_NEAR(
k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1],
k_p * allowed_delta);
EXPECT_NEAR(
k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2],
k_p * allowed_delta);

// double check state msg
EXPECT_LT(0.0, state_msg->output.effort[0]);
EXPECT_LT(0.0, state_msg->output.effort[1]);
EXPECT_LT(0.0, state_msg->output.effort[2]);
}

executor.cancel();
}

/**
* @brief cmd_timeout must be greater than constraints.goal_time
*/
Expand Down Expand Up @@ -594,133 +721,6 @@ TEST_P(TrajectoryControllerTestParameterized, timeout)
executor.cancel();
}

// Floating-point value comparison threshold
const double EPS = 1e-6;
/**
* @brief check if position error of revolute joints are angle_wraparound if not configured so
*/
TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparound)
{
rclcpp::executors::MultiThreadedExecutor executor;
constexpr double k_p = 10.0;
std::vector<rclcpp::Parameter> params = {
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
bool angle_wraparound = false;
SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, angle_wraparound);
subscribeToState();

size_t n_joints = joint_names_.size();

// send msg
constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
// *INDENT-OFF*
std::vector<std::vector<double>> points{
{{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
std::vector<std::vector<double>> points_velocities{
{{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
// *INDENT-ON*
publish(time_from_start, points, rclcpp::Time(), {}, {});
traj_controller_->wait_for_trajectory(executor);

// first update
updateController(rclcpp::Duration(FIRST_POINT_TIME));

// Spin to receive latest state
executor.spin_some();
auto state_msg = getState();
ASSERT_TRUE(state_msg);

const auto allowed_delta = 0.1;

// no update of state_interface
EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS);

// has the msg the correct vector sizes?
EXPECT_EQ(n_joints, state_msg->reference.positions.size());
EXPECT_EQ(n_joints, state_msg->feedback.positions.size());
EXPECT_EQ(n_joints, state_msg->error.positions.size());

// are the correct reference positions used?
EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta);
EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta);
EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta);

// no normalization of position error
EXPECT_NEAR(
state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS);
EXPECT_NEAR(
state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS);
EXPECT_NEAR(
state_msg->error.positions[2], state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2], EPS);

if (traj_controller_->has_position_command_interface())
{
// check command interface
EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta);
EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta);
EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta);
// double check state msg
EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta);
EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta);
EXPECT_NEAR(points[0][2], state_msg->output.positions[2], allowed_delta);
}

if (traj_controller_->has_velocity_command_interface())
{
// use_closed_loop_pid_adapter_
if (traj_controller_->use_closed_loop_pid_adapter())
{
// we expect u = k_p * (s_d-s)
EXPECT_NEAR(
k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
k_p * allowed_delta);
EXPECT_NEAR(
k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
k_p * allowed_delta);
// no normalization of position error
EXPECT_NEAR(
k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2],
k_p * allowed_delta);
}
else
{
// check command interface
EXPECT_LT(0.0, joint_vel_[0]);
EXPECT_LT(0.0, joint_vel_[1]);
EXPECT_LT(0.0, joint_vel_[2]);
// double check state msg
EXPECT_LT(0.0, state_msg->output.velocities[0]);
EXPECT_LT(0.0, state_msg->output.velocities[1]);
EXPECT_LT(0.0, state_msg->output.velocities[2]);
}
}

if (traj_controller_->has_effort_command_interface())
{
// with effort command interface, use_closed_loop_pid_adapter is always true

// check command interface
// we expect u = k_p * (s_d-s) for joint0, joint1, joint2
EXPECT_NEAR(
k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0],
k_p * allowed_delta);
EXPECT_NEAR(
k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1],
k_p * allowed_delta);
EXPECT_NEAR(
k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2],
k_p * allowed_delta);

// double check state msg
EXPECT_LT(0.0, state_msg->output.effort[0]);
EXPECT_LT(0.0, state_msg->output.effort[1]);
EXPECT_LT(0.0, state_msg->output.effort[2]);
}

executor.cancel();
}

/**
* @brief check if position error of revolute joints are normalized if configured so
*/
Expand Down

0 comments on commit 877486e

Please sign in to comment.