diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 36ed39a7b9..d42838494a 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -72,7 +72,7 @@ class Trajectory * start_segment_itr = iterator before the sampled point, end_segment_itr = iterator after start_segment_itr * - Sampling after entire trajectory: * start_segment_itr = --end(), end_segment_itr = end() - * - Sampling empty msg: + * - Sampling empty msg or before the time given in set_point_before_trajectory_msg() * return false */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC @@ -83,6 +83,32 @@ class Trajectory TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr); + /** + * Do interpolation between 2 states given a time in between their respective timestamps + * + * The start and end states need not necessarily be specified all the way to the acceleration level: + * - If only \b positions are specified, linear interpolation will be used. + * - If \b positions and \b velocities are specified, a cubic spline will be used. + * - If \b positions, \b velocities and \b accelerations are specified, a quintic spline will be used. + * + * If start and end states have different specifications + * (eg. start is position-only, end is position-velocity), the lowest common specification will be used + * (position-only in the example). + * + * \param[in] time_a Time at which the segment state equals \p state_a. + * \param[in] state_a State at \p time_a. + * \param[in] time_b Time at which the segment state equals \p state_b. + * \param[in] state_b State at time \p time_b. + * \param[in] sample_time The time to sample, between time_a and time_b. + * \param[out] output The state at \p sample_time. + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void interpolate_between_points( + const rclcpp::Time & time_a, const trajectory_msgs::msg::JointTrajectoryPoint & state_a, + const rclcpp::Time & time_b, const trajectory_msgs::msg::JointTrajectoryPoint & state_b, + const rclcpp::Time & sample_time, + trajectory_msgs::msg::JointTrajectoryPoint & output); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC TrajectoryPointConstIter begin() const; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 200f9df14c..d191c62c1e 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -111,12 +111,7 @@ JointTrajectoryController::update() // error defined as the difference between current and desired error.positions[index] = angles::shortest_angular_distance( current.positions[index], desired.positions[index]); - - if (!desired.velocities.empty()) { - error.velocities[index] = desired.velocities[index] - current.velocities[index]; - } else { - error.velocities[index] = 0.0; - } + error.velocities[index] = desired.velocities[index] - current.velocities[index]; error.accelerations[index] = 0.0; }; @@ -178,7 +173,6 @@ JointTrajectoryController::update() default_tolerances_.goal_state_tolerance[index], false)) { outside_goal_tolerance = true; - break; } } @@ -194,10 +188,16 @@ JointTrajectoryController::update() rt_active_goal_->setFeedback(feedback); // check abort - if (abort) { - RCLCPP_WARN(lifecycle_node_->get_logger(), "Aborted due to state tolerance violation"); + if (abort || outside_goal_tolerance) { auto result = std::make_shared(); - result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); + + if (abort) { + RCLCPP_WARN(lifecycle_node_->get_logger(), "Aborted due to state tolerance violation"); + result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); + } else if (outside_goal_tolerance) { + RCLCPP_WARN(lifecycle_node_->get_logger(), "Aborted due to goal tolerance violation"); + result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); + } rt_active_goal_->setAborted(result); rt_active_goal_.reset(); } diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index a90399e788..54b151be98 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -43,6 +43,7 @@ Trajectory::Trajectory( trajectory_start_time_(static_cast(joint_trajectory->header.stamp)) { set_point_before_trajectory_msg(current_time, current_point); + update(joint_trajectory); } void @@ -70,6 +71,7 @@ Trajectory::sample( TrajectoryPointConstIter & end_segment_itr) { THROW_ON_NULLPTR(trajectory_msg_) + expected_state = trajectory_msgs::msg::JointTrajectoryPoint(); if (trajectory_msg_->points.empty()) { start_segment_itr = end(); @@ -86,49 +88,19 @@ Trajectory::sample( sampled_already_ = true; } - auto linear_interpolation = [&]( - const rclcpp::Time & time_a, const trajectory_msgs::msg::JointTrajectoryPoint & state_a, - const rclcpp::Time & time_b, const trajectory_msgs::msg::JointTrajectoryPoint & state_b, - const rclcpp::Time & sample_time, - trajectory_msgs::msg::JointTrajectoryPoint & output) - { - rclcpp::Duration duration_so_far = sample_time - time_a; - rclcpp::Duration duration_btwn_points = time_b - time_a; - double percent = duration_so_far.seconds() / duration_btwn_points.seconds(); - percent = rcppmath::clamp(percent, 0.0, 1.0); - - output.positions.resize(state_a.positions.size()); - for (auto i = 0ul; i < state_a.positions.size(); ++i) { - output.positions[i] = - state_a.positions[i] + percent * (state_b.positions[i] - state_a.positions[i]); - } - - if (!state_a.velocities.empty() && !state_b.velocities.empty()) { - output.velocities.resize(state_b.velocities.size()); - for (auto i = 0ul; i < state_b.velocities.size(); ++i) { - output.velocities[i] = - state_a.velocities[i] + percent * (state_b.velocities[i] - state_a.velocities[i]); - } - } - - if (!state_a.accelerations.empty() && !state_b.accelerations.empty()) { - output.accelerations.resize(state_b.accelerations.size()); - for (auto i = 0ul; i < state_b.accelerations.size(); ++i) { - output.accelerations[i] = - state_a.accelerations[i] + percent * - (state_b.accelerations[i] - state_a.accelerations[i]); - } - } - }; + // sampling before the current point + if (sample_time < time_before_traj_msg_) { + return false; + } - // current time hasn't reached traj time of the first msg yet + // current time hasn't reached traj time of the first point in the msg yet const auto & first_point_in_msg = trajectory_msg_->points[0]; const rclcpp::Duration offset = first_point_in_msg.time_from_start; const rclcpp::Time first_point_timestamp = trajectory_start_time_ + offset; if (sample_time < first_point_timestamp) { const rclcpp::Time t0 = time_before_traj_msg_; - linear_interpolation( + interpolate_between_points( t0, state_before_traj_msg_, first_point_timestamp, first_point_in_msg, sample_time, expected_state); start_segment_itr = begin(); // no segments before the first @@ -148,11 +120,7 @@ Trajectory::sample( const rclcpp::Time t1 = trajectory_start_time_ + t1_offset; if (sample_time >= t0 && sample_time < t1) { - // TODO(ddengster): Find a way to add custom interpolation implementations. - // Likely a lambda + parameters supplied from the controller would do - // do simple linear interpolation for now - // reference: https://github.com/ros-controls/ros_controllers/blob/melodic-devel/joint_trajectory_controller/include/trajectory_interface/quintic_spline_segment.h#L84 - linear_interpolation(t0, point, t1, next_point, sample_time, expected_state); + interpolate_between_points(t0, point, t1, next_point, sample_time, expected_state); start_segment_itr = begin() + i; end_segment_itr = begin() + (i + 1); return true; @@ -163,9 +131,146 @@ Trajectory::sample( start_segment_itr = --end(); end_segment_itr = end(); expected_state = (*start_segment_itr); + // the trajectories in msg may have empty velocities/accel, so resize them + if (expected_state.velocities.empty()) { + expected_state.velocities.resize(expected_state.positions.size(), 0.0); + } + if (expected_state.accelerations.empty()) { + expected_state.accelerations.resize(expected_state.positions.size(), 0.0); + } return true; } +void Trajectory::interpolate_between_points( + const rclcpp::Time & time_a, const trajectory_msgs::msg::JointTrajectoryPoint & state_a, + const rclcpp::Time & time_b, const trajectory_msgs::msg::JointTrajectoryPoint & state_b, + const rclcpp::Time & sample_time, + trajectory_msgs::msg::JointTrajectoryPoint & output) +{ + rclcpp::Duration duration_so_far = sample_time - time_a; + rclcpp::Duration duration_btwn_points = time_b - time_a; + + const size_t dim = state_a.positions.size(); + output.positions.resize(dim, 0.0); + output.velocities.resize(dim, 0.0); + output.accelerations.resize(dim, 0.0); + + auto generate_powers = [](int n, double x, double * powers) + { + powers[0] = 1.0; + for (int i = 1; i <= n; ++i) { + powers[i] = powers[i - 1] * x; + } + }; + + bool has_velocity = !state_a.velocities.empty() && !state_b.velocities.empty(); + bool has_accel = !state_a.accelerations.empty() && !state_b.accelerations.empty(); + if (duration_so_far.seconds() < 0.0) { + duration_so_far = rclcpp::Duration::from_seconds(0.0); + has_velocity = has_accel = false; + } + if (duration_so_far.seconds() > duration_btwn_points.seconds()) { + duration_so_far = duration_btwn_points; + has_velocity = has_accel = false; + } + + double t[6]; + generate_powers(5, duration_so_far.seconds(), t); + + if (!has_velocity && !has_accel) { + // do linear interpolation + for (size_t i = 0; i < dim; ++i) { + double start_pos = state_a.positions[i]; + double end_pos = state_b.positions[i]; + + double coefficients[2] = {0.0, 0.0}; + coefficients[0] = start_pos; + if (duration_btwn_points.seconds() != 0.0) { + coefficients[1] = (end_pos - start_pos) / duration_btwn_points.seconds(); + } + + output.positions[i] = t[0] * coefficients[0] + + t[1] * coefficients[1]; + output.velocities[i] = t[0] * coefficients[1]; + } + } else if (has_velocity && !has_accel) { + // do cubic interpolation + double T[4]; + generate_powers(3, duration_btwn_points.seconds(), T); + + for (size_t i = 0; i < dim; ++i) { + double start_pos = state_a.positions[i]; + double start_vel = state_a.velocities[i]; + double end_pos = state_b.positions[i]; + double end_vel = state_b.velocities[i]; + + double coefficients[4] = {0.0, 0.0, 0.0, 0.0}; + coefficients[0] = start_pos; + coefficients[1] = start_vel; + if (duration_btwn_points.seconds() != 0.0) { + coefficients[2] = + (-3.0 * start_pos + 3.0 * end_pos - 2.0 * start_vel * T[1] - end_vel * T[1]) / T[2]; + coefficients[3] = + ( 2.0 * start_pos - 2.0 * end_pos + start_vel * T[1] + end_vel * T[1]) / T[3]; + } + + output.positions[i] = t[0] * coefficients[0] + + t[1] * coefficients[1] + + t[2] * coefficients[2] + + t[3] * coefficients[3]; + output.velocities[i] = t[0] * coefficients[1] + + t[1] * 2.0 * coefficients[2] + + t[2] * 3.0 * coefficients[3]; + output.accelerations[i] = t[0] * 2.0 * coefficients[2] + + t[1] * 6.0 * coefficients[3]; + } + } else if (has_velocity && has_accel) { + // do quintic interpolation + double T[6]; + generate_powers(5, duration_btwn_points.seconds(), T); + + for (size_t i = 0; i < dim; ++i) { + double start_pos = state_a.positions[i]; + double start_vel = state_a.velocities[i]; + double start_acc = state_a.accelerations[i]; + double end_pos = state_b.positions[i]; + double end_vel = state_b.velocities[i]; + double end_acc = state_b.accelerations[i]; + + double coefficients[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + coefficients[0] = start_pos; + coefficients[1] = start_vel; + coefficients[2] = 0.5 * start_acc; + if (duration_btwn_points.seconds() != 0.0) { + coefficients[3] = + (-20.0 * start_pos + 20.0 * end_pos - 3.0 * start_acc * T[2] + end_acc * T[2] - + 12.0 * start_vel * T[1] - 8.0 * end_vel * T[1]) / (2.0 * T[3]); + coefficients[4] = + (30.0 * start_pos - 30.0 * end_pos + 3.0 * start_acc * T[2] - 2.0 * end_acc * T[2] + + 16.0 * start_vel * T[1] + 14.0 * end_vel * T[1]) / (2.0 * T[4]); + coefficients[5] = (-12.0 * start_pos + 12.0 * end_pos - start_acc * T[2] + end_acc * T[2] - + 6.0 * start_vel * T[1] - 6.0 * end_vel * T[1]) / (2.0 * T[5]); + } + + output.positions[i] = t[0] * coefficients[0] + + t[1] * coefficients[1] + + t[2] * coefficients[2] + + t[3] * coefficients[3] + + t[4] * coefficients[4] + + t[5] * coefficients[5]; + output.velocities[i] = t[0] * coefficients[1] + + t[1] * 2.0 * coefficients[2] + + t[2] * 3.0 * coefficients[3] + + t[3] * 4.0 * coefficients[4] + + t[4] * 5.0 * coefficients[5]; + output.accelerations[i] = t[0] * 2.0 * coefficients[2] + + t[1] * 6.0 * coefficients[3] + + t[2] * 12.0 * coefficients[4] + + t[3] * 20.0 * coefficients[5]; + } + } +} + TrajectoryPointConstIter Trajectory::begin() const { diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index 3ee67053ca..676387ac83 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include @@ -30,6 +31,10 @@ using namespace std::chrono_literals; +// Floating-point value comparison threshold +const double EPS = 1e-8; + + TEST(TestTrajectory, initialize_trajectory) { { auto empty_msg = std::make_shared(); @@ -65,87 +70,218 @@ TEST(TestTrajectory, initialize_trajectory) { } } -TEST(TestTrajectory, sample_trajectory) { +TEST(TestTrajectory, sample_trajectory_positions) { auto full_msg = std::make_shared(); - full_msg->header.stamp.sec = 0; - full_msg->header.stamp.nanosec = 0; + full_msg->header.stamp = rclcpp::Time(0); trajectory_msgs::msg::JointTrajectoryPoint p1; p1.positions.push_back(1.0); - p1.velocities.push_back(0.0); - p1.accelerations.push_back(0.0); - p1.time_from_start.sec = 1; - p1.time_from_start.nanosec = 0; + p1.time_from_start = rclcpp::Duration::from_seconds(1.0); + full_msg->points.push_back(p1); trajectory_msgs::msg::JointTrajectoryPoint p2; p2.positions.push_back(2.0); - p2.velocities.push_back(0.0); - p2.accelerations.push_back(0.0); - p2.time_from_start.sec = 2; - p2.time_from_start.nanosec = 0; + p2.time_from_start = rclcpp::Duration::from_seconds(2.0); + full_msg->points.push_back(p2); trajectory_msgs::msg::JointTrajectoryPoint p3; p3.positions.push_back(3.0); - p3.velocities.push_back(0.0); - p3.accelerations.push_back(0.0); - p3.time_from_start.sec = 3; - p3.time_from_start.nanosec = 0; - - full_msg->points.push_back(p1); - full_msg->points.push_back(p2); + p3.time_from_start = rclcpp::Duration::from_seconds(3.0); full_msg->points.push_back(p3); - // set current state before trajectory msg was sent - auto traj = joint_trajectory_controller::Trajectory(full_msg); - - trajectory_msgs::msg::JointTrajectoryPoint current_point; - current_point.time_from_start.sec = 0; - current_point.time_from_start.nanosec = 0; - current_point.positions.push_back(0.0); - current_point.velocities.push_back(0.0); - current_point.accelerations.push_back(0.0); + 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); + // set current state before trajectory msg was sent const rclcpp::Time time_now = rclcpp::Clock().now(); - traj.set_point_before_trajectory_msg(time_now, current_point); + auto traj = joint_trajectory_controller::Trajectory(time_now, point_before_msg, full_msg); trajectory_msgs::msg::JointTrajectoryPoint expected_state; joint_trajectory_controller::TrajectoryPointConstIter start, end; - traj.sample(time_now, expected_state, start, end); - ASSERT_EQ(traj.begin(), start); - ASSERT_EQ(traj.begin(), end); - EXPECT_EQ(0.0, expected_state.positions[0]); + double duration_first_seg = 1.0; + double velocity = (p1.positions[0] - point_before_msg.positions[0]) / duration_first_seg; - // sample before trajectory starts - traj.sample(time_now - rclcpp::Duration::from_seconds(0.5), expected_state, start, end); - ASSERT_EQ(traj.begin(), start); - ASSERT_EQ(traj.begin(), end); - EXPECT_EQ(0.0, expected_state.positions[0]); + // sample at trajectory starting time + { + traj.sample(time_now, expected_state, start, end); + ASSERT_EQ(traj.begin(), start); + ASSERT_EQ(traj.begin(), end); + EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR(velocity, expected_state.velocities[0], EPS); + EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); + } - traj.sample(time_now + rclcpp::Duration::from_seconds(0.5), expected_state, start, end); - ASSERT_EQ(traj.begin(), start); - ASSERT_EQ(traj.begin(), end); - EXPECT_EQ(0.5, expected_state.positions[0]); + // sample before time_now + { + bool result = + traj.sample(time_now - rclcpp::Duration::from_seconds(0.5), expected_state, start, end); + ASSERT_EQ(result, false); + } - traj.sample(time_now + rclcpp::Duration::from_seconds(1.0), expected_state, start, end); - ASSERT_EQ(traj.begin(), start); - ASSERT_EQ((++traj.begin()), end); - EXPECT_EQ(1.0, expected_state.positions[0]); + // sample 0.5s after msg + { + traj.sample(time_now + rclcpp::Duration::from_seconds(0.5), expected_state, start, end); + 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; + EXPECT_NEAR(half_current_to_p1, expected_state.positions[0], EPS); + EXPECT_NEAR(velocity, expected_state.velocities[0], EPS); + EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); + } - traj.sample(time_now + rclcpp::Duration::from_seconds(1.5), expected_state, start, end); - ASSERT_EQ(traj.begin(), start); - ASSERT_EQ((++traj.begin()), end); - EXPECT_EQ(1.5, expected_state.positions[0]); + // sample 1s after msg + { + traj.sample(time_now + rclcpp::Duration::from_seconds(1.0), expected_state, start, end); + ASSERT_EQ(traj.begin(), start); + ASSERT_EQ((++traj.begin()), end); + EXPECT_NEAR(p1.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR(velocity, expected_state.velocities[0], EPS); + EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); + } - traj.sample(time_now + rclcpp::Duration::from_seconds(2.5), expected_state, start, end); - EXPECT_EQ(2.5, expected_state.positions[0]); + // sample 1.5s after msg + { + traj.sample(time_now + rclcpp::Duration::from_seconds(1.5), expected_state, start, end); + ASSERT_EQ(traj.begin(), start); + ASSERT_EQ((++traj.begin()), end); + double half_p1_to_p2 = (p1.positions[0] + p2.positions[0]) * 0.5; + EXPECT_NEAR(half_p1_to_p2, expected_state.positions[0], EPS); + } - traj.sample(time_now + rclcpp::Duration::from_seconds(3.0), expected_state, start, end); - EXPECT_EQ(3.0, expected_state.positions[0]); + // sample 2.5s after msg + { + traj.sample(time_now + rclcpp::Duration::from_seconds(2.5), expected_state, start, end); + double half_p2_to_p3 = (p2.positions[0] + p3.positions[0]) * 0.5; + EXPECT_NEAR(half_p2_to_p3, expected_state.positions[0], EPS); + } + + // sample 3s after msg + { + traj.sample(time_now + rclcpp::Duration::from_seconds(3.0), expected_state, start, end); + EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); + } // sample past given points - traj.sample(time_now + rclcpp::Duration::from_seconds(3.125), expected_state, start, end); - ASSERT_EQ((--traj.end()), start); - ASSERT_EQ(traj.end(), end); - EXPECT_EQ(3.0, expected_state.positions[0]); + { + traj.sample(time_now + rclcpp::Duration::from_seconds(3.125), expected_state, start, end); + ASSERT_EQ((--traj.end()), start); + ASSERT_EQ(traj.end(), end); + EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); + } +} + +TEST(TestTrajectory, interpolation_pos_vel) { + // taken from ros1_controllers QuinticSplineSegmentTest::PosVelEnpointsSampler + + // Start and end state taken from x^3 - 2x + trajectory_msgs::msg::JointTrajectoryPoint start_state; + start_state.time_from_start = rclcpp::Duration::from_seconds(1.0); + start_state.positions.push_back(0.0); + start_state.velocities.push_back(-2.0); + start_state.accelerations.clear(); + + trajectory_msgs::msg::JointTrajectoryPoint end_state; + end_state.time_from_start = rclcpp::Duration::from_seconds(3.0); + end_state.positions.push_back(4.0); + end_state.velocities.push_back(10.0); + end_state.accelerations.push_back(0.0); // Should be ignored, start state does not specify it + + auto traj = joint_trajectory_controller::Trajectory(); + rclcpp::Time time_now(0); + + trajectory_msgs::msg::JointTrajectoryPoint expected_state; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + + // sample at start_time + { + traj.interpolate_between_points( + time_now + start_state.time_from_start, start_state, + time_now + end_state.time_from_start, end_state, + time_now + start_state.time_from_start, expected_state); + EXPECT_NEAR(start_state.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR(start_state.velocities[0], expected_state.velocities[0], EPS); + EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); + } + + // Sample at mid-segment: Zero-crossing + { + auto t = rclcpp::Duration::from_seconds(std::sqrt(2.0)); + traj.interpolate_between_points( + time_now + start_state.time_from_start, start_state, + time_now + end_state.time_from_start, end_state, + time_now + start_state.time_from_start + t, expected_state); + EXPECT_NEAR(0.0, expected_state.positions[0], EPS); + EXPECT_NEAR(4.0, expected_state.velocities[0], EPS); + EXPECT_NEAR(6.0 * std::sqrt(2.0), expected_state.accelerations[0], EPS); + } + + // sample at end_time + { + traj.interpolate_between_points( + time_now + start_state.time_from_start, start_state, + time_now + end_state.time_from_start, end_state, + time_now + end_state.time_from_start, expected_state); + EXPECT_NEAR(end_state.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR(end_state.velocities[0], expected_state.velocities[0], EPS); + EXPECT_NEAR(12.0, expected_state.accelerations[0], EPS); + } +} + +TEST(TestTrajectory, interpolation_pos_vel_accel) { + // taken from ros1_controllers QuinticSplineSegmentTest::PosVeAcclEnpointsSampler + + // Start and end state taken from x(x-1)(x-2)(x-3)(x-4) = x^5 -10x^4 + 35x^3 -50x^2 + 24x + trajectory_msgs::msg::JointTrajectoryPoint start_state; + start_state.time_from_start = rclcpp::Duration::from_seconds(1.0); + start_state.positions.push_back(0.0); + start_state.velocities.push_back(24.0); + start_state.accelerations.push_back(-100.0); + + trajectory_msgs::msg::JointTrajectoryPoint end_state; + end_state.time_from_start = rclcpp::Duration::from_seconds(3.0); + end_state.positions.push_back(0.0); + end_state.velocities.push_back(4.0); + end_state.accelerations.push_back(0.0); + + auto traj = joint_trajectory_controller::Trajectory(); + rclcpp::Time time_now(0); + + trajectory_msgs::msg::JointTrajectoryPoint expected_state; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + + // sample at start_time + { + traj.interpolate_between_points( + time_now + start_state.time_from_start, start_state, + time_now + end_state.time_from_start, end_state, + time_now + start_state.time_from_start, expected_state); + EXPECT_NEAR(start_state.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR(start_state.velocities[0], expected_state.velocities[0], EPS); + EXPECT_NEAR(start_state.accelerations[0], expected_state.accelerations[0], EPS); + } + + // Sample at mid-segment: Zero-crossing + { + auto t = rclcpp::Duration::from_seconds(1.0); + traj.interpolate_between_points( + time_now + start_state.time_from_start, start_state, + time_now + end_state.time_from_start, end_state, + time_now + start_state.time_from_start + t, expected_state); + EXPECT_NEAR(0.0, expected_state.positions[0], EPS); + EXPECT_NEAR(-6.0, expected_state.velocities[0], EPS); + EXPECT_NEAR(10.0, expected_state.accelerations[0], EPS); + } + + // sample at end_time + { + traj.interpolate_between_points( + time_now + start_state.time_from_start, start_state, + time_now + end_state.time_from_start, end_state, + time_now + end_state.time_from_start, expected_state); + EXPECT_NEAR(end_state.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR(end_state.velocities[0], expected_state.velocities[0], EPS); + EXPECT_NEAR(end_state.accelerations[0], expected_state.accelerations[0], EPS); + } } diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 40dad6c99b..28ae06d8cb 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -85,6 +85,10 @@ class TestTrajectoryActions : public ::testing::Test rclcpp::Parameter operation_mode_parameters("write_op_modes", op_mode_); traj_lifecycle_node_->set_parameter(operation_mode_parameters); + // ignore velocity tolerances for this test since they arent commited in test_robot->write() + rclcpp::Parameter stopped_velocity_parameters("constraints.stopped_velocity_tolerance", 0.0); + traj_lifecycle_node_->set_parameter(stopped_velocity_parameters); + goal_options_.goal_response_callback = std::bind(&TestTrajectoryActions::common_goal_response, this, _1); goal_options_.result_callback = @@ -270,7 +274,7 @@ TEST_F(TestTrajectoryActions, test_success_single_point_sendgoal) { { std::vector points; JointTrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(0.0); // start asap + point.time_from_start = rclcpp::Duration::from_seconds(0.5); point.positions.resize(joint_names_.size()); point.positions[0] = 1.0; @@ -352,7 +356,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) { { std::vector points; JointTrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(0.0); // start asap + point.time_from_start = rclcpp::Duration::from_seconds(0.5); point.positions.resize(joint_names_.size()); point.positions[0] = 1.0;