ODE仿真引擎使用

Posted 2021-03-11 zhaochenliang

1. Force and torque of a body
dBodyGetForce() and dBodyGetTorqueForce() get force and torque on a body. dBodySetForce() and dBodySetTorque() set force and torque on the body. The coordinate system is the absolute coordinate system.
const dReal * dBodyGetForce (dBodyID body);
const dReal * dBodyGetTorque (dBodyID body);
Get force and torque vector of a body.Return a pointer to the array of 3 elements.
void dBodySetForce (dBodyID body, dReal x, dReal y, dReal z);
void dBodySetTorque (dBodyID body, dReal x, dReal y, dReal z);
Set force and torque vector.
2. Force and torque of a joint
(1) Get force and torque
For the next, let’s study how to get force and torque of a joint. In order to get force and torque, firstly, dJointSetFeedback() must be called to specify a joint, secondly, call dJointGetFeedback() to get the information. This is to improve performance.You do not always needs force and torque information of all joints.
void dJointSetFeedback(dJointID, dJointFeedback *);
Set the joint to get force and torque. The dJointFeedback structure is defined as follows.

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;

dJointFeedback *dJointGetFeedback(dJointID);
Get the information about force and torque of the joint
(2) Set power and torque
Depending on the type of joints, force or torque will be set. In other words, such as a rotary joint, a hinge joint, torque must be applied. For a slider joint, force must be applied.
dJointAddHingeTorque (dJointID joint, dReal torque)
Add torque to the hinge joint.
dJointAddSliderForce (dJointID joint, dReal force)
Add force to the slider joint.
Next, a sample program using these APIs are introduced.Two boxes are connected by a fixed joint. Weight of each box is 1[kg]. So, the force along z-axis should be 9.8 [N] in theoretically. In my environment, the simulated value is about 9.8 [N].
You can easily make pressure sensors for a humanoid robot using this sample program.

  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 
",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 }