ODE仿真引擎使用(十一)

1 typedef struct dJointFeedback {
2   dVector3 f1;  //  joints in the body influence the power of one
3   dVector3 t1;  //  joints in the body influence the torque 1
4   dVector3 f2;  //  joints in the body influence the power of 2
5   dVector3 t2;  //  joints in the body influence the torque 2
6 } dJointFeedback;

  1 // 力和力矩
  2 #include 
  3 #include <drawstuff/drawstuff.h>
  4 
  5 static dWorldID world;
  6 static dSpaceID space;
  7 static dGeomID  ground;
  8 static dJointID fixed;
  9 static dJointGroupID contactgroup;
 10 dJointFeedback *feedback = new dJointFeedback;
 11 
 12 dsFunctions fn;
 13 typedef struct {
 14   dBodyID body;
 15   dGeomID geom;
 16   dReal   radius,length,width,height,mass;
 17 } myLink;
 18 
 19 myLink box,sensor;
 20 
 21 static void nearCallback (void *data, dGeomID o1, dGeomID o2)
 22 {
 23   static int MAX_CONTACTS = 10;
 24   int i;
 25 
 26   dBodyID b1 = dGeomGetBody(o1);
 27   dBodyID b2 = dGeomGetBody(o2);
 28 
 29     if (b1 && b2 && dAreConnected (b1,b2)) return;
 30 
 31     dContact contact[MAX_CONTACTS];
 32     int numc = dCollide(o1,o2,MAX_CONTACTS,&contact[0].geom,   sizeof(dContact));
 33     if (numc > 0) {
 34       for (i=0; i < numc; i++) {
 35         contact[i].surface.mode  =  dContactSoftCFM | dContactSoftERP;
 36         contact[i].surface.mu       = dInfinity;　　
 37         contact[i].surface.soft_cfm = 1e-8;
 38         contact[i].surface.soft_erp = 1.0;
 39         dJointID c = dJointCreateContact(world,contactgroup,&contact[i]);
 40         dJointAttach (c,dGeomGetBody(contact[i].geom.g1),        dGeomGetBody(contact[i].geom.g2));
 41      }
 42    }
 43 }
 44 
 45 static void simLoop (int pause)
 46 {
 47   static int steps = 0;
 48   dSpaceCollide(space,0,&nearCallback);
 49   dWorldStep(world,0.01);
 50   dJointGroupEmpty(contactgroup);
 51   feedback = dJointGetFeedback(fixed);　// Get force and torque of the joint
 52   printf("%5d Force fx=%6.2f ",steps++,feedback->f1[0]);　// x axis
 53   printf("fy=%6.2f ",feedback->f1[1]);　　　　　　　　　　　// y axis
 54   printf("fz=%6.2f \n",feedback->f1[2]);　　　　　　　　　　// z axis
 55 
 56   // Draw a box
 57   dsSetColor(1.0,0.0,0.0);
 58   dReal sides1[] = {box.length,box.width,box.height};
 59   dsDrawBoxD(dBodyGetPosition(box.body),
 60   dBodyGetRotation(box.body),sides1);    // Draw a sensor
 61   dsSetColor(0.0,0.0,1.0);
 62   dReal sides2[] = {sensor.length,sensor.width,sensor.height};
 63   dsDrawBoxD(dBodyGetPosition(sensor.body),
 64   dBodyGetRotation(sensor.body),sides2);
 65 }
 66 
 67 void start()
 68 {
 69   static float xyz[3] = {0.0,-3.0,1.0};
 70   static float hpr[3] = {90.0,0.0,0.0};
 71   dsSetViewpoint (xyz,hpr);
 72 }
 73 
 74 void  setDrawStuff()
 75 {
 76   fn.version = DS_VERSION;
 77   fn.start   = &start;
 78   fn.step    = &simLoop;
 79   fn.command = NULL;
 80   fn.stop    = NULL;
 81    fn.path_to_textures = "../../drawstuff/textures";
 82 }
 83 
 84 int main (int argc, char **argv)
 85 {
 86   setDrawStuff();
 87   dInitODE();
 88   world = dWorldCreate();
 89   space = dHashSpaceCreate(0);
 90   contactgroup = dJointGroupCreate(0);
 91   dWorldSetGravity(world,0,0,-9.8);
 92   ground = dCreatePlane(space,0,0,1,0);
 93   dMass m1;   dReal x0 = 0.0, y0 = 0.0, z0 = 0.0;
 94 
 95   // A sensor (lower box)
 96   sensor.length = 0.2;
 97   sensor.width  = 0.2;
 98   sensor.height = 0.2;
 99   sensor.mass   = 1.0;
100   sensor.body   = dBodyCreate(world);
101   dMassSetZero(&m1);
102   dMassSetBoxTotal(&m1,sensor.mass,sensor.length,sensor.width,sensor.height);
103   dBodySetMass(sensor.body,&m1);
104   dBodySetPosition(sensor.body, x0, y0, 0.5 * sensor.height + z0);
105   sensor.geom = dCreateBox(space,sensor.length,sensor.width,sensor.height);
106   dGeomSetBody(sensor.geom,sensor.body);
107 
108   // The upper box
109   box.length = 0.2;
110   box.width  = 0.2;
111   box.height = 0.2;
112   box.mass   = 1.0;
113   box.body   = dBodyCreate(world);
114   dMassSetZero(&m1);
115   dMassSetBoxTotal(&m1,box.mass,box.length,box.width,box.height);
116   dBodySetMass(box.body,&m1);
117   dBodySetPosition(box.body, x0, y0, sensor.height + 0.5 * box.height + z0);
118   box.geom = dCreateBox(space,box.length,box.width,box.height);
119   dGeomSetBody(box.geom,box.body);
120 
121   // A fixed joint
122   fixed = dJointCreateFixed(world,0);
123   dJointAttach(fixed,box.body,sensor.body);
124   dJointSetFixed(fixed);    // Set a joint to get information about force and torque
125   dJointSetFeedback(fixed,feedback);
126 
127   dsSimulationLoop(argc,argv,352,288,&fn);
128 
129   dWorldDestroy(world);
130   dCloseODE();
131   return 0;
132 }