/ 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
http://www.koj-m.sakura.ne.jp/ode/index.php?%A5%C7%A5%E2%A1%CA%B4%F0%CB%DC%CA%D4%A1%CB
Expand |
Embed | Plain Text
Copy this code and paste it in your HTML
#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; }