diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 091a82d716..e692abb6d1 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -8,3 +8,10 @@ This list summarizes important changes between Kilted Kaiju (previous) and Lyric force_torque_sensor_broadcaster ******************************* * Added support for transforming Wrench messages to a given list of target frames. This is useful when applications need force/torque data in their preferred coordinate frames. (`#2021 `__). + +joint_trajectory_controller +*************************** +* Fill in 0 velocities and accelerations into point before trajectories if the state interfaces + don't contain velocity / acceleration information, but the trajectory does. This way, the segment + up to the first waypoint will use the same interpolation as the rest of the trajectory. (`#2043 + `_) diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 86282e875e..ae9d8d5efe 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -50,6 +50,20 @@ void Trajectory::set_point_before_trajectory_msg( time_before_traj_msg_ = current_time; state_before_traj_msg_ = current_point; + // If the current state doesn't contain velocities / accelerations, but the first trajectory + // point does, initialize them to zero. Otherwise the segment going from the current state to the + // first trajectory point will use another degree of spline interpolation than the rest of the + // trajectory. + if (current_point.velocities.empty() && !trajectory_msg_->points[0].velocities.empty()) + { + state_before_traj_msg_.velocities.resize(trajectory_msg_->points[0].velocities.size(), 0.0); + } + if (current_point.accelerations.empty() && !trajectory_msg_->points[0].accelerations.empty()) + { + state_before_traj_msg_.accelerations.resize( + trajectory_msg_->points[0].accelerations.size(), 0.0); + } + // Compute offsets due to wrapping joints wraparound_joint( state_before_traj_msg_.positions, trajectory_msg_->points[0].positions, diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index 44f511aa31..fea7a2d024 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -725,6 +725,59 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) } } +TEST(TestTrajectory, fill_point_before_with_same_degree_as_traj) +{ + auto full_msg = std::make_shared(); + full_msg->header.stamp = rclcpp::Time(0); + + // definitions + double time_first_seg = 1.0; + double time_second_seg = 2.0; + double position_first_seg = 1.0; + double position_second_seg = 2.0; + double velocity_first_seg = 0.0; + double velocity_second_seg = 0.0; + double acceleration_first_seg = 0.0; + double acceleration_second_seg = 0.0; + + trajectory_msgs::msg::JointTrajectoryPoint p1; + p1.time_from_start = rclcpp::Duration::from_seconds(time_first_seg); + p1.positions.push_back(position_first_seg); + p1.velocities.push_back(velocity_first_seg); + p1.accelerations.push_back(acceleration_first_seg); + full_msg->points.push_back(p1); + + trajectory_msgs::msg::JointTrajectoryPoint p2; + p2.time_from_start = rclcpp::Duration::from_seconds(time_second_seg); + p2.positions.push_back(position_second_seg); + p2.velocities.push_back(velocity_second_seg); + p2.accelerations.push_back(acceleration_second_seg); + full_msg->points.push_back(p2); + + 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, full_msg); + + trajectory_msgs::msg::JointTrajectoryPoint expected_state; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + + // sample at trajectory starting time + // Since the trajectory defines positions, velocities, and accelerations, we expect quintic + // spline interpolation. Due to the unspecified initial acceleration, it should be zero. + { + 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); + EXPECT_NEAR(0.0, expected_state.velocities[0], EPS); + EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); + } +} + TEST(TestTrajectory, skip_interpolation) { // Simple passthrough without extra interpolation