Fix: Ignore first trajectory point in passthrough traj controller if time is 0

ur_controllers/src/passthrough_trajectory_controller.cpp

@@ -265,12 +265,21 @@
     active_joint_traj_ = active_goal->gh_->get_goal()->trajectory;
 
     if (current_index_ == 0 && current_transfer_state == TRANSFER_STATE_IDLE) {
+      size_t traj_size = active_joint_traj_.points.size();
+      if (active_joint_traj_.points[0].time_from_start.sec == 0 &&
+          active_joint_traj_.points[0].time_from_start.nanosec == 0) {
+        RCLCPP_WARN(get_node()->get_logger(),
+                    "First point of trajectory has time_from_start set to 0. This is not recommended, as it may lead "
+                    "to unexpected behavior. Removing the first point");
+        current_index_++;
+        traj_size--;
+      }
       active_trajectory_elapsed_time_ = rclcpp::Duration(0, 0);
       max_trajectory_time_ =
           rclcpp::Duration::from_seconds(duration_to_double(active_joint_traj_.points.back().time_from_start));
       write_success &= transfer_command_interface_->get().set_value(TRANSFER_STATE_NEW_TRAJECTORY);
       write_success &=
-          trajectory_size_command_interface_->get().set_value(static_cast<double>(active_joint_traj_.points.size()));
+          trajectory_size_command_interface_->get().set_value(static_cast<double>(traj_size));
     }
     auto active_goal_time_tol = goal_time_tolerance_.readFromRT();
     auto joint_mapping = joint_trajectory_mapping_.readFromRT();
@@ -547,6 +556,17 @@
                                                    << goal_handle->get_goal()->trajectory.points.size() << " points.");
   current_index_ = 0;
 
+  for (auto& point : goal_handle->get_goal()->trajectory.points) {
+    std::stringstream ss;
+    ss << "time_from_start: " << point.time_from_start.sec << "." << point.time_from_start.nanosec
+       << " sec, positions: " << std::setprecision(3) << std::fixed << "[";
+    for (const auto& pos : point.positions) {
+      ss << std::setw(8) << pos << ", ";
+    }
+    ss << "]";
+    RCLCPP_INFO_STREAM(get_node()->get_logger(), ss.str());
+  }
+
   // TODO(fexner): Merge goal tolerances with default tolerances
 
   joint_trajectory_mapping_.writeFromNonRT(create_joint_mapping(goal_handle->get_goal()->trajectory.joint_names));

ur_moveit_config/config/moveit_controllers.yaml

@@ -10,9 +10,11 @@ trajectory_execution:
 moveit_simple_controller_manager:
   controller_names:
     - scaled_joint_trajectory_controller
+    - passthrough_trajectory_controller
     - joint_trajectory_controller
 
-  scaled_joint_trajectory_controller:
+  #scaled_joint_trajectory_controller:
+  passthrough_trajectory_controller:
     action_ns: follow_joint_trajectory
     type: FollowJointTrajectory
     default: true

ur_robot_driver/launch/ur_control.launch.py

@@ -337,7 +337,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "initial_joint_controller",
-            default_value="scaled_joint_trajectory_controller",
+            default_value="passthrough_trajectory_controller",
             choices=[
                 "scaled_joint_trajectory_controller",
                 "joint_trajectory_controller",