Introduce Open Dynamics Engine examples

Took the ODE bouncing ball tutorial sim and interfaced it within Trick.  Created classes to handle
interfacing with ODE and the DrawStuff graphics that comes with ODE.  Created classes
to contain the ball and the drawstuff graphics for the ball.

refs #172

trick_models/ODE/Ball/Ball.cpp

@@ -0,0 +1,18 @@
+
+#include <ode/ode.h>
+#include "Ball.hh"
+#include "ODE/OdeWorld/OdeWorld.hh"
+
+Ball::Ball() {}
+
+int Ball::init() {
+    body = dBodyCreate (OdeWorld::getWorldID());
+    geom = dCreateSphere (OdeWorld::getSpaceID(),0.5);
+    dMassSetSphere (&m,1,0.5);
+    dBodySetMass (body,&m);
+    dGeomSetBody (geom,body);
+    // set initial position
+    dBodySetPosition (body,0,0,3);
+    return 0 ;
+}
+

trick_models/ODE/Ball/Ball.hh

@@ -0,0 +1,27 @@
+
+/*
+PURPOSE:
+    (Bouncing ball)
+LIBRARY DEPENDENCY:
+    ((Ball.cpp))
+*/
+
+#ifndef ODEBALL_HH
+#define ODEBALL_HH
+
+#include <ode/common.h>
+#include <ode/src/objects.h>
+
+class Ball {
+    public:
+
+        dBodyID body;
+        dGeomID geom;
+        dMass m;
+
+        Ball() ;
+        int init() ;
+
+} ;
+
+#endif

trick_models/ODE/Ball/BallDrawStuffObject.cpp

@@ -0,0 +1,18 @@
+
+#include <ode/collision.h>
+#include <drawstuff/drawstuff.h>
+#include "BallDrawStuffObject.hh"
+
+BallDrawStuffObject::BallDrawStuffObject() {}
+
+int BallDrawStuffObject::init(dGeomID in_geom) {
+    geom = in_geom ;
+    add_object() ;
+    return 0 ;
+}
+
+int BallDrawStuffObject::draw() {
+    dsDrawSphere (dGeomGetPosition(geom),dGeomGetRotation(geom),dGeomSphereGetRadius(geom));
+    return 0 ;
+}
+

trick_models/ODE/Ball/BallDrawStuffObject.hh

@@ -0,0 +1,24 @@
+
+/*
+PURPOSE:
+    (Bouncing ball)
+LIBRARY DEPENDENCY:
+    ((BallDrawStuffObject.cpp))
+*/
+
+#ifndef ODEBALLDRAWSTUFFOBJECT_HH
+#define ODEBALLDRAWSTUFFOBJECT_HH
+
+#include <ode/common.h>
+#include "ODE/DrawStuff/DrawStuffObject.hh"
+
+class BallDrawStuffObject : public DrawStuffObject {
+    public:
+        dGeomID geom ;
+
+        BallDrawStuffObject() ;
+        int init(dGeomID in_geom) ;
+        virtual int draw() ;
+} ;
+
+#endif

trick_models/ODE/DrawStuff/DrawStuff.cpp

@@ -0,0 +1,47 @@
+
+#include <string.h>
+#include <unistd.h>
+#include "DrawStuff.hh"
+
+std::vector<DrawStuffObject *> DrawStuff::objects ;
+
+void DrawStuff::start() {
+    static float xyz[3] = {2.0f,-2.0f,1.7600f};
+    static float hpr[3] = {140.000f,-17.0000f,0.0000f};
+    dsSetViewpoint (xyz,hpr);
+}
+
+void DrawStuff::step(int) {
+    std::vector<DrawStuffObject *>::iterator it ;
+    for( it = objects.begin() ; it != objects.end() ; it++ ) {
+        (*it)->draw() ;
+    }
+}
+
+DrawStuff::DrawStuff() :
+ fn(),
+ window_width(1024),
+ window_height(768)
+{
+    fn.version = DS_VERSION;
+    fn.path_to_textures = "/users/alin/ode-0.14/drawstuff/textures";
+
+    fn.start = &DrawStuff::start ;
+    fn.step = &DrawStuff::step ;
+}
+
+int DrawStuff::draw() {
+    dsSimulationLoop(0 , NULL, window_width, window_height, &fn) ;
+    return 0 ;
+}
+
+int DrawStuff::shutdown() {
+    dsStop() ;
+    // wait a second to allow time for the loop to exit.  Yeah, it's unscientific.
+    sleep(1) ;
+    return 0 ;
+}
+
+void DrawStuff::add_object( DrawStuffObject * dso ) {
+    objects.push_back(dso) ;
+}

trick_models/ODE/DrawStuff/DrawStuff.hh

@@ -0,0 +1,34 @@
+
+/*
+PURPOSE:
+    (Create the ODE world)
+LIBRARY DEPENDENCY:
+    ((DrawStuff.cpp))
+*/
+
+#ifndef DRAWSTUFF_HH
+#define DRAWSTUFF_HH
+
+#include <vector>
+#include <drawstuff/drawstuff.h>
+#include "DrawStuffObject.hh"
+
+class DrawStuff {
+    public:
+        dsFunctions fn;
+
+        int window_width ;
+        int window_height ;
+
+        DrawStuff() ;
+        static void start() ;
+        static void step(int) ;
+
+        int draw() ;
+        int shutdown() ;
+
+        static void add_object(DrawStuffObject * dso) ;
+        static std::vector<DrawStuffObject *> objects ;
+} ;
+
+#endif

trick_models/ODE/DrawStuff/DrawStuffObject.cpp

@@ -0,0 +1,8 @@
+
+#include "DrawStuffObject.hh"
+#include "DrawStuff.hh"
+
+void DrawStuffObject::add_object() {
+    DrawStuff::add_object(this) ;
+}
+

trick_models/ODE/DrawStuff/DrawStuffObject.hh

@@ -0,0 +1,19 @@
+
+/*
+PURPOSE:
+    (Bouncing ball)
+LIBRARY DEPENDENCY:
+    ((DrawStuffObject.cpp))
+*/
+
+#ifndef DRAWSTUFFOBJECT_HH
+#define DRAWSTUFFOBJECT_HH
+
+class DrawStuffObject {
+    public:
+        virtual ~DrawStuffObject() {}
+        void add_object() ;
+        virtual int draw() = 0 ;
+} ;
+
+#endif

trick_models/ODE/OdeWorld/OdeWorld.cpp

@@ -0,0 +1,77 @@
+
+#include <ode/ode.h>
+#include "OdeWorld.hh"
+
+dWorldID OdeWorld::world ;
+dSpaceID OdeWorld::space ;
+dJointGroupID OdeWorld::contactgroup ;
+
+extern int current_state ;
+
+OdeWorld::OdeWorld() {}
+
+int OdeWorld::init() {
+    dInitODE ();
+    world = dWorldCreate ();
+    space = dHashSpaceCreate (0);
+    dWorldSetGravity (world,0,0,-9.81);
+    dWorldSetCFM (world,1e-5);
+    dCreatePlane (space,0,0,1,0);
+    contactgroup = dJointGroupCreate (0);
+    return 0 ;
+}
+
+int OdeWorld::step( double time_step) {
+
+    // find collisions and add contact joints
+    dSpaceCollide (space,0,&OdeWorld::nearCallback);
+
+    // step the simulation
+    //dWorldQuickStep (world,time_step);
+    dWorldStep (world,time_step);
+    // remove all contact joints
+    dJointGroupEmpty (contactgroup);
+
+    return 0 ;
+}
+
+int OdeWorld::shutdown() {
+    // clean up
+    dJointGroupDestroy (contactgroup);
+    dSpaceDestroy (space);
+    dWorldDestroy (world);
+    dCloseODE();
+
+    return 0 ;
+}
+
+// this is called by dSpaceCollide when two objects in space are
+// potentially colliding.
+void OdeWorld::nearCallback (void *, dGeomID o1, dGeomID o2)
+{
+    dBodyID b1 = dGeomGetBody(o1);
+    dBodyID b2 = dGeomGetBody(o2);
+    dContact contact;
+    contact.surface.mode = dContactBounce | dContactSoftCFM;
+    // friction parameter
+    contact.surface.mu = dInfinity;
+    // bounce is the amount of "bouncyness".
+    contact.surface.bounce = 0.999;
+    // bounce_vel is the minimum incoming velocity to cause a bounce
+    contact.surface.bounce_vel = 0.1;
+    // constraint force mixing parameter
+    contact.surface.soft_cfm = 0.001;
+    if (int numc = dCollide (o1,o2,1,&contact.geom,sizeof(dContact))) {
+        dJointID c = dJointCreateContact (OdeWorld::world,OdeWorld::contactgroup,&contact);
+        dJointAttach (c,b1,b2);
+    }
+}
+
+dWorldID OdeWorld::getWorldID() {
+    return world ;
+}
+
+dSpaceID OdeWorld::getSpaceID() {
+    return space ;
+}
+

trick_models/ODE/OdeWorld/OdeWorld.hh

@@ -0,0 +1,33 @@
+
+/*
+PURPOSE:
+    (Create the ODE world)
+LIBRARY DEPENDENCY:
+    ((OdeWorld.cpp))
+*/
+
+#ifndef ODEWORLD_HH
+#define ODEWORLD_HH
+
+#include <ode/common.h>
+
+class OdeWorld {
+    public:
+        static dWorldID world;
+        static dJointGroupID contactgroup;
+        static dSpaceID space;
+
+        static dWorldID getWorldID() ;
+        static dSpaceID getSpaceID() ;
+
+        OdeWorld() ;
+
+        int init() ;
+        int step(double time_step) ;
+        int shutdown() ;
+
+        static void nearCallback(void *data, dGeomID o1, dGeomID o2) ;
+
+} ;
+
+#endif

trick_sims/SIM_ode/RUN_test/input.py

@@ -0,0 +1,22 @@
+
+import trick
+
+def main():
+    trick.sim_control_panel_set_enabled(True)
+    trick.real_time_enable()
+    trick.itimer_enable()
+
+    trick.exec_set_software_frame(0.05)
+    trick.exec_set_freeze_frame(0.05)
+    trick.exec_set_freeze_command(True)
+
+    trick.frame_log_set_max_samples(100)
+    trick.frame_log_on()
+
+    trick.exec_set_thread_process_type(1, trick.PROCESS_TYPE_ASYNC_CHILD)
+    trick.exec_set_thread_amf_cycle_time(1, 0.1)
+
+if __name__ == "__main__":
+    main()
+
+

trick_sims/SIM_ode/S_define

@@ -0,0 +1,68 @@
+/************************TRICK HEADER*************************
+PURPOSE:
+    (blah blah blah)
+LIBRARY DEPENDENCIES:
+    (
+    )
+*************************************************************/
+
+#define DRAW_THREAD C1
+
+#include "sim_objects/default_trick_sys.sm"
+
+##include "ODE/OdeWorld/OdeWorld.hh"
+##include "ODE/DrawStuff/DrawStuff.hh"
+
+##include "ODE/Ball/Ball.hh"
+##include "ODE/Ball/BallDrawStuffObject.hh"
+
+%{
+const double OdeWorldSimObject::time_step = 0.01 ;
+%}
+
+/**
+  This class is the base ball class
+ */
+class OdeWorldSimObject : public Trick::SimObject {
+
+    public:
+
+        OdeWorld ode_world ;
+        DrawStuff drawstuff ;
+
+        static const double time_step ;
+
+        /** Constructor to add the jobs */
+        OdeWorldSimObject() {
+            ("initialization") ode_world.init() ;
+            (time_step, "scheduled") ode_world.step(time_step) ;
+            DRAW_THREAD (1000000.0, "scheduled") drawstuff.draw() ;
+            ("shutdown") drawstuff.shutdown() ;
+            ("shutdown") ode_world.shutdown() ;
+        }
+} ;
+
+class OdeObjectsSimObject : public Trick::SimObject {
+
+    public:
+
+        Ball ball ;
+        BallDrawStuffObject ball_drawstuff_object ;
+
+        /** Constructor to add the jobs */
+        OdeObjectsSimObject() {
+            ("initialization") ball.init() ;
+            ("initialization") ball_drawstuff_object.init(ball.geom) ;
+        }
+} ;
+
+// Instantiations
+OdeWorldSimObject world ;
+OdeObjectsSimObject ode_objects ;
+
+// Connect objects
+void create_connections() {
+    // Set the default termination time
+    exec_set_terminate_time(300.0) ;
+}
+

trick_sims/SIM_ode/S_overrides.mk

@@ -0,0 +1,10 @@
+
+ODE_HOME = /users/alin/ode-0.14
+
+TRICK_ICG_EXCLUDE = ${ODE_HOME}/ode/src/array.h:${ODE_HOME}/ode/src/threading_base.h
+TRICK_SWIG_EXCLUDE = ${ODE_HOME}
+
+TRICK_CFLAGS += -I${TRICK_HOME}/trick_models -I${ODE_HOME}/include -I${ODE_HOME}
+TRICK_CXXFLAGS += -I${TRICK_HOME}/trick_models -I${ODE_HOME}/include -I${ODE_HOME}
+
+TRICK_EXEC_LINK_LIBS += -L${ODE_HOME}/drawstuff/src/.libs -ldrawstuff -L${ODE_HOME}/ode/src/.libs -lode -lGLU -lGL -L/usr/X11R6/lib -lXext -lX11