2 armed pendulum control start kit using Open Dynamics Engine


/ Published in: C++
Save to your folder(s)

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


Copy this code and paste it in your HTML
  1. #include <ode/ode.h>
  2.  
  3. #include <drawstuff/drawstuff.h>
  4.  
  5.  
  6.  
  7. // select correct drawing functions
  8.  
  9. #ifdef dDOUBLE
  10.  
  11. #define dsDrawBox dsDrawBoxD
  12.  
  13. #define dsDrawSphere dsDrawSphereD
  14.  
  15. #define dsDrawCylinder dsDrawCylinderD
  16.  
  17. #define dsDrawCapsule dsDrawCapsuleD
  18.  
  19. #endif
  20.  
  21.  
  22.  
  23. static dWorldID world;
  24.  
  25. static dSpaceID space;
  26.  
  27. static dJointGroupID contactgroup;
  28.  
  29. static dGeomID ground;
  30.  
  31. static dBodyID body_up,body_low;
  32.  
  33. static dGeomID box_up,box_low;
  34.  
  35. static dJointID joint_up2low;
  36.  
  37. static dJointID joint_up2env;
  38.  
  39. static dSpaceID geom_group;
  40.  
  41.  
  42.  
  43. // box location, size, mass
  44.  
  45. double box_up_loc[3] = {0 , 0 , 1 };
  46.  
  47. double box_low_loc[3] = {0.4, 0 , 1 };
  48.  
  49. double box_up_size[3] = {0.4, 0.1, 0.1};
  50.  
  51. double box_low_size[3] = {0.4, 0.1, 0.1};
  52.  
  53. double box_up_mass=1.0, box_low_mass=1.0;
  54.  
  55.  
  56.  
  57. // joint location
  58.  
  59. double joint_up2low_x, joint_up2low_y, joint_up2low_z,
  60.  
  61. joint_up2env_x, joint_up2env_y, joint_up2env_z;
  62.  
  63.  
  64.  
  65. #define DT 0.001
  66.  
  67. #define PI 3.14159
  68.  
  69.  
  70.  
  71. // start simulation - set viewpoint
  72.  
  73. static void start()
  74.  
  75. {
  76.  
  77. static float xyz[3] = {0.8317f,-0.9817f,0.8000f};
  78.  
  79. static float hpr[3] = {136.5000f,0.5000f,0.0000f};
  80.  
  81. dsSetViewpoint (xyz,hpr);
  82.  
  83. printf ("start simulation\n");
  84.  
  85. }
  86.  
  87.  
  88.  
  89. // control
  90.  
  91. double theta1, theta2;
  92.  
  93. //double omega1, omega2;
  94.  
  95. int controler_cnt=0;
  96.  
  97. void controler(){
  98.  
  99. controler_cnt++;
  100.  
  101. //--- get angular
  102.  
  103. theta1 = dJointGetHingeAngle (joint_up2env);
  104.  
  105. theta2 = dJointGetHingeAngle (joint_up2low);
  106.  
  107. //omega1 = dJointGetHingeAngleRate (joint_up2env);
  108.  
  109. //omega2 = dJointGetHingeAngleRate (joint_up2low);
  110.  
  111.  
  112.  
  113. //--- control
  114.  
  115. // velocity contorl
  116.  
  117. double v;
  118.  
  119. v = ( (2*PI * sin( 2*PI*(controler_cnt * DT) ))
  120.  
  121. - PI/2 - theta1 );
  122.  
  123. dJointSetHingeParam (joint_up2env,dParamVel,v);
  124.  
  125. dJointSetHingeParam (joint_up2env,dParamFMax,20000);
  126.  
  127. // torque control
  128.  
  129. //dJointAddHingeTorque(joint_up2env,1);
  130.  
  131. }
  132.  
  133.  
  134.  
  135. // simulation loop
  136.  
  137. double t=0;
  138.  
  139. static void simLoop (int pause)
  140.  
  141. {
  142.  
  143. int i,j;
  144.  
  145. if (!pause) {
  146.  
  147. dWorldStep (world,DT);
  148.  
  149. // remove all contact joints
  150.  
  151. dJointGroupEmpty (contactgroup);
  152.  
  153. // control
  154.  
  155. controler();
  156.  
  157. // print real time
  158.  
  159. //if( t > 1 && fmod(t,1) < DT ){ printf("%f sec\n",t); }
  160.  
  161. t += DT;
  162.  
  163. }
  164.  
  165.  
  166.  
  167. dsSetTexture (DS_WOOD);
  168.  
  169. dReal sides[3];
  170.  
  171.  
  172.  
  173. dsSetColor (1,0,0);
  174.  
  175. for(i=0; i<3; i++){ sides[i]=box_up_size[i]; }
  176.  
  177. dsDrawBox( dBodyGetPosition(body_up),
  178.  
  179. dBodyGetRotation(body_up),
  180.  
  181. sides);
  182.  
  183. dsSetColor (0,1,0);
  184.  
  185. for(i=0; i<3; i++){ sides[i]=box_low_size[i]; }
  186.  
  187. dsDrawBox( dBodyGetPosition(body_low),
  188.  
  189. dBodyGetRotation(body_low),
  190.  
  191. sides);
  192.  
  193. }
  194.  
  195.  
  196.  
  197. int main (int argc, char **argv)
  198.  
  199. {
  200.  
  201. dMass m;
  202.  
  203.  
  204.  
  205. //--- setup pointers to drawstuff callback functions
  206.  
  207. dsFunctions fn;
  208.  
  209. fn.version = DS_VERSION;
  210.  
  211. fn.start = &start;
  212.  
  213. fn.step = &simLoop;
  214.  
  215. fn.stop = 0;
  216.  
  217. fn.path_to_textures = "../simulator/ode-0.9/drawstuff/textures";
  218.  
  219. if(argc==2){ fn.path_to_textures = argv[1]; }
  220.  
  221.  
  222.  
  223. //--- create world
  224.  
  225. world = dWorldCreate();
  226.  
  227. space = dHashSpaceCreate (0);
  228.  
  229. contactgroup = dJointGroupCreate (0);
  230.  
  231. dWorldSetGravity (world,0,0,-9.8);
  232.  
  233. ground = dCreatePlane (space,0,0,1,0);
  234.  
  235.  
  236.  
  237. //--- create pendulum arm
  238.  
  239. body_up = dBodyCreate(world);
  240.  
  241. body_low = dBodyCreate(world);
  242.  
  243. dBodySetPosition( body_up,
  244.  
  245. box_up_loc[0], box_up_loc[1], box_up_loc[2]);
  246.  
  247. dBodySetPosition( body_low,
  248.  
  249. box_low_loc[0], box_low_loc[1], box_low_loc[2]);
  250.  
  251. dMassSetBox(&m,1,box_up_size[0], box_up_size[1], box_up_size[2] );
  252.  
  253. dMassSetBox(&m,1,box_low_size[0], box_low_size[1], box_low_size[2] );
  254.  
  255. dMassAdjust(&m,box_up_mass);
  256.  
  257. dMassAdjust(&m,box_low_mass);
  258.  
  259. dBodySetMass(body_up,&m);
  260.  
  261. dBodySetMass(body_low,&m);
  262.  
  263. box_up = dCreateBox(0,box_up_size[0], box_up_size[1], box_up_size[2]);
  264.  
  265. box_low = dCreateBox(0,box_low_size[0], box_low_size[1], box_low_size[2]);
  266.  
  267. dGeomSetBody ( box_up, body_up );
  268.  
  269. dGeomSetBody ( box_low, body_low );
  270.  
  271.  
  272.  
  273. //--- create joint
  274.  
  275. dVector3 a;
  276.  
  277. dBodyGetRelPointPos(body_up,box_up_size[0]/2,0,0,a);
  278.  
  279. joint_up2low_x=a[0];
  280.  
  281. joint_up2low_y=a[1];
  282.  
  283. joint_up2low_z=a[2];
  284.  
  285. dBodyGetRelPointPos(body_up,-box_up_size[0]/2,0,0,a);
  286.  
  287. joint_up2env_x=a[0];
  288.  
  289. joint_up2env_y=a[1];
  290.  
  291. joint_up2env_z=a[2];
  292.  
  293.  
  294.  
  295. //--- joint up to low
  296.  
  297. joint_up2low = dJointCreateHinge (world,0);
  298.  
  299. dJointAttach (joint_up2low,body_up,body_low);
  300.  
  301. dJointSetHingeAnchor (joint_up2low,
  302.  
  303. joint_up2low_x,joint_up2low_y,joint_up2low_z);
  304.  
  305. dJointSetHingeAxis ( joint_up2low,0,1,0);
  306.  
  307. dJointSetHingeParam (joint_up2low,dParamLoStop,-dInfinity);
  308.  
  309. dJointSetHingeParam (joint_up2low,dParamHiStop,dInfinity);
  310.  
  311. dJointSetHingeParam (joint_up2low,dParamFudgeFactor,0.1);
  312.  
  313. //--- joint up to evnvironment
  314.  
  315. joint_up2env = dJointCreateHinge (world,0);
  316.  
  317. dJointAttach (joint_up2env, body_up,0);
  318.  
  319. dJointSetHingeAnchor (joint_up2env,
  320.  
  321. joint_up2env_x,joint_up2env_y,joint_up2env_z);
  322.  
  323. dJointSetHingeAxis ( joint_up2env,0,1,0);
  324.  
  325. dJointSetHingeParam (joint_up2env,dParamLoStop,-dInfinity);
  326.  
  327. dJointSetHingeParam (joint_up2env,dParamHiStop,dInfinity);
  328.  
  329. dJointSetHingeParam (joint_up2env,dParamFudgeFactor,0.1);
  330.  
  331.  
  332.  
  333. //--- add pendulum to world
  334.  
  335. geom_group = dSimpleSpaceCreate (space);
  336.  
  337. dSpaceSetCleanup (geom_group,0);
  338.  
  339. dSpaceAdd(geom_group,box_up);
  340.  
  341. dSpaceAdd(geom_group,box_low);
  342.  
  343.  
  344.  
  345. //--- run simulation
  346.  
  347. dsSimulationLoop (argc,argv,640,480,&fn);
  348.  
  349.  
  350.  
  351. //--- destroy
  352.  
  353. dGeomDestroy(box_up);
  354.  
  355. dGeomDestroy(box_low);
  356.  
  357. dJointGroupDestroy (contactgroup);
  358.  
  359. dSpaceDestroy (space);
  360.  
  361. dWorldDestroy (world);
  362.  
  363.  
  364.  
  365. return 0;
  366.  
  367. }

Report this snippet


Comments

RSS Icon Subscribe to comments

You need to login to post a comment.