added the feature to plot 2D trajectories; see bipedStand for example.

apps/bipedStand/MyWindow.cpp

@@ -134,6 +134,9 @@ void MyWindow::keyboard(unsigned char _key, int _x, int _y) {
       mImpulseDuration = 100.0;
       std::cout << "push left" << std::endl;
       break;
+    case 'g':
+      plotCOMX();
+      break;
     default:
       Win3D::keyboard(_key, _x, _y);
   }
@@ -144,3 +147,21 @@ void MyWindow::setController(Controller* _controller) {
   mController = _controller;
 }
 
+void MyWindow::plotCOMX() {
+  int nFrame = mWorld->getRecording()->getNumFrames();
+  Eigen::VectorXd data(nFrame);
+  for (int i = 0; i < nFrame; i++) {
+    Eigen::VectorXd pose = mWorld->getRecording()->getConfig(i, 1);
+    mWorld->getSkeleton(1)->setPositions(pose);
+    mWorld->getSkeleton(1)->computeForwardKinematics(true, true, false);    
+    data[i] = mWorld->getSkeleton(1)->getWorldCOM()[0];
+  }
+  if (nFrame != 0)
+  {
+    Eigen::VectorXd pose = mWorld->getRecording()->getConfig(mPlayFrame, 1);
+    mWorld->getSkeleton(1)->setPositions(pose);
+    mWorld->getSkeleton(1)->computeForwardKinematics(true, true, false);
+  }
+
+  plot(data);
+}

apps/bipedStand/MyWindow.h

@@ -58,6 +58,7 @@ class MyWindow : public dart::gui::SimWindow {
   void setController(Controller* _controller);
 
 private:
+  void plotCOMX();
   Eigen::Vector3d mForce;
   Controller* mController;
   int mImpulseDuration;

dart/gui/GLFuncs.cpp

@@ -47,7 +47,8 @@
 namespace dart {
 namespace gui {
 
-void drawStringOnScreen(float _x, float _y, const std::string& _s) {
+  void drawStringOnScreen(float _x, float _y, const std::string& _s,
+                          bool _bigFont) {
   // draws text on the screen
   GLint oldMode;
   glGetIntegerv(GL_MATRIX_MODE, &oldMode);
@@ -62,8 +63,12 @@ void drawStringOnScreen(float _x, float _y, const std::string& _s) {
   glLoadIdentity();
   glRasterPos2f(_x, _y);
   unsigned int length = _s.length();
-  for (unsigned int c = 0; c < length; c++)
-    glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18, _s.at(c) );
+  for (unsigned int c = 0; c < length; c++) {
+    if (_bigFont)
+      glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18, _s.at(c) );
+    else
+      glutBitmapCharacter(GLUT_BITMAP_HELVETICA_10, _s.at(c) );
+  }  
   glPopMatrix();
 
   glMatrixMode(GL_PROJECTION);

dart/gui/GLFuncs.h

@@ -46,7 +46,8 @@ namespace dart {
 namespace gui {
 
 /// \brief
-void drawStringOnScreen(float _x, float _y, const std::string& _s);
+  void drawStringOnScreen(float _x, float _y, const std::string& _s,
+                          bool _bigFont = true);
 
 /// \brief
 void drawArrow3D(const Eigen::Vector3d& _pt, const Eigen::Vector3d& _dir,

dart/gui/GraphWindow.cpp

@@ -0,0 +1,133 @@
+/*
+ * Copyright (c) 2015, Georgia Tech Research Corporation
+ * All rights reserved.
+ *
+ * Author(s): Karen Liu <karenliu@cc.gatech.edu>
+ *
+ * Georgia Tech Graphics Lab and Humanoid Robotics Lab
+ *
+ * Directed by Prof. C. Karen Liu and Prof. Mike Stilman
+ * <karenliu@cc.gatech.edu> <mstilman@cc.gatech.edu>
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * This code incorporates portions of Open Dynamics Engine
+ *     (Copyright (c) 2001-2004, Russell L. Smith. All rights
+ *     reserved.) and portions of FCL (Copyright (c) 2011, Willow
+ *     Garage, Inc. All rights reserved.), which were released under
+ *     the same BSD license as below
+ *
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "dart/gui/GraphWindow.h"
+
+#include <cstdio>
+#include <iostream>
+#include <string>
+
+#include "dart/gui/GLFuncs.h"
+
+namespace dart {
+namespace gui {
+
+GraphWindow::GraphWindow()
+  : Win2D() {
+  mBackground[0] = 1.0;
+  mBackground[1] = 1.0;
+  mBackground[2] = 1.0;
+  mBackground[3] = 1.0;
+}
+
+GraphWindow::~GraphWindow() {
+}
+
+void GraphWindow::draw() {
+  mRI->setPenColor(Eigen::Vector3d(0.2, 0.8, 0.2));
+  glPointSize(2);
+  glMatrixMode(GL_MODELVIEW);
+
+  int nPoints = mData.size();
+
+  double upperBound = +1.0;
+  double lowerBound = -1.0;
+  if (nPoints > 0) {
+    upperBound = mData.maxCoeff();
+    lowerBound = mData.minCoeff();
+  }
+
+  for (int i = 0; i < nPoints; i++) {
+    glPushMatrix();
+    glLoadIdentity();
+    glBegin(GL_POINTS);
+    glVertex2f(i / (double)nPoints * mWinWidth - mWinWidth / 2.0, mWinHeight * (mData[i] - lowerBound) / (upperBound - lowerBound) - mWinHeight / 2.0);
+    glEnd();
+    glPopMatrix();
+  }    
+  glMatrixMode(GL_PROJECTION);
+
+  double xPos = 0.1;
+  while (xPos < 1.0) {
+    char buff[64];
+    int v = xPos * nPoints;
+#ifdef WIN32
+    _snprintf(buff, sizeof(buff), "%d", v);
+#else
+    std::snprintf(buff, sizeof(buff), "%d", v);
+#endif
+    std::string frame(buff);
+    glColor3f(0.0, 0.0, 0.0);
+    gui::drawStringOnScreen(xPos, 0.01f, frame, false);
+    xPos += 0.2;
+  }
+
+  double yPos = 0.1;
+  while (yPos < 1.0) {
+    char buff[64];
+    double v = yPos * (upperBound - lowerBound) + lowerBound;
+#ifdef WIN32
+    _snprintf(buff, sizeof(buff), "%.2e", v);
+#else
+    std::snprintf(buff, sizeof(buff), "%.2e", v);
+#endif
+    std::string frame(buff);
+    glColor3f(0.0, 0.0, 0.0);
+    gui::drawStringOnScreen(0.01f, yPos, frame, false);
+    yPos += 0.2;
+  }
+}
+
+void GraphWindow::keyboard(unsigned char _key, int _x, int _y) {
+  switch (_key) {
+    default:
+      Win2D::keyboard(_key, _x, _y);
+  }
+  glutPostRedisplay();
+}
+
+void GraphWindow::setData(Eigen::VectorXd _data) {
+  mData = _data;
+}
+
+}  // namespace gui
+}  // namespace dart

dart/gui/GraphWindow.h

@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2015, Georgia Tech Research Corporation
+ * All rights reserved.
+ *
+ * Author(s): Karen Liu <karenliu@cc.gatech.edu>
+ *
+ * Georgia Tech Graphics Lab and Humanoid Robotics Lab
+ *
+ * Directed by Prof. C. Karen Liu and Prof. Mike Stilman
+ * <karenliu@cc.gatech.edu> <mstilman@cc.gatech.edu>
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * This code incorporates portions of Open Dynamics Engine
+ *     (Copyright (c) 2001-2004, Russell L. Smith. All rights
+ *     reserved.) and portions of FCL (Copyright (c) 2011, Willow
+ *     Garage, Inc. All rights reserved.), which were released under
+ *     the same BSD license as below
+ *
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DART_GUI_GRAPHWINDOW_H_
+#define DART_GUI_GRAPHWINDOW_H_
+
+#include <vector>
+
+#include <Eigen/Dense>
+
+#include "dart/gui/Win2D.h"
+
+namespace dart {
+namespace gui {
+
+/// \brief
+class GraphWindow : public Win2D {
+public:
+  /// \brief
+  GraphWindow();
+
+  /// \brief
+  virtual ~GraphWindow();
+
+  /// \brief
+  virtual void draw();
+
+  /// \brief
+  virtual void keyboard(unsigned char _key, int _x, int _y);
+
+  void setData(Eigen::VectorXd _data);
+
+protected:
+  Eigen::VectorXd mData;
+};
+
+}  // namespace gui
+}  // namespace dart
+
+#endif  // DART_GUI_GRAPHWINDOW_H_

dart/gui/SimWindow.cpp

@@ -52,6 +52,7 @@
 #include "dart/collision/CollisionDetector.h"
 #include "dart/gui/GLFuncs.h"
 #include "dart/utils/FileInfoWorld.h"
+#include "dart/gui/GraphWindow.h"
 
 namespace dart {
 namespace gui {
@@ -72,6 +73,8 @@ SimWindow::SimWindow()
 }
 
 SimWindow::~SimWindow() {
+  for (const auto& graphWindow : mGraphWindows)
+    delete graphWindow;
 }
 
 void SimWindow::timeStepping() {
@@ -227,5 +230,12 @@ void SimWindow::saveWorld() {
   worldFile.saveFile("tempWorld.txt", mWorld->getRecording());
 }
 
+void SimWindow::plot(Eigen::VectorXd& _data) {
+  GraphWindow* figure = new GraphWindow();
+  figure->setData(_data);
+  figure->initWindow(480, 240, "figure");
+  mGraphWindows.push_back(figure);
+}
+
 }  // namespace gui
 }  // namespace dart

dart/gui/SimWindow.h

@@ -57,6 +57,8 @@ class World;
 namespace dart {
 namespace gui {
 
+class GraphWindow;
+
 /// \brief
 class SimWindow : public Win3D {
 public:
@@ -87,6 +89,8 @@ class SimWindow : public Win3D {
   /// \brief Save world in 'tempWorld.txt'
   void saveWorld();
 
+  /// \brief Plot _data in a 2D window
+  void plot(Eigen::VectorXd& _data);
 //  bool isSimulating() const { return mSimulating; }
 
 //  void setSimulatingFlag(int _flag) { mSimulating = _flag; }
@@ -107,6 +111,8 @@ class SimWindow : public Win3D {
   /// \brief
   bool mShowMarkers;
 
+  /// \brief Array of graph windows
+  std::vector<GraphWindow*> mGraphWindows;
 };
 
 }  // namespace gui