Skip to content

Commit 7f6ebb9

Browse files
christophfroehlichchristophfroehlich
committed
Add a test for compute_error function
1 parent 916cc1c commit 7f6ebb9

File tree

2 files changed

+213
-0
lines changed

2 files changed

+213
-0
lines changed

‎joint_trajectory_controller/test/test_trajectory_controller.cpp‎

Lines changed: 189 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -466,6 +466,195 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup)
466466

467467
// Floating-point value comparison threshold
468468
const double EPS = 1e-6;
469+
470+
/**
471+
* @brief check if calculated trajectory error is correct with angle wraparound=true
472+
*/
473+
TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_true)
474+
{
475+
rclcpp::executors::MultiThreadedExecutor executor;
476+
std::vector<rclcpp::Parameter> params = {};
477+
SetUpAndActivateTrajectoryController(
478+
executor, params, true, 0.0, 1.0, true /* angle_wraparound */);
479+
480+
size_t n_joints = joint_names_.size();
481+
482+
// send msg
483+
constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
484+
builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
485+
// *INDENT-OFF*
486+
std::vector<std::vector<double>> points{
487+
{{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
488+
std::vector<std::vector<double>> points_velocities{
489+
{{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
490+
std::vector<std::vector<double>> points_accelerations{
491+
{{0.1, 0.1, 0.1}}, {{0.5, 0.5, 0.5}}, {{0.6, 0.6, 0.6}}};
492+
// *INDENT-ON*
493+
494+
trajectory_msgs::msg::JointTrajectoryPoint error, current, desired;
495+
current.positions = {points[0].begin(), points[0].end()};
496+
current.velocities = {points_velocities[0].begin(), points_velocities[0].end()};
497+
current.accelerations = {points_accelerations[0].begin(), points_accelerations[0].end()};
498+
traj_controller_->resize_joint_trajectory_point(error, n_joints);
499+
500+
// zero error
501+
desired = current;
502+
for (size_t i = 0; i < n_joints; ++i)
503+
{
504+
traj_controller_->testable_compute_error_for_joint(error, i, current, desired);
505+
}
506+
EXPECT_NEAR(error.positions[0], 0., EPS);
507+
EXPECT_NEAR(error.positions[1], 0., EPS);
508+
EXPECT_NEAR(error.positions[2], 0., EPS);
509+
if (
510+
traj_controller_->has_velocity_state_interface() &&
511+
(traj_controller_->has_velocity_command_interface() ||
512+
traj_controller_->has_effort_command_interface()))
513+
{
514+
// error.velocities[index] = desired.velocities[index] - current.velocities[index];
515+
EXPECT_NEAR(error.velocities[0], 0., EPS);
516+
EXPECT_NEAR(error.velocities[1], 0., EPS);
517+
EXPECT_NEAR(error.velocities[2], 0., EPS);
518+
}
519+
if (
520+
traj_controller_->has_acceleration_state_interface() &&
521+
traj_controller_->has_acceleration_command_interface())
522+
{
523+
// error.accelerations[index] = desired.accelerations[index] - current.accelerations[index];
524+
EXPECT_NEAR(error.accelerations[0], 0., EPS);
525+
EXPECT_NEAR(error.accelerations[1], 0., EPS);
526+
EXPECT_NEAR(error.accelerations[2], 0., EPS);
527+
}
528+
529+
// angle wraparound of position error
530+
desired.positions[0] += 3.0 * M_PI_2;
531+
desired.velocities[0] += 1.0;
532+
desired.accelerations[0] += 1.0;
533+
traj_controller_->resize_joint_trajectory_point(error, n_joints);
534+
for (size_t i = 0; i < n_joints; ++i)
535+
{
536+
traj_controller_->testable_compute_error_for_joint(error, i, current, desired);
537+
}
538+
EXPECT_NEAR(error.positions[0], desired.positions[0] - current.positions[0] - 2.0 * M_PI, EPS);
539+
EXPECT_NEAR(error.positions[1], desired.positions[1] - current.positions[1], EPS);
540+
EXPECT_NEAR(error.positions[2], desired.positions[2] - current.positions[2], EPS);
541+
if (
542+
traj_controller_->has_velocity_state_interface() &&
543+
(traj_controller_->has_velocity_command_interface() ||
544+
traj_controller_->has_effort_command_interface()))
545+
{
546+
// error.velocities[index] = desired.velocities[index] - current.velocities[index];
547+
EXPECT_NEAR(error.velocities[0], desired.velocities[0] - current.velocities[0], EPS);
548+
EXPECT_NEAR(error.velocities[1], desired.velocities[1] - current.velocities[1], EPS);
549+
EXPECT_NEAR(error.velocities[2], desired.velocities[2] - current.velocities[2], EPS);
550+
}
551+
if (
552+
traj_controller_->has_acceleration_state_interface() &&
553+
traj_controller_->has_acceleration_command_interface())
554+
{
555+
// error.accelerations[index] = desired.accelerations[index] - current.accelerations[index];
556+
EXPECT_NEAR(error.accelerations[0], desired.accelerations[0] - current.accelerations[0], EPS);
557+
EXPECT_NEAR(error.accelerations[1], desired.accelerations[1] - current.accelerations[1], EPS);
558+
EXPECT_NEAR(error.accelerations[2], desired.accelerations[2] - current.accelerations[2], EPS);
559+
}
560+
561+
executor.cancel();
562+
}
563+
564+
/**
565+
* @brief check if calculated trajectory error is correct with angle wraparound=false
566+
*/
567+
TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_false)
568+
{
569+
rclcpp::executors::MultiThreadedExecutor executor;
570+
std::vector<rclcpp::Parameter> params = {};
571+
SetUpAndActivateTrajectoryController(
572+
executor, params, true, 0.0, 1.0, false /* angle_wraparound */);
573+
574+
size_t n_joints = joint_names_.size();
575+
576+
// send msg
577+
constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
578+
builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
579+
// *INDENT-OFF*
580+
std::vector<std::vector<double>> points{
581+
{{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
582+
std::vector<std::vector<double>> points_velocities{
583+
{{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
584+
std::vector<std::vector<double>> points_accelerations{
585+
{{0.1, 0.1, 0.1}}, {{0.5, 0.5, 0.5}}, {{0.6, 0.6, 0.6}}};
586+
// *INDENT-ON*
587+
588+
trajectory_msgs::msg::JointTrajectoryPoint error, current, desired;
589+
current.positions = {points[0].begin(), points[0].end()};
590+
current.velocities = {points_velocities[0].begin(), points_velocities[0].end()};
591+
current.accelerations = {points_accelerations[0].begin(), points_accelerations[0].end()};
592+
traj_controller_->resize_joint_trajectory_point(error, n_joints);
593+
594+
// zero error
595+
desired = current;
596+
for (size_t i = 0; i < n_joints; ++i)
597+
{
598+
traj_controller_->testable_compute_error_for_joint(error, i, current, desired);
599+
}
600+
EXPECT_NEAR(error.positions[0], 0., EPS);
601+
EXPECT_NEAR(error.positions[1], 0., EPS);
602+
EXPECT_NEAR(error.positions[2], 0., EPS);
603+
if (
604+
traj_controller_->has_velocity_state_interface() &&
605+
(traj_controller_->has_velocity_command_interface() ||
606+
traj_controller_->has_effort_command_interface()))
607+
{
608+
// error.velocities[index] = desired.velocities[index] - current.velocities[index];
609+
EXPECT_NEAR(error.velocities[0], 0., EPS);
610+
EXPECT_NEAR(error.velocities[1], 0., EPS);
611+
EXPECT_NEAR(error.velocities[2], 0., EPS);
612+
}
613+
if (
614+
traj_controller_->has_acceleration_state_interface() &&
615+
traj_controller_->has_acceleration_command_interface())
616+
{
617+
// error.accelerations[index] = desired.accelerations[index] - current.accelerations[index];
618+
EXPECT_NEAR(error.accelerations[0], 0., EPS);
619+
EXPECT_NEAR(error.accelerations[1], 0., EPS);
620+
EXPECT_NEAR(error.accelerations[2], 0., EPS);
621+
}
622+
623+
// no normalization of position error
624+
desired.positions[0] += 3.0 * M_PI_4;
625+
desired.velocities[0] += 1.0;
626+
desired.accelerations[0] += 1.0;
627+
traj_controller_->resize_joint_trajectory_point(error, n_joints);
628+
for (size_t i = 0; i < n_joints; ++i)
629+
{
630+
traj_controller_->testable_compute_error_for_joint(error, i, current, desired);
631+
}
632+
EXPECT_NEAR(error.positions[0], desired.positions[0] - current.positions[0], EPS);
633+
EXPECT_NEAR(error.positions[1], desired.positions[1] - current.positions[1], EPS);
634+
EXPECT_NEAR(error.positions[2], desired.positions[2] - current.positions[2], EPS);
635+
if (
636+
traj_controller_->has_velocity_state_interface() &&
637+
(traj_controller_->has_velocity_command_interface() ||
638+
traj_controller_->has_effort_command_interface()))
639+
{
640+
// error.velocities[index] = desired.velocities[index] - current.velocities[index];
641+
EXPECT_NEAR(error.velocities[0], desired.velocities[0] - current.velocities[0], EPS);
642+
EXPECT_NEAR(error.velocities[1], desired.velocities[1] - current.velocities[1], EPS);
643+
EXPECT_NEAR(error.velocities[2], desired.velocities[2] - current.velocities[2], EPS);
644+
}
645+
if (
646+
traj_controller_->has_acceleration_state_interface() &&
647+
traj_controller_->has_acceleration_command_interface())
648+
{
649+
// error.accelerations[index] = desired.accelerations[index] - current.accelerations[index];
650+
EXPECT_NEAR(error.accelerations[0], desired.accelerations[0] - current.accelerations[0], EPS);
651+
EXPECT_NEAR(error.accelerations[1], desired.accelerations[1] - current.accelerations[1], EPS);
652+
EXPECT_NEAR(error.accelerations[2], desired.accelerations[2] - current.accelerations[2], EPS);
653+
}
654+
655+
executor.cancel();
656+
}
657+
469658
/**
470659
* @brief check if position error of revolute joints are angle_wraparound if not configured so
471660
*/

‎joint_trajectory_controller/test/test_trajectory_controller_utils.hpp‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -108,6 +108,13 @@ class TestableJointTrajectoryController
108108

109109
void trigger_declare_parameters() { param_listener_->declare_params(); }
110110

111+
void testable_compute_error_for_joint(
112+
JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current,
113+
const JointTrajectoryPoint & desired)
114+
{
115+
compute_error_for_joint(error, index, current, desired);
116+
}
117+
111118
trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset() const
112119
{
113120
return last_commanded_state_;
@@ -155,6 +162,23 @@ class TestableJointTrajectoryController
155162
trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; }
156163
trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; }
157164

165+
/**
166+
* a copy of the private member function
167+
*/
168+
void resize_joint_trajectory_point(
169+
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size)
170+
{
171+
point.positions.resize(size, 0.0);
172+
if (has_velocity_state_interface_)
173+
{
174+
point.velocities.resize(size, 0.0);
175+
}
176+
if (has_acceleration_state_interface_)
177+
{
178+
point.accelerations.resize(size, 0.0);
179+
}
180+
}
181+
158182
rclcpp::WaitSet joint_cmd_sub_wait_set_;
159183
};
160184

0 commit comments

Comments
 (0)