diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py
index 97493355..c2ad802a 100644
--- a/klippy/chelper/__init__.py
+++ b/klippy/chelper/__init__.py
@@ -83,6 +83,8 @@ defs_trapq = """
     struct trapq *trapq_alloc(void);
     void trapq_free(struct trapq *tq);
     void trapq_finalize_moves(struct trapq *tq, double print_time);
+    void trapq_set_position(struct trapq *tq, double print_time
+        , double pos_x, double pos_y, double pos_z);
     int trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
         , double start_time, double end_time);
 """
diff --git a/klippy/chelper/trapq.c b/klippy/chelper/trapq.c
index eaeb5949..2d4c2093 100644
--- a/klippy/chelper/trapq.c
+++ b/klippy/chelper/trapq.c
@@ -197,6 +197,23 @@ trapq_finalize_moves(struct trapq *tq, double print_time)
     }
 }
 
+// Note a position change in the trapq history
+void __visible
+trapq_set_position(struct trapq *tq, double print_time
+                   , double pos_x, double pos_y, double pos_z)
+{
+    // Flush all moves from trapq
+    trapq_finalize_moves(tq, NEVER_TIME);
+
+    // Add a marker to the trapq history
+    struct move *m = move_alloc();
+    m->print_time = print_time;
+    m->start_pos.x = pos_x;
+    m->start_pos.y = pos_y;
+    m->start_pos.z = pos_z;
+    list_add_head(&m->node, &tq->history);
+}
+
 // Return history of movement queue
 int __visible
 trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
diff --git a/klippy/chelper/trapq.h b/klippy/chelper/trapq.h
index 154fe04d..dbde8d3e 100644
--- a/klippy/chelper/trapq.h
+++ b/klippy/chelper/trapq.h
@@ -44,6 +44,8 @@ void trapq_free(struct trapq *tq);
 void trapq_check_sentinels(struct trapq *tq);
 void trapq_add_move(struct trapq *tq, struct move *m);
 void trapq_finalize_moves(struct trapq *tq, double print_time);
+void trapq_set_position(struct trapq *tq, double print_time
+                        , double pos_x, double pos_y, double pos_z);
 int trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
                       , double start_time, double end_time);
 
diff --git a/klippy/toolhead.py b/klippy/toolhead.py
index 3097c7dc..155848be 100644
--- a/klippy/toolhead.py
+++ b/klippy/toolhead.py
@@ -401,7 +401,9 @@ class ToolHead:
         return list(self.commanded_pos)
     def set_position(self, newpos, homing_axes=()):
         self.flush_step_generation()
-        self.trapq_finalize_moves(self.trapq, self.reactor.NEVER)
+        ffi_main, ffi_lib = chelper.get_ffi()
+        ffi_lib.trapq_set_position(self.trapq, self.print_time,
+                                   newpos[0], newpos[1], newpos[2])
         self.commanded_pos[:] = newpos
         self.kin.set_position(newpos, homing_axes)
         self.printer.send_event("toolhead:set_position")