Uses fast forward to correct the history

python/attime.py

@@ -71,6 +71,6 @@ class Calendar:
         return calendarDeco
 
     def fastForward(self,t):
-        for i in range(t): self.run(i)
+        for i in range(t+1): self.run(i)
 
 attime=Calendar()

python/history.py

@@ -23,7 +23,10 @@ class History:
         t= int(t/self.freq)
         print "robot.set(",self.q[t],")"
         print "robot.setVelocity(",self.qdot[t],")"
-        print "robot.state.time = ",t
+        print "T0 = ",t
+        print "robot.state.time = T0"
+        print "[ t.feature.position.recompute(T0) for t in taskrh,tasklh]"
+        print "attime.fastForward(T0)"
     def dumpToOpenHRP(self,baseName = "dyninv",sample = 1):
         filePos = open(baseName+'.pos','w')
         fileRPY = open(baseName+'.hip','w')