Introduce Open Dynamics Engine examples

Added a 3 wheeled car example from the ODE demos.  I collapsed all of the separate classes
handling the world, objects, and drawings into a single class.  Each world is different
and just including that with the objects for these simple examples is easier.  Same goes
for the drawing.

refs 
This commit is contained in:
Alex Lin 2016-02-02 11:19:25 -06:00
parent 362366b908
commit b9c871328b
18 changed files with 490 additions and 191 deletions

View File

@ -1,18 +1,83 @@
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "Ball.hh"
#include "ODE/OdeWorld/OdeWorld.hh"
#include "trick/memorymanager_c_intf.h"
Ball::Ball() {}
int Ball::init() {
body = dBodyCreate (OdeWorld::getWorldID());
geom = dCreateSphere (OdeWorld::getSpaceID(),0.5);
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);
body = dBodyCreate (world);
geom = dCreateSphere (space,0.5);
dMassSetSphere (&m,1,0.5);
dBodySetMass (body,&m);
dGeomSetBody (geom,body);
// set initial position
dBodySetPosition (body,0,0,3);
pos = (dReal *)dGeomGetPosition(geom) ;
TMM_declare_ext_var_1d( pos , "float" , 3) ;
add_object() ;
return 0 ;
}
int Ball::step( double time_step) {
// find collisions and add contact joints
dSpaceCollide (space,this,&Ball::nearCallback);
// step the simulation
//dWorldQuickStep (world,time_step);
dWorldStep (world,time_step);
// remove all contact joints
dJointGroupEmpty (contactgroup);
return 0 ;
}
int Ball::shutdown() {
// clean up
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0 ;
}
int Ball::draw() {
dsDrawSphere (dGeomGetPosition(geom),dGeomGetRotation(geom),dGeomSphereGetRadius(geom));
return 0 ;
}
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
void Ball::nearCallback (void * void_ball, dGeomID o1, dGeomID o2) {
Ball * bp = (Ball *)void_ball ;
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 (bp->world,bp->contactgroup,&contact);
dJointAttach (c,b1,b2);
}
}

View File

@ -11,16 +11,30 @@ LIBRARY DEPENDENCY:
#include <ode/common.h>
#include <ode/src/objects.h>
#include "ODE/DrawStuff/DrawStuffObject.hh"
class Ball {
class Ball : public DrawStuffObject {
public:
dBodyID body;
dGeomID geom;
dMass m;
dWorldID world; // ** world
dSpaceID space; // ** space
dJointGroupID contactgroup; // ** contact
dBodyID body; // ** the ball body
dGeomID geom; // ** contact geometry
dMass m; // ** the mass of the ball
dReal * pos ;
Ball() ;
int init() ;
int step(double time_step) ;
int shutdown() ;
virtual int draw() ;
static void nearCallback(void *data, dGeomID o1, dGeomID o2) ;
} ;

View File

@ -1,18 +0,0 @@
#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 ;
}

View File

@ -1,24 +0,0 @@
/*
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

View File

@ -0,0 +1,217 @@
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "Buggy.hh"
#include "trick/memorymanager_c_intf.h"
Buggy::Buggy() :
gravity(-0.5),
chassis_length(0.7),
chassis_width(0.5),
chassis_height(0.2),
chassis_mass(1.0),
wheel_radius(.18),
wheel_width(0.02),
wheel_mass(0.2),
start_z(0.5)
{}
int Buggy::init() {
dMass m;
int i ;
dInitODE2(0);
world = dWorldCreate ();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate(0);
dWorldSetGravity (world,0,0,gravity);
ground = dCreatePlane (space,0,0,1,0);
// chassis body
body[0] = dBodyCreate (world);
dBodySetPosition (body[0],0,0,start_z);
dMassSetBox (&m,1,chassis_length,chassis_width,chassis_height);
dMassAdjust (&m,chassis_mass);
dBodySetMass (body[0],&m);
box[0] = dCreateBox (0,chassis_length,chassis_width,chassis_height);
dGeomSetBody (box[0],body[0]);
buggy_body_pos = (dReal *)dGeomGetPosition(box[0]) ;
TMM_declare_ext_var_1d( buggy_body_pos , "float" , 3) ;
// wheel bodies
for (i=1; i<=3; i++) {
body[i] = dBodyCreate (world);
dQuaternion q;
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
dBodySetQuaternion (body[i],q);
dMassSetSphere (&m,1,wheel_radius);
dMassAdjust (&m,wheel_mass);
dBodySetMass (body[i],&m);
sphere[i-1] = dCreateSphere (0,wheel_radius);
dGeomSetBody (sphere[i-1],body[i]);
}
dBodySetPosition (body[1],0.5*chassis_length,0,start_z-chassis_height*0.5);
dBodySetPosition (body[2],-0.5*chassis_length, chassis_width*0.5,start_z-chassis_height*0.5);
dBodySetPosition (body[3],-0.5*chassis_length,-chassis_width*0.5,start_z-chassis_height*0.5);
// front and back wheel hinges
for (i=0; i<3; i++) {
joint[i] = dJointCreateHinge2 (world,0);
dJointAttach (joint[i],body[0],body[i+1]);
const dReal *a = dBodyGetPosition (body[i+1]);
dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (joint[i],0,0,1);
dJointSetHinge2Axis2 (joint[i],0,1,0);
}
// set joint suspension
for (i=0; i<3; i++) {
dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4);
dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.8);
}
// lock back wheels along the steering axis
for (i=1; i<3; i++) {
// set stops to make sure wheels always stay in alignment
dJointSetHinge2Param (joint[i],dParamLoStop,0);
dJointSetHinge2Param (joint[i],dParamHiStop,0);
}
// create car space and add it to the top level space
car_space = dSimpleSpaceCreate (space);
dSpaceSetCleanup (car_space,0);
dSpaceAdd (car_space,box[0]);
dSpaceAdd (car_space,sphere[0]);
dSpaceAdd (car_space,sphere[1]);
dSpaceAdd (car_space,sphere[2]);
// environment
ground_box = dCreateBox (space,2,1.5,1);
dMatrix3 R;
dRFromAxisAndAngle (R,0,1,0,-0.15);
dGeomSetPosition (ground_box,2,0,-0.34);
dGeomSetRotation (ground_box,R);
// Add this object to DrawStuff
add_object() ;
return 0 ;
}
int Buggy::step(double time_step) {
dJointSetHinge2Param (joint[0],dParamVel2,-speed);
dJointSetHinge2Param (joint[0],dParamFMax2,0.1);
// steering
dReal v = steer - dJointGetHinge2Angle1 (joint[0]);
if (v > 0.1) v = 0.1;
if (v < -0.1) v = -0.1;
v *= 10.0;
dJointSetHinge2Param (joint[0],dParamVel,v);
dJointSetHinge2Param (joint[0],dParamFMax,0.2);
dJointSetHinge2Param (joint[0],dParamLoStop,-0.75);
dJointSetHinge2Param (joint[0],dParamHiStop,0.75);
dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1);
dSpaceCollide (space,this,&Buggy::nearCallback);
dWorldStep (world,time_step);
// remove all contact joints
dJointGroupEmpty (contactgroup);
return 0 ;
}
int Buggy::shutdown() {
dGeomDestroy (box[0]);
dGeomDestroy (sphere[0]);
dGeomDestroy (sphere[1]);
dGeomDestroy (sphere[2]);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0 ;
}
int Buggy::draw() {
int i ;
dsSetColor (0,1,1);
dsSetTexture (DS_WOOD);
dReal sides[3] = {chassis_length,chassis_width,chassis_height};
dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides);
dsSetColor (1,1,1);
for (i=1; i<=3; i++) {
dsDrawCylinder (dBodyGetPosition(body[i]), dBodyGetRotation(body[i]),wheel_width,wheel_radius);
}
dVector3 ss;
dGeomBoxGetLengths (ground_box,ss);
dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss);
return 0 ;
}
void Buggy::command( int cmd ) {
switch (cmd) {
case 'a': case 'A':
speed += 0.3;
break;
case 'z': case 'Z':
speed -= 0.3;
break;
case ',':
steer -= 0.5;
break;
case '.':
steer += 0.5;
break;
case ' ':
speed = 0;
steer = 0;
break;
case '1': {
FILE *f = fopen ("state.dif","wt");
if (f) {
dWorldExportDIF (world,f,"");
fclose (f);
}
}
}
}
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
void Buggy::nearCallback (void * void_buggy , dGeomID o1, dGeomID o2) {
int i,n;
Buggy * bp = (Buggy *)void_buggy ;
// only collide things with the ground
int g1 = (o1 == bp->ground || o1 == bp->ground_box);
int g2 = (o2 == bp->ground || o2 == bp->ground_box);
if (!(g1 ^ g2)) return;
const int N = 10;
dContact contact[N];
n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
if (n > 0) {
for (i=0; i<n; i++) {
contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
dContactSoftERP | dContactSoftCFM | dContactApprox1;
contact[i].surface.mu = dInfinity;
contact[i].surface.slip1 = 0.1;
contact[i].surface.slip2 = 0.1;
contact[i].surface.soft_erp = 0.5;
contact[i].surface.soft_cfm = 0.3;
dJointID c = dJointCreateContact (bp->world,bp->contactgroup,&contact[i]);
dJointAttach (c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2));
}
}
}

View File

@ -0,0 +1,72 @@
/*
PURPOSE:
(Bouncing ball)
LIBRARY DEPENDENCY:
((Buggy.cpp))
*/
#ifndef BUGGY_HH
#define BUGGY_HH
#include <ode/common.h>
#include <ode/src/objects.h>
#include "ODE/DrawStuff/DrawStuffObject.hh"
#if SWIG
%apply float { dReal }
#endif
class Buggy : public DrawStuffObject {
public:
dWorldID world; // ** world
dSpaceID space; // ** space
dJointGroupID contactgroup; // ** contact
dReal speed ;
dReal steer ;
dReal * buggy_body_pos ;
dReal gravity ;
dReal chassis_length ;
dReal chassis_width ;
dReal chassis_height ;
dReal chassis_mass ;
dReal wheel_radius ;
dReal wheel_width ;
dReal wheel_mass ;
dReal start_z ;
Buggy() ;
// Simulation jobs
int init() ;
int step(double time_step) ;
int shutdown() ;
// DrawStuff callbacks
virtual int draw() ;
virtual void command(int cmd) ;
// collision callback
static void nearCallback(void *data, dGeomID o1, dGeomID o2) ;
protected:
dBodyID body[4]; // ** buggy body and 3 wheels
dJointID joint[3]; // ** joints from body to wheels
dSpaceID car_space; // ** car space. :P
dGeomID box[1]; // ** collision for buggy body
dGeomID sphere[3]; // ** collision for wheels
dGeomID ground; // ** ground
dGeomID ground_box; // ** ground
} ;
#endif

View File

@ -18,6 +18,13 @@ void DrawStuff::step(int) {
}
}
void DrawStuff::command(int cmd) {
std::vector<DrawStuffObject *>::iterator it ;
for( it = objects.begin() ; it != objects.end() ; it++ ) {
(*it)->command(cmd) ;
}
}
DrawStuff::DrawStuff() :
fn(),
window_width(1024),
@ -28,6 +35,7 @@ DrawStuff::DrawStuff() :
fn.start = &DrawStuff::start ;
fn.step = &DrawStuff::step ;
fn.command = &DrawStuff::command ;
}
int DrawStuff::draw() {

View File

@ -15,7 +15,7 @@ LIBRARY DEPENDENCY:
class DrawStuff {
public:
dsFunctions fn;
dsFunctions fn; // ** draw stuff function pointer structure
int window_width ;
int window_height ;
@ -23,6 +23,7 @@ class DrawStuff {
DrawStuff() ;
static void start() ;
static void step(int) ;
static void command(int) ;
int draw() ;
int shutdown() ;

View File

@ -2,7 +2,11 @@
#include "DrawStuffObject.hh"
#include "DrawStuff.hh"
DrawStuffObject::~DrawStuffObject() {}
void DrawStuffObject::add_object() {
DrawStuff::add_object(this) ;
}
void DrawStuffObject::command(int) {
}

View File

@ -11,9 +11,10 @@ LIBRARY DEPENDENCY:
class DrawStuffObject {
public:
virtual ~DrawStuffObject() {}
void add_object() ;
virtual ~DrawStuffObject() ;
virtual void add_object() ;
virtual int draw() = 0 ;
virtual void command(int cmd) ;
} ;
#endif

View File

@ -1,77 +0,0 @@
#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 ;
}

View File

@ -1,33 +0,0 @@
/*
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

View File

@ -10,55 +10,37 @@ LIBRARY DEPENDENCIES:
#include "sim_objects/default_trick_sys.sm"
##include "ODE/OdeWorld/OdeWorld.hh"
##include "ODE/Ball/Ball.hh"
##include "ODE/DrawStuff/DrawStuff.hh"
##include "ODE/Ball/Ball.hh"
##include "ODE/Ball/BallDrawStuffObject.hh"
%{
const double OdeWorldSimObject::time_step = 0.01 ;
const double OdeBallSimObject::time_step = 0.01 ;
%}
/**
This class is the base ball class
*/
class OdeWorldSimObject : public Trick::SimObject {
class OdeBallSimObject : public Trick::SimObject {
public:
OdeWorld ode_world ;
Ball ball ;
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) ;
OdeBallSimObject() {
("initialization") ball.init() ;
(time_step, "scheduled") ball.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) ;
("shutdown") ball.shutdown() ;
}
} ;
// Instantiations
OdeWorldSimObject world ;
OdeObjectsSimObject ode_objects ;
OdeBallSimObject ball_so ;
// Connect objects
void create_connections() {

View File

@ -1,7 +1,7 @@
ODE_HOME = /users/alin/ode-0.14
ODE_HOME = ${HOME}/ode-0.14
TRICK_ICG_EXCLUDE = ${ODE_HOME}/ode/src/array.h:${ODE_HOME}/ode/src/threading_base.h
TRICK_ICG_EXCLUDE = ${ODE_HOME}
TRICK_ICG_NOCOMMENT = ${ODE_HOME}
TRICK_SWIG_EXCLUDE = ${ODE_HOME}

View File

@ -0,0 +1,26 @@
import trick
def main():
buggy_so.buggy.chassis_length = 1.2
buggy_so.buggy.chassis_width = 1.0
buggy_so.buggy.wheel_width = 0.2
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_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()

View File

@ -0,0 +1,50 @@
/************************TRICK HEADER*************************
PURPOSE:
(blah blah blah)
LIBRARY DEPENDENCIES:
(
)
*************************************************************/
#define DRAW_THREAD C1
#include "sim_objects/default_trick_sys.sm"
##include "ODE/Buggy/Buggy.hh"
##include "ODE/DrawStuff/DrawStuff.hh"
%{
const double BuggySimObject::time_step = 0.05 ;
%}
/**
This class is the base ball class
*/
class BuggySimObject : public Trick::SimObject {
public:
Buggy buggy ;
DrawStuff drawstuff ;
static const double time_step ;
/** Constructor to add the jobs */
BuggySimObject() {
("initialization") buggy.init() ;
(time_step, "scheduled") buggy.step(time_step) ;
DRAW_THREAD (1000000.0, "scheduled") drawstuff.draw() ;
("shutdown") drawstuff.shutdown() ;
("shutdown") buggy.shutdown() ;
}
} ;
// Instantiations
BuggySimObject buggy_so ;
// Connect objects
void create_connections() {
// Set the default termination time
//exec_set_terminate_time(300.0) ;
}

View File

@ -0,0 +1,11 @@
ODE_HOME ?= ${HOME}/ode-0.14
TRICK_ICG_EXCLUDE = ${ODE_HOME}
TRICK_ICG_NOCOMMENT = ${ODE_HOME}
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