Fill point_before_trajectory with same information as trajectory (#2043)

Author: fmauch

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 <https://github.com/ros-controls/ros2_controllers/pull/2021/files>`__).
+
+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
+  <https://github.com/ros-controls/ros2_controllers/pull/2043>`_)

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,

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<trajectory_msgs::msg::JointTrajectory>();
+  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