Posted By

tkf on 06/12/08

Tagged

Versions (?)

Last Edited at 06/12/08 08:53pm

Statistics

Viewed 640 times

Favorited by 0 user(s)

Related snippets

2 armed pendulum control start kit using Open Dynamics Engine

/ Published in: C++

Ref:
http://www.koj-m.sakura.ne.jp/ode/index.php?%A5%C7%A5%E2%A1%CA%B4%F0%CB%DC%CA%D4%A1%CB

Highlighted TextExpand

#include <ode/ode.h>

#include <drawstuff/drawstuff.h>

// select correct drawing functions

#ifdef dDOUBLE

#define dsDrawBox dsDrawBoxD

#define dsDrawSphere dsDrawSphereD

#define dsDrawCylinder dsDrawCylinderD

#define dsDrawCapsule dsDrawCapsuleD

#endif

static dWorldID world;

static dSpaceID space;

static dJointGroupID contactgroup;

static dGeomID ground;

static dBodyID body_up,body_low;

static dGeomID box_up,box_low;

static dJointID joint_up2low;

static dJointID joint_up2env;

static dSpaceID geom_group;

// box location, size, mass

double box_up_loc[3]   = {0  , 0  , 1  };

double box_low_loc[3]  = {0.4, 0  , 1  };

double box_up_size[3]  = {0.4, 0.1, 0.1};

double box_low_size[3] = {0.4, 0.1, 0.1};

double box_up_mass=1.0, box_low_mass=1.0;

// joint location

double joint_up2low_x, joint_up2low_y, joint_up2low_z,

joint_up2env_x, joint_up2env_y, joint_up2env_z;

#define DT 0.001

#define PI 3.14159

// start simulation - set viewpoint

static void start()

{

static float xyz[3] = {0.8317f,-0.9817f,0.8000f};

static float hpr[3] = {136.5000f,0.5000f,0.0000f};

dsSetViewpoint (xyz,hpr);

printf ("start simulation\n");

}

// control

double theta1, theta2;

//double omega1, omega2;

int controler_cnt=0;

void controler(){

controler_cnt++;

//--- get angular

theta1 = dJointGetHingeAngle (joint_up2env);

theta2 = dJointGetHingeAngle (joint_up2low);

//omega1 = dJointGetHingeAngleRate (joint_up2env);

//omega2 = dJointGetHingeAngleRate (joint_up2low);

//--- control

// velocity contorl

double v;

v = ( (2*PI * sin( 2*PI*(controler_cnt * DT) ))

- PI/2 - theta1 );

dJointSetHingeParam (joint_up2env,dParamVel,v);

dJointSetHingeParam (joint_up2env,dParamFMax,20000);

// torque control

//dJointAddHingeTorque(joint_up2env,1);

}

// simulation loop

double t=0;

static void simLoop (int pause)

{

int i,j;

if (!pause) {

dWorldStep (world,DT);

// remove all contact joints

dJointGroupEmpty (contactgroup);

// control

controler();

// print real time

//if( t > 1 && fmod(t,1) < DT ){ printf("%f sec\n",t); }

t += DT;

}

dsSetTexture (DS_WOOD);

dReal sides[3];

dsSetColor (1,0,0);

for(i=0; i<3; i++){ sides[i]=box_up_size[i]; }

dsDrawBox( dBodyGetPosition(body_up),

dBodyGetRotation(body_up),

sides);

dsSetColor (0,1,0);

for(i=0; i<3; i++){ sides[i]=box_low_size[i]; }

dsDrawBox( dBodyGetPosition(body_low),

dBodyGetRotation(body_low),

sides);

}

int main (int argc, char **argv)

{

dMass m;

//--- setup pointers to drawstuff callback functions

dsFunctions fn;

fn.version = DS_VERSION;

fn.start = &start;

fn.step = &simLoop;

fn.stop = 0;

fn.path_to_textures = "../simulator/ode-0.9/drawstuff/textures";

if(argc==2){ fn.path_to_textures = argv[1]; }

//--- create world

world = dWorldCreate();

space = dHashSpaceCreate (0);

contactgroup = dJointGroupCreate (0);

dWorldSetGravity (world,0,0,-9.8);

ground = dCreatePlane (space,0,0,1,0);

//--- create pendulum arm

body_up  = dBodyCreate(world);

body_low = dBodyCreate(world);

dBodySetPosition( body_up,

box_up_loc[0], box_up_loc[1], box_up_loc[2]);

dBodySetPosition( body_low,

box_low_loc[0], box_low_loc[1], box_low_loc[2]);

dMassSetBox(&m,1,box_up_size[0], box_up_size[1], box_up_size[2] );

dMassSetBox(&m,1,box_low_size[0], box_low_size[1], box_low_size[2] );

dMassAdjust(&m,box_up_mass);

dMassAdjust(&m,box_low_mass);

dBodySetMass(body_up,&m);

dBodySetMass(body_low,&m);

box_up  = dCreateBox(0,box_up_size[0], box_up_size[1], box_up_size[2]);

box_low = dCreateBox(0,box_low_size[0], box_low_size[1], box_low_size[2]);

dGeomSetBody ( box_up, body_up );

dGeomSetBody ( box_low, body_low );

//--- create joint

dVector3 a;

dBodyGetRelPointPos(body_up,box_up_size[0]/2,0,0,a);

joint_up2low_x=a[0];

joint_up2low_y=a[1];

joint_up2low_z=a[2];

dBodyGetRelPointPos(body_up,-box_up_size[0]/2,0,0,a);

joint_up2env_x=a[0];

joint_up2env_y=a[1];

joint_up2env_z=a[2];

//--- joint up to low

joint_up2low = dJointCreateHinge (world,0);

dJointAttach (joint_up2low,body_up,body_low);

dJointSetHingeAnchor (joint_up2low,

joint_up2low_x,joint_up2low_y,joint_up2low_z);

dJointSetHingeAxis ( joint_up2low,0,1,0);

dJointSetHingeParam (joint_up2low,dParamLoStop,-dInfinity);

dJointSetHingeParam (joint_up2low,dParamHiStop,dInfinity);

dJointSetHingeParam (joint_up2low,dParamFudgeFactor,0.1);

//--- joint up to evnvironment

joint_up2env = dJointCreateHinge (world,0);

dJointAttach (joint_up2env, body_up,0);

dJointSetHingeAnchor (joint_up2env,

joint_up2env_x,joint_up2env_y,joint_up2env_z);

dJointSetHingeAxis ( joint_up2env,0,1,0);

dJointSetHingeParam (joint_up2env,dParamLoStop,-dInfinity);

dJointSetHingeParam (joint_up2env,dParamHiStop,dInfinity);

dJointSetHingeParam (joint_up2env,dParamFudgeFactor,0.1);

//--- add pendulum to world

geom_group = dSimpleSpaceCreate (space);

dSpaceSetCleanup (geom_group,0);

dSpaceAdd(geom_group,box_up);

dSpaceAdd(geom_group,box_low);

//--- run simulation

dsSimulationLoop (argc,argv,640,480,&fn);

//--- destroy

dGeomDestroy(box_up);

dGeomDestroy(box_low);

dJointGroupDestroy (contactgroup);

dSpaceDestroy (space);

dWorldDestroy (world);

return 0;

}

Report this snippet Tweet

Comments

Subscribe to comments

Comment:

You need to login to post a comment.