From 2c24549851699939ec29236cffba09e79a8689e7 Mon Sep 17 00:00:00 2001 From: RobertWilbrandt Date: Thu, 7 Nov 2024 09:06:38 +0100 Subject: [PATCH] [jtc] Improve trajectory sampling efficiency (#1297) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --------- Co-authored-by: Christoph Fröhlich Co-authored-by: Bence Magyar (cherry picked from commit b0391e2069c8ec9a94ce35cb3e395bac3ca62eb6) --- .../trajectory.hpp | 21 +++- .../src/trajectory.cpp | 5 +- .../test/test_trajectory.cpp | 115 ++++++++++++++++++ 3 files changed, 137 insertions(+), 4 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index b00d79481c..14373b006e 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -69,6 +69,9 @@ class Trajectory * acceleration respectively. Deduction assumes that the provided velocity or acceleration have to * be reached at the time defined in the segment. * + * This function assumes that sampling is only done at monotonically increasing \p sample_time + * for any trajectory. + * * Specific case returns for start_segment_itr and end_segment_itr: * - Sampling before the trajectory start: * start_segment_itr = begin(), end_segment_itr = begin() @@ -85,9 +88,12 @@ class Trajectory * * \param[in] sample_time Time at which trajectory will be sampled. * \param[in] interpolation_method Specify whether splines, another method, or no interpolation at - * all. \param[out] expected_state Calculated new at \p sample_time. \param[out] start_segment_itr - * Iterator to the start segment for given \p sample_time. See description above. \param[out] - * end_segment_itr Iterator to the end segment for given \p sample_time. See description above. + * all. + * \param[out] output_state Calculated new at \p sample_time. + * \param[out] start_segment_itr Iterator to the start segment for given \p sample_time. See + * description above. + * \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See + * description above. */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool sample( @@ -147,6 +153,14 @@ class Trajectory JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool is_sampled_already() const { return sampled_already_; } + /// Get the index of the segment start returned by the last \p sample() operation. + /** + * As the trajectory is only accessed at monotonically increasing sampling times, this index is + * used to speed up the selection of relevant trajectory points. + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + size_t last_sample_index() const { return last_sample_idx_; } + private: void deduce_from_derivatives( trajectory_msgs::msg::JointTrajectoryPoint & first_state, @@ -160,6 +174,7 @@ class Trajectory trajectory_msgs::msg::JointTrajectoryPoint state_before_traj_msg_; bool sampled_already_ = false; + size_t last_sample_idx_ = 0; }; /** diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 0ed7f2ff13..872cbe8357 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -85,6 +85,7 @@ void Trajectory::update(std::shared_ptr j trajectory_msg_ = joint_trajectory; trajectory_start_time_ = static_cast(joint_trajectory->header.stamp); sampled_already_ = false; + last_sample_idx_ = 0; } bool Trajectory::sample( @@ -150,7 +151,7 @@ bool Trajectory::sample( // time_from_start + trajectory time is the expected arrival time of trajectory const auto last_idx = trajectory_msg_->points.size() - 1; - for (size_t i = 0; i < last_idx; ++i) + for (size_t i = last_sample_idx_; i < last_idx; ++i) { auto & point = trajectory_msg_->points[i]; auto & next_point = trajectory_msg_->points[i + 1]; @@ -176,6 +177,7 @@ bool Trajectory::sample( } start_segment_itr = begin() + i; end_segment_itr = begin() + (i + 1); + last_sample_idx_ = i; return true; } } @@ -183,6 +185,7 @@ bool Trajectory::sample( // whole animation has played out start_segment_itr = --end(); end_segment_itr = end(); + last_sample_idx_ = last_idx; output_state = (*start_segment_itr); // the trajectories in msg may have empty velocities/accel, so resize them if (output_state.velocities.empty()) diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index 6e0c53ac77..eb12cda175 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -47,10 +47,12 @@ TEST(TestTrajectory, initialize_trajectory) empty_msg->header.stamp.nanosec = 2; const rclcpp::Time empty_time = empty_msg->header.stamp; auto traj = joint_trajectory_controller::Trajectory(empty_msg); + EXPECT_EQ(0, traj.last_sample_index()); trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.end(), start); EXPECT_EQ(traj.end(), end); @@ -67,6 +69,7 @@ TEST(TestTrajectory, initialize_trajectory) trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.end(), start); EXPECT_EQ(traj.end(), end); @@ -112,6 +115,7 @@ TEST(TestTrajectory, sample_trajectory_positions) // sample at trajectory starting time { traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -124,6 +128,7 @@ TEST(TestTrajectory, sample_trajectory_positions) bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(result, false); } @@ -132,6 +137,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); double half_current_to_p1 = (point_before_msg.positions[0] + p1.positions[0]) * 0.5; @@ -145,6 +151,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p1.positions[0], expected_state.positions[0], EPS); @@ -157,6 +164,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); double half_p1_to_p2 = (p1.positions[0] + p2.positions[0]) * 0.5; @@ -168,6 +176,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); double half_p2_to_p3 = (p2.positions[0] + p3.positions[0]) * 0.5; EXPECT_NEAR(half_p2_to_p3, expected_state.positions[0], EPS); } @@ -177,6 +186,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -185,6 +195,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); @@ -196,6 +207,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(30.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); @@ -353,6 +365,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) // sample at trajectory starting time { traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(traj.last_sample_index(), 0); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -365,6 +378,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(result, false); } @@ -373,6 +387,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); double half_current_to_p1 = @@ -393,6 +408,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -407,6 +423,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); double half_p1_to_p2 = @@ -427,6 +444,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(2), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); EXPECT_NEAR(position_second_seg, expected_state.positions[0], EPS); @@ -441,6 +459,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); double half_p2_to_p3 = @@ -461,6 +480,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -474,6 +494,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -526,6 +547,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho // sample at trajectory starting time { traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -539,6 +561,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(result, false); } @@ -547,6 +570,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); // double half_current_to_p1 = point_before_msg.positions[0] + @@ -567,6 +591,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -618,6 +643,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) // sample at trajectory starting time { traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -631,6 +657,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(result, false); } @@ -645,6 +672,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -661,6 +689,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(2), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); EXPECT_NEAR(position_second_seg, expected_state.positions[0], EPS); @@ -677,6 +706,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -689,6 +719,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -735,6 +766,7 @@ TEST(TestTrajectory, skip_interpolation) // sample at trajectory starting time { traj.sample(time_now, no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -750,6 +782,7 @@ TEST(TestTrajectory, skip_interpolation) bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(result, false); } @@ -758,6 +791,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); // For passthrough, this should just return the first waypoint @@ -774,6 +808,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p2.positions[0], expected_state.positions[0], EPS); @@ -789,6 +824,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p2.positions[0], expected_state.positions[0], EPS); @@ -799,6 +835,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), no_interpolation, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -807,6 +844,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), no_interpolation, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -815,6 +853,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), no_interpolation, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); @@ -822,6 +861,82 @@ TEST(TestTrajectory, skip_interpolation) } } +TEST(TestTrajectory, update_trajectory) +{ + // Verify that sampling works correctly after updating with a new trajectory + auto first_msg = std::make_shared(); + first_msg->header.stamp = rclcpp::Time(0); + + trajectory_msgs::msg::JointTrajectoryPoint p1; + p1.positions.push_back(1.0); + p1.time_from_start = rclcpp::Duration::from_seconds(1.0); + first_msg->points.push_back(p1); + + trajectory_msgs::msg::JointTrajectoryPoint p2; + p2.positions.push_back(2.0); + p2.time_from_start = rclcpp::Duration::from_seconds(2.0); + first_msg->points.push_back(p2); + + trajectory_msgs::msg::JointTrajectoryPoint p3; + p3.positions.push_back(3.0); + p3.time_from_start = rclcpp::Duration::from_seconds(3.0); + first_msg->points.push_back(p3); + + trajectory_msgs::msg::JointTrajectoryPoint point_before_msg; + point_before_msg.time_from_start = rclcpp::Duration::from_seconds(0.0); + point_before_msg.positions.push_back(0.0); + + const rclcpp::Time time_now = rclcpp::Clock().now(); + auto traj = joint_trajectory_controller::Trajectory(time_now, point_before_msg, first_msg); + EXPECT_EQ(0, traj.last_sample_index()); + + trajectory_msgs::msg::JointTrajectoryPoint expected_state; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + + // Sample at starting time + traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); + + // Sample 2.5s after msg + traj.sample( + time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ(1, traj.last_sample_index()); + + // Update trajectory + auto snd_msg = std::make_shared(); + snd_msg->header.stamp = rclcpp::Time(0); + + snd_msg->points.push_back(p1); + snd_msg->points.push_back(p2); + snd_msg->points.push_back(p3); + + traj.update(snd_msg); + + // Sample at starting time + { + traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ(traj.begin(), end); + EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR( + (p1.positions[0] - point_before_msg.positions[0]), expected_state.velocities[0], EPS); + EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); + } + + // Sample 1.5s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ(0, traj.last_sample_index()); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ(std::next(traj.begin()), end); + EXPECT_NEAR((p1.positions[0] + p2.positions[0]) / 2, expected_state.positions[0], EPS); + } +} + TEST(TestWrapAroundJoint, no_wraparound) { const std::vector initial_position(3, 0.);