motion.log uses absolute times

This is to plot multiple trajectories one after another

diff --git a/src/motion/trajectory.cc b/src/motion/trajectory.cc
index 4794f51d179d342d422095314bbb2924b146cd3d..d8fc04e2d374fcd298b3d086918aed12d66d3ec5 100644 (file)
--- a/src/motion/trajectory.cc
+++ b/src/motion/trajectory.cc
@@ -725,32 +725,6 @@ Trajectory::prepare(Pos _initPos)
     currentSeg = begin();
     prepared = true;
 
-    #ifdef MOTION_LOG
-    char c[200];
-    Pos p1, p2;
-    for (iterator seg = begin(); seg != end(); seg++) { // logging segment parameters
-       (*seg)->getRefPos((*seg)->getT1(), p1);
-       (*seg)->getRefPos((*seg)->getT2(), p2);
-       sprintf(c, "segment: begin %lf %lf, end  %lf %lf, angles %lf %lf, v1 %lf, v2 %lf, t1 %lf, t2 %lf, len %lf\n", 
-               p1.x, p1.y, p2.x, p2.y, p1.phi, p2.phi,(*seg)->v1, (*seg)->v2, (*seg)->getT1(), (*seg)->getT2(), (*seg)->getLength());
-       log(c);
-       sprintf(c, "gnuplot: %lf %lf %lf %lf %d\n", (*seg)->getT1(), p1.x, p1.y, p1.phi, 1); //gnuplot start point
-       log(c);
-       sprintf(c, "gnuplot: %lf %lf %lf %lf %d\n", (*seg)->getT2(), p2.x, p2.y, p2.phi, 3); //gnuplot end point
-       log(c);
-       }
-    iterator end_seg = --end();
-    double max_tm = (*end_seg)->getT2();
-    // for gnuplot
-    for(double tm = 0; tm <= max_tm; tm += 0.05) {
-       getRefPos(tm, p1);
-       sprintf(c, "gnuplot: %lf %lf %lf %lf %d\n", tm, p1.x, p1.y, p1.phi, 2); // gnuplot middle part
-        log(c);
-       }
-
-    log("trajectory prepared\n");
-    #endif
-
     return true;
 }
 
@@ -994,3 +968,30 @@ Trajectory::plotSpeed(const char *style)
 };
 #endif
 
+#ifdef MOTION_LOG
+void
+Trajectory::logTraj(double start_time)
+{
+    char c[200];
+    Pos p1, p2;
+    for (iterator seg = begin(); seg != end(); seg++) { // logging segment parameters
+       (*seg)->getRefPos((*seg)->getT1(), p1);
+       (*seg)->getRefPos((*seg)->getT2(), p2);
+       sprintf(c, "segment: begin %lf %lf, end  %lf %lf, angles %lf %lf, v1 %lf, v2 %lf, t1 %lf, t2 %lf, len %lf\n", 
+               p1.x, p1.y, p2.x, p2.y, p1.phi, p2.phi,(*seg)->v1, (*seg)->v2, (*seg)->getT1(), (*seg)->getT2(), (*seg)->getLength());
+       log(c);
+       sprintf(c, "gnuplot: %lf %lf %lf %lf %d\n", start_time+(*seg)->getT1(), p1.x, p1.y, p1.phi, 1); //gnuplot start point
+       log(c);
+       sprintf(c, "gnuplot: %lf %lf %lf %lf %d\n", start_time+(*seg)->getT2(), p2.x, p2.y, p2.phi, 3); //gnuplot end point
+       log(c);
+       }
+    iterator end_seg = --end();
+    double max_tm = (*end_seg)->getT2();
+    // for gnuplot
+    for(double tm = 0; tm <= max_tm; tm += 0.05) {
+       getRefPos(tm, p1);
+       sprintf(c, "gnuplot: %lf %lf %lf %lf %d\n", start_time+tm, p1.x, p1.y, p1.phi, 2); // gnuplot middle part
+        log(c);
+       }
+}
+#endif

diff --git a/src/motion/trgen.h b/src/motion/trgen.h
index 2f89c99a92be29e7a15651b8e033f4912a622e36..7dceecd3869c7b6381e61bddc8f44c925005083d 100644 (file)
--- a/src/motion/trgen.h
+++ b/src/motion/trgen.h
@@ -282,6 +282,10 @@ public:
     void plot(const char *style);
     void plotSpeed(const char *style);
 #endif
+#ifdef MOTION_LOG
+    void logTraj(double start_time);
+#endif
+
 };
 
 namespace Segment {

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index 1166d00a99de5c874a355c505e50f27bd26b1d25..4a36e7ac103b147fd41004a1157077d8cfd300f1 100644 (file)
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -413,6 +413,9 @@ void go(Trajectory *t, double append_time)
                old = actual_trajectory;
                gettimeofday(&tv_start, NULL);
                actual_trajectory = t;
+#ifdef MOTION_LOG              
+               t->logTraj(tv_start.tv_sec + 1e-6*tv_start.tv_usec);
+#endif
                if (old)
                        delete(old);
         }