From 68ffd04dff7879e27af896838b6810596a9d760e Mon Sep 17 00:00:00 2001 From: Alex Lin Date: Fri, 29 Jan 2016 09:15:00 -0600 Subject: [PATCH] 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 | 18 +++++ trick_models/ODE/Ball/Ball.hh | 27 +++++++ trick_models/ODE/Ball/BallDrawStuffObject.cpp | 18 +++++ trick_models/ODE/Ball/BallDrawStuffObject.hh | 24 ++++++ trick_models/ODE/DrawStuff/DrawStuff.cpp | 47 +++++++++++ trick_models/ODE/DrawStuff/DrawStuff.hh | 34 ++++++++ .../ODE/DrawStuff/DrawStuffObject.cpp | 8 ++ trick_models/ODE/DrawStuff/DrawStuffObject.hh | 19 +++++ trick_models/ODE/OdeWorld/OdeWorld.cpp | 77 +++++++++++++++++++ trick_models/ODE/OdeWorld/OdeWorld.hh | 33 ++++++++ trick_sims/SIM_ode/RUN_test/input.py | 22 ++++++ trick_sims/SIM_ode/S_define | 68 ++++++++++++++++ trick_sims/SIM_ode/S_overrides.mk | 10 +++ 13 files changed, 405 insertions(+) create mode 100644 trick_models/ODE/Ball/Ball.cpp create mode 100644 trick_models/ODE/Ball/Ball.hh create mode 100644 trick_models/ODE/Ball/BallDrawStuffObject.cpp create mode 100644 trick_models/ODE/Ball/BallDrawStuffObject.hh create mode 100644 trick_models/ODE/DrawStuff/DrawStuff.cpp create mode 100644 trick_models/ODE/DrawStuff/DrawStuff.hh create mode 100644 trick_models/ODE/DrawStuff/DrawStuffObject.cpp create mode 100644 trick_models/ODE/DrawStuff/DrawStuffObject.hh create mode 100644 trick_models/ODE/OdeWorld/OdeWorld.cpp create mode 100644 trick_models/ODE/OdeWorld/OdeWorld.hh create mode 100644 trick_sims/SIM_ode/RUN_test/input.py create mode 100644 trick_sims/SIM_ode/S_define create mode 100644 trick_sims/SIM_ode/S_overrides.mk diff --git a/trick_models/ODE/Ball/Ball.cpp b/trick_models/ODE/Ball/Ball.cpp new file mode 100644 index 00000000..884c7492 --- /dev/null +++ b/trick_models/ODE/Ball/Ball.cpp @@ -0,0 +1,18 @@ + +#include +#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 ; +} + diff --git a/trick_models/ODE/Ball/Ball.hh b/trick_models/ODE/Ball/Ball.hh new file mode 100644 index 00000000..ae3610de --- /dev/null +++ b/trick_models/ODE/Ball/Ball.hh @@ -0,0 +1,27 @@ + +/* +PURPOSE: + (Bouncing ball) +LIBRARY DEPENDENCY: + ((Ball.cpp)) +*/ + +#ifndef ODEBALL_HH +#define ODEBALL_HH + +#include +#include + +class Ball { + public: + + dBodyID body; + dGeomID geom; + dMass m; + + Ball() ; + int init() ; + +} ; + +#endif diff --git a/trick_models/ODE/Ball/BallDrawStuffObject.cpp b/trick_models/ODE/Ball/BallDrawStuffObject.cpp new file mode 100644 index 00000000..103e32d0 --- /dev/null +++ b/trick_models/ODE/Ball/BallDrawStuffObject.cpp @@ -0,0 +1,18 @@ + +#include +#include +#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 ; +} + diff --git a/trick_models/ODE/Ball/BallDrawStuffObject.hh b/trick_models/ODE/Ball/BallDrawStuffObject.hh new file mode 100644 index 00000000..6bb36288 --- /dev/null +++ b/trick_models/ODE/Ball/BallDrawStuffObject.hh @@ -0,0 +1,24 @@ + +/* +PURPOSE: + (Bouncing ball) +LIBRARY DEPENDENCY: + ((BallDrawStuffObject.cpp)) +*/ + +#ifndef ODEBALLDRAWSTUFFOBJECT_HH +#define ODEBALLDRAWSTUFFOBJECT_HH + +#include +#include "ODE/DrawStuff/DrawStuffObject.hh" + +class BallDrawStuffObject : public DrawStuffObject { + public: + dGeomID geom ; + + BallDrawStuffObject() ; + int init(dGeomID in_geom) ; + virtual int draw() ; +} ; + +#endif diff --git a/trick_models/ODE/DrawStuff/DrawStuff.cpp b/trick_models/ODE/DrawStuff/DrawStuff.cpp new file mode 100644 index 00000000..456b5e09 --- /dev/null +++ b/trick_models/ODE/DrawStuff/DrawStuff.cpp @@ -0,0 +1,47 @@ + +#include +#include +#include "DrawStuff.hh" + +std::vector 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::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) ; +} diff --git a/trick_models/ODE/DrawStuff/DrawStuff.hh b/trick_models/ODE/DrawStuff/DrawStuff.hh new file mode 100644 index 00000000..f0a02e34 --- /dev/null +++ b/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 +#include +#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 objects ; +} ; + +#endif diff --git a/trick_models/ODE/DrawStuff/DrawStuffObject.cpp b/trick_models/ODE/DrawStuff/DrawStuffObject.cpp new file mode 100644 index 00000000..776139d2 --- /dev/null +++ b/trick_models/ODE/DrawStuff/DrawStuffObject.cpp @@ -0,0 +1,8 @@ + +#include "DrawStuffObject.hh" +#include "DrawStuff.hh" + +void DrawStuffObject::add_object() { + DrawStuff::add_object(this) ; +} + diff --git a/trick_models/ODE/DrawStuff/DrawStuffObject.hh b/trick_models/ODE/DrawStuff/DrawStuffObject.hh new file mode 100644 index 00000000..f64e6fa8 --- /dev/null +++ b/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 diff --git a/trick_models/ODE/OdeWorld/OdeWorld.cpp b/trick_models/ODE/OdeWorld/OdeWorld.cpp new file mode 100644 index 00000000..1861374d --- /dev/null +++ b/trick_models/ODE/OdeWorld/OdeWorld.cpp @@ -0,0 +1,77 @@ + +#include +#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 ; +} + diff --git a/trick_models/ODE/OdeWorld/OdeWorld.hh b/trick_models/ODE/OdeWorld/OdeWorld.hh new file mode 100644 index 00000000..acb17f05 --- /dev/null +++ b/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 + +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 diff --git a/trick_sims/SIM_ode/RUN_test/input.py b/trick_sims/SIM_ode/RUN_test/input.py new file mode 100644 index 00000000..b7108888 --- /dev/null +++ b/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() + + diff --git a/trick_sims/SIM_ode/S_define b/trick_sims/SIM_ode/S_define new file mode 100644 index 00000000..4c602684 --- /dev/null +++ b/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) ; +} + diff --git a/trick_sims/SIM_ode/S_overrides.mk b/trick_sims/SIM_ode/S_overrides.mk new file mode 100644 index 00000000..304de7cc --- /dev/null +++ b/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