diff --git a/trunk/fcl/include/fcl/motion.h b/trunk/fcl/include/fcl/motion.h
index 0df7beaf470943472cc0fd1598fe85b6b9d9f2c4..47a617e8dfbc58fcd41d42af07054043e198392c 100644
--- a/trunk/fcl/include/fcl/motion.h
+++ b/trunk/fcl/include/fcl/motion.h
@@ -84,7 +84,9 @@ public:
     if(dt > 1) dt = 1;
 
     t.setQuatRotation(absoluteRotation(dt));
-    t.setTranslation(p + axis * (dt * linear_vel) - t.getQuatRotation().transform(p));
+
+    SimpleQuaternion delta_rot = deltaRotation(dt);
+    t.setTranslation(p + axis * (dt * linear_vel) + delta_rot.transform(t1.getTranslation() - p));
 
     return true;
   }