always add current poisition as the first point in trajectory points

RethinkRobotics · Jul 23, 2016 · e1484b8 · e1484b8
1 parent bc76d5e
commit e1484b8
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/src/joint_trajectory_action/joint_trajectory_action.py b/src/joint_trajectory_action/joint_trajectory_action.py
@@ -361,7 +361,7 @@ def _on_trajectory_action(self, goal):
 
         dimensions_dict = self._determine_dimensions(trajectory_points)
 
-        if num_points == 1:
+        if num_points >= 1 and trajectory_points[0].time_from_start.to_sec() > 0:
             # Add current position as trajectory point
             first_trajectory_point = JointTrajectoryPoint()
             first_trajectory_point.positions = self._get_current_position(joint_names)