Add test for wraparound_joint method

ros-controls · Nov 15, 2023 · 43d03c1 · 43d03c1
1 parent b0854ff
commit 43d03c1
Showing 1 changed file with 51 additions and 0 deletions.
diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp
@@ -821,3 +821,54 @@ TEST(TestTrajectory, skip_interpolation)
     }
   }
 }
+
+// test wraparound_joint method
+TEST(TestTrajectory, test_wraparound_joint)
+{
+  const std::vector<double> initial_position(3, 0.);
+  std::vector<double> next_position(3, M_PI * 3. / 2.);
+
+  // no wraparound
+  {
+    std::vector<double> current_position(initial_position);
+    std::vector<bool> joints_angle_wraparound(3, false);
+    joint_trajectory_controller::wraparound_joint(
+      current_position, next_position, joints_angle_wraparound);
+    EXPECT_EQ(current_position[0], initial_position[0]);
+    EXPECT_EQ(current_position[1], initial_position[1]);
+    EXPECT_EQ(current_position[2], initial_position[2]);
+  }
+
+  // wraparound of a single joint
+  {
+    std::vector<double> current_position(initial_position);
+    std::vector<bool> joints_angle_wraparound{true, false, false};
+    joint_trajectory_controller::wraparound_joint(
+      current_position, next_position, joints_angle_wraparound);
+    EXPECT_EQ(current_position[0], initial_position[0] + 2 * M_PI);
+    EXPECT_EQ(current_position[1], initial_position[1]);
+    EXPECT_EQ(current_position[2], initial_position[2]);
+  }
+
+  // wraparound of all joints
+  {
+    std::vector<double> current_position(initial_position);
+    std::vector<bool> joints_angle_wraparound(3, true);
+    joint_trajectory_controller::wraparound_joint(
+      current_position, next_position, joints_angle_wraparound);
+    EXPECT_EQ(current_position[0], initial_position[0] + 2 * M_PI);
+    EXPECT_EQ(current_position[1], initial_position[1] + 2 * M_PI);
+    EXPECT_EQ(current_position[2], initial_position[2] + 2 * M_PI);
+  }
+
+  // wraparound of all joints, but no offset
+  {
+    std::vector<double> current_position(next_position);
+    std::vector<bool> joints_angle_wraparound(3, true);
+    joint_trajectory_controller::wraparound_joint(
+      current_position, next_position, joints_angle_wraparound);
+    EXPECT_EQ(current_position[0], next_position[0]);
+    EXPECT_EQ(current_position[1], next_position[1]);
+    EXPECT_EQ(current_position[2], next_position[2]);
+  }
+}