ODE/gnuplotでリアルタイム描画 - bambooflow Note

gnuplotでリアルタイム描画（実験）

実験的にgnuplotを使って、ODE実行中にデータをリアルタイムでプロットできるか試してみました。
メモ書きです。

test_furiko_gnuplot.zip

振り子のプログラムに少し手を加えたものです。

サンプルコード

gnuplotでリアルタイム描画するためにパイプストリームを行っています。
パイプストリームは、pipestream.hにより実現しています。
使い方はiostreamのstd::coutに似ています。

#ifdef WIN32
#include <windows.h>
#endif
 
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
 
#include <deque>
#include "pipestream.h"
 
using namespace std;
using namespace ode_utils;
 
 
#ifdef dDOUBLE
  #define dsDrawBox dsDrawBoxD
#endif
 
static dWorldID world;
 
static dBodyID body_box1;
static dBodyID body_box2;
dReal  box_sizes[3] = { 1.0, 1.0, 1.0 };
 
dJointID joint_ball;
 
ps::pipestream *gnuplot;
 
#define STEP_SIZE 0.01
double time = 0;
 
struct PlotData {
    double x;
    double y1;
    double y2;
};
deque<PlotData> plotData;
 
 
// start simulation - set viewpoint
static void start()
{
  static float xyz[3] = { 0.f, -10.f, 10.f };
  static float hpr[3] = { 90.f, -15.f, 0.f };
 
  dsSetViewpoint( xyz, hpr );
}
 
int cnt = 0;
 
// simulation loop
static void simLoop( int pause )
{
 
    dsSetColor( 1.0f, 1.0f, 1.0f );
    dsDrawBox( dBodyGetPosition( body_box1 ), dBodyGetRotation( body_box1 ), box_sizes );
 
    dsSetColor( 0.0f, 1.0f, 0.0f );
    dsDrawBox( dBodyGetPosition( body_box2 ), dBodyGetRotation( body_box2 ), box_sizes );
 
 
 
    //
    //gnuplotダンプ
    //
    if (!pause) {
        const dReal* pos = dBodyGetPosition( body_box2 );
        PlotData d = { time, pos[0], pos[2] }; // x, y
        plotData.push_back( d );
 
        if (cnt%10 == 0) {
 
            *gnuplot << "set xrange[" << plotData.front().x << ":" << plotData.back().x << "]" <<ps::endl;
            *gnuplot << "set multiplot" << ps::endl;
            // y1
            *gnuplot << "plot '-' ls 1" << ps::endl;
            deque<PlotData>::iterator it = plotData.begin();
            while (it != plotData.end() ) {
                *gnuplot << (*it).x << " " << (*it).y1 << ps::endl;
                ++it;
            }
            *gnuplot << "e" << ps::endl;
            // y2
            *gnuplot << "plot '-' ls 2" << ps::endl;
            it = plotData.begin();
            while (it != plotData.end() ) {
                *gnuplot << (*it).x << " " << (*it).y2 << ps::endl;
                ++it;
            }
            *gnuplot << "e" << ps::endl;
            *gnuplot << "set nomultiplot" << ps::endl;
 
        }
 
        while (plotData.size() > 100) {
            plotData.pop_front();
        }
    }
 
    // Ctl+p が押されたらifに入らない
    if (!pause)
    {
        dWorldStep( world, STEP_SIZE );
        time += STEP_SIZE;
        ++cnt;
    }
}
 
 
int main( int argc, char* argv[] )
{
    dInitODE();
 
    // setup pointers to drawstuff callback functions
    dsFunctions fn;
    fn.version = DS_VERSION;
    fn.start   = &start;
    fn.step    = &simLoop;
    fn.command = 0;
    fn.stop    = 0;
    fn.path_to_textures = "../drawstuff/textures";
 
    world = dWorldCreate();
    dWorldSetGravity( world, 0.0, 0.0, -9.8 );
 
 
    // box1 creating
    {
        body_box1 = dBodyCreate( world );
        dReal pos[3] = { 0.0, 0.0, 10.0 };
        dBodySetPosition( body_box1, pos[0], pos[1], pos[2] );
    }
 
    // box2 creating
    {
        body_box2 = dBodyCreate( world );
        dReal pos[3] = { 5.0, 0.0, 10.0 };
        dBodySetPosition( body_box2, pos[0], pos[1], pos[2] );
    }
 
    // box1の固定
    {
        dJointID fixed;
        fixed = dJointCreateFixed( world, 0 );
        dJointAttach( fixed, NULL, body_box1 );
        dJointSetFixed( fixed );
    }
 
    // ボールジョイント
    {
        joint_ball = dJointCreateBall( world, 0 );
        dJointSetBallAnchor( joint_ball, 0.0, 0.0, 10.0 ); // box1と同じ座標
        dJointAttach( joint_ball, body_box1, body_box2 );
 
        // ボールジョイントのフィードバック情報
        dJointFeedback *fb = new dJointFeedback;
        dJointSetFeedback( joint_ball, fb );
    }
 
    // gnuplot準備
    gnuplot = new ps::pipestream( "gnuplot" );
    *gnuplot << "set grid" << ps::endl;
    *gnuplot << "set nokey" << ps::endl;
    *gnuplot << "set yrange[-10:15]"<<ps::endl;
    *gnuplot << "set linestyle 1 lt 1 lw 3 pt 5" << ps::endl;
    *gnuplot << "set linestyle 2 lt 2 lw 3 pt 5" << ps::endl;
 
    dsSimulationLoop( argc, argv, 320, 240, &fn );
 
    //fclose( fp );
 
    dWorldDestroy( world );
    dCloseODE();
    return 0;
}
 

まとめ

実験としてはうまくリアルタイム描画できました。
ただし、描画のタイミングを間引きしたりプロット数を減らしたりしないとかなりつらいものになってしまいました。
マルチスレッド化できれば軽くなると思いますが。。。

それでもgnuplotを知っていれば簡単に扱えるのでちょっとしたテストには使えるのではないかと。
ODEのサブセットとしてこういうのがほしいですな。

参考

• aki-yanさん：http://d.hatena.ne.jp/aki-yam/20090722/1248284541