diff --git a/pr2eus_impedance/test/test-impedance.l b/pr2eus_impedance/test/test-impedance.l
index b6237a6ba..5511da8f2 100755
--- a/pr2eus_impedance/test/test-impedance.l
+++ b/pr2eus_impedance/test/test-impedance.l
@@ -26,6 +26,37 @@
   (send *ri* :stop-impedance :rarm)
   )
 
+  (send *ri* :start-impedance :rarm)
+  (send *ri* :start-impedance-trajectory
+        :rarm (list (list (make-coords :pos #f(500 0 700) :rpy (list 0 0 pi/2)) 2000.0
+                          :control-rot (send (make-coords) :worldrot)
+                          :target-force #f(1000 1000 1000 20 20 20)
+                          :force-control (list nil nil nil nil nil nil))
+                    (list (make-coords :pos #f(500 0 700) :rpy (list 0 0 pi/2)) 4000.0
+                          :control-rot (send (make-coords) :worldrot)
+                          :target-force #f(1000 1000 1000 20 20 20)
+                          :force-control (list nil nil nil nil nil nil))))
+  (send *ri* :wait-impedance-trajectory :rarm 10)
+
 (run-all-tests)
 (exit)
 
+(send *ri* :start-impedance-trajectory
+        :rarm (list (list (make-coords :pos #f(500 0 700) :rpy (list 0 0 pi/2)) 3000.0
+                          :control-rot (send (make-coords) :worldrot)
+                          :target-force #f(1000 1000 1000 20 20 20)
+                          :force-control (list nil nil nil nil nil nil))
+                    (list (make-coords :pos #f(500 0 700) :rpy (list 0 0 pi/2)) 3500.0
+                          :control-rot (send (make-coords) :worldrot)
+                          :target-force #f(1000 1000 1000 20 20 20)
+                          :force-control (list nil nil nil nil nil nil))))
+
+(send *ri* :start-impedance-trajectory
+        :rarm (list (list (make-coords :pos #f(500 0 700) :rpy (list 0 0 pi/2)) 500.0
+                          :control-rot (send (make-coords) :worldrot)
+                          :target-force #f(1000 1000 1000 20 20 20)
+                          :force-control (list nil nil nil nil nil nil))
+                    (list (make-coords :pos #f(750 0 700) :rpy (list 0 0 pi/2)) 8500.0
+                          :control-rot (send (make-coords) :worldrot)
+                          :target-force #f(1000 1000 1000 20 20 20)
+                          :force-control (list nil nil nil nil nil nil))))