[JTC] Allow integration of states in goal trajectories (ros-controls#190)

* Add allow_integration flag

* Added position and velocity deduction to trajectory.

* Added support for deduction of states from their derivatives.

* Apply suggestions from code review

Co-authored-by: Victor Lopez <3469405+v-lopez@users.noreply.github.com>

* Correct warnings and formating.

* Update test to use more human-readable values.

* Apply reviewers comments.

* Last test fixes.

Co-authored-by: Denis Štogl <destogl@users.noreply.github.com>
Co-authored-by: Victor Lopez <3469405+v-lopez@users.noreply.github.com>

Author: destogl

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -129,6 +129,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   /// This is useful when robot is not exactly following the commanded trajectory.
   bool open_loop_control_ = false;
   trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_;
+  /// Allow integration in goal trajectories to accept goals without position or velocity specified
+  bool allow_integration_in_goal_trajectories_ = false;
 
   // The interfaces are defined as the types in 'allowed_interface_types_' member.
   // For convenience, for each type the interfaces are ordered so that i-th position

joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp

@@ -58,18 +58,27 @@ class Trajectory
   /// Find the segment (made up of 2 points) and its expected state from the
   /// containing 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()
-  * - Sampling exactly on a point of the trajectory:
-  *    start_segment_itr = iterator where point is, end_segment_itr = iterator after start_segment_itr
-  * - Sampling between points:
-  *    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 or before the time given in set_point_before_trajectory_msg()
-  *    return false
-  */
+   * Sampling trajectory at given \p sample_time.
+   * If position in the \p end_segment_itr is missing it will be deduced from provided velocity, or acceleration respectively.
+   * Deduction assumes that the provided velocity or acceleration have to be reached at the time defined in the segment.
+   *
+   * Specific case returns for start_segment_itr and end_segment_itr:
+   * - Sampling before the trajectory start:
+   *   start_segment_itr = begin(), end_segment_itr = begin()
+   * - Sampling exactly on a point of the trajectory:
+   *    start_segment_itr = iterator where point is, end_segment_itr = iterator after start_segment_itr
+   * - Sampling between points:
+   *    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 or before the time given in set_point_before_trajectory_msg()
+   *    return false
+   *
+   * \param[in] sample_time Time at which trajectory will be sampled.
+   * \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.
+   */
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   bool sample(
     const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & output_state,
@@ -125,6 +134,11 @@ class Trajectory
   bool is_sampled_already() const { return sampled_already_; }
 
 private:
+  void deduce_from_derivatives(
+    trajectory_msgs::msg::JointTrajectoryPoint & first_state,
+    trajectory_msgs::msg::JointTrajectoryPoint & second_state, const size_t dim,
+    const double delta_t);
+
   std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg_;
   rclcpp::Time trajectory_start_time_;
 

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -62,6 +62,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init()
     auto_declare<double>("action_monitor_rate", 20.0);
     auto_declare<bool>("allow_partial_joints_goal", allow_partial_joints_goal_);
     auto_declare<bool>("open_loop_control", open_loop_control_);
+    auto_declare<bool>(
+      "allow_integration_in_goal_trajectories", allow_integration_in_goal_trajectories_);
     auto_declare<double>("constraints.stopped_velocity_tolerance", 0.01);
     auto_declare<double>("constraints.goal_time", 0.0);
   }
@@ -138,6 +140,7 @@ controller_interface::return_type JointTrajectoryController::update(
   {
     fill_partial_goal(*new_external_msg);
     sort_to_local_joint_order(*new_external_msg);
+    // TODO(denis): Add here integration of position and velocity
     traj_external_point_ptr_->update(*new_external_msg);
   }
 
@@ -178,7 +181,6 @@ controller_interface::return_type JointTrajectoryController::update(
 
     // find segment for current timestamp
     TrajectoryPointConstIter start_segment_itr, end_segment_itr;
-    // TODO(anyone): this is kind-of open-loop concept? I am right?
     const bool valid_point =
       (*traj_point_active_ptr_)->sample(time, state_desired, start_segment_itr, end_segment_itr);
 
@@ -659,6 +661,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
 
   // Read parameters customizing controller for special cases
   open_loop_control_ = node_->get_parameter("open_loop_control").get_value<bool>();
+  allow_integration_in_goal_trajectories_ =
+    node_->get_parameter("allow_integration_in_goal_trajectories").get_value<bool>();
 
   // subscriber callback
   // non realtime
@@ -1205,7 +1209,27 @@ bool JointTrajectoryController::validate_trajectory_msg(
 
     const size_t joint_count = trajectory.joint_names.size();
     const auto & points = trajectory.points;
-    if (
+    // This currently supports only position, velocity and acceleration inputs
+    if (allow_integration_in_goal_trajectories_)
+    {
+      const bool all_empty = points[i].positions.empty() && points[i].velocities.empty() &&
+                             points[i].accelerations.empty();
+      const bool position_error =
+        !points[i].positions.empty() &&
+        !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false);
+      const bool velocity_error =
+        !points[i].velocities.empty() &&
+        !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, false);
+      const bool acceleration_error =
+        !points[i].accelerations.empty() &&
+        !validate_trajectory_point_field(
+          joint_count, points[i].accelerations, "accelerations", i, false);
+      if (all_empty || position_error || velocity_error || acceleration_error)
+      {
+        return false;
+      }
+    }
+    else if (
       !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false) ||
       !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, true) ||
       !validate_trajectory_point_field(

joint_trajectory_controller/src/trajectory.cpp

@@ -89,16 +89,21 @@ bool Trajectory::sample(
   }
 
   // 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;
+  auto & first_point_in_msg = trajectory_msg_->points[0];
+  const rclcpp::Time first_point_timestamp =
+    trajectory_start_time_ + first_point_in_msg.time_from_start;
+
   if (sample_time < first_point_timestamp)
   {
-    const rclcpp::Time t0 = time_before_traj_msg_;
+    // it changes points only if position and velocity are not exist, but their derivatives
+    deduce_from_derivatives(
+      state_before_traj_msg_, first_point_in_msg, state_before_traj_msg_.positions.size(),
+      (first_point_timestamp - time_before_traj_msg_).seconds());
 
     interpolate_between_points(
-      t0, state_before_traj_msg_, first_point_timestamp, first_point_in_msg, sample_time,
-      output_state);
+      time_before_traj_msg_, state_before_traj_msg_, first_point_timestamp, first_point_in_msg,
+      sample_time, output_state);
+
     start_segment_itr = begin();  // no segments before the first
     end_segment_itr = begin();
     return true;
@@ -108,17 +113,20 @@ bool Trajectory::sample(
   const auto last_idx = trajectory_msg_->points.size() - 1;
   for (size_t i = 0; i < last_idx; ++i)
   {
-    const auto & point = trajectory_msg_->points[i];
-    const auto & next_point = trajectory_msg_->points[i + 1];
+    auto & point = trajectory_msg_->points[i];
+    auto & next_point = trajectory_msg_->points[i + 1];
 
-    const rclcpp::Duration t0_offset = point.time_from_start;
-    const rclcpp::Duration t1_offset = next_point.time_from_start;
-    const rclcpp::Time t0 = trajectory_start_time_ + t0_offset;
-    const rclcpp::Time t1 = trajectory_start_time_ + t1_offset;
+    const rclcpp::Time t0 = trajectory_start_time_ + point.time_from_start;
+    const rclcpp::Time t1 = trajectory_start_time_ + next_point.time_from_start;
 
     if (sample_time >= t0 && sample_time < t1)
     {
+      // it changes points only if position and velocity are not exist, but their derivatives
+      deduce_from_derivatives(
+        point, next_point, state_before_traj_msg_.positions.size(), (t1 - t0).seconds());
+
       interpolate_between_points(t0, point, t1, next_point, sample_time, output_state);
+
       start_segment_itr = begin() + i;
       end_segment_itr = begin() + (i + 1);
       return true;
@@ -272,6 +280,41 @@ void Trajectory::interpolate_between_points(
   }
 }
 
+void Trajectory::deduce_from_derivatives(
+  trajectory_msgs::msg::JointTrajectoryPoint & first_state,
+  trajectory_msgs::msg::JointTrajectoryPoint & second_state, const size_t dim, const double delta_t)
+{
+  if (second_state.positions.empty())
+  {
+    second_state.positions.resize(dim);
+    if (first_state.velocities.empty())
+    {
+      first_state.velocities.resize(dim, 0.0);
+    }
+    if (second_state.velocities.empty())
+    {
+      second_state.velocities.resize(dim);
+      if (first_state.accelerations.empty())
+      {
+        first_state.accelerations.resize(dim, 0.0);
+      }
+      for (size_t i = 0; i < dim; ++i)
+      {
+        second_state.velocities[i] =
+          first_state.velocities[i] +
+          (first_state.accelerations[i] + second_state.accelerations[i]) * 0.5 * delta_t;
+      }
+    }
+    for (size_t i = 0; i < dim; ++i)
+    {
+      // second state velocity should be reached on the end of the segment, so use middle
+      second_state.positions[i] =
+        first_state.positions[i] +
+        (first_state.velocities[i] + second_state.velocities[i]) * 0.5 * delta_t;
+    }
+  }
+}
+
 TrajectoryPointConstIter Trajectory::begin() const
 {
   THROW_ON_NULLPTR(trajectory_msg_)