demura.net:RoboCup・ODE・夢考房

This is the 11th ODE (Open Dynamics Engine) tutorial. Let's study force and torque.

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 b, dReal x, dReal y, dReal z);
• void dBodySetTorque (dBodyID b, 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.
  

  
  

  
  

  typedef struct dJointFeedback (
  dVector3 f1;  //  joints in the body influence the power of one
  dVector3 t1;  //  joints in the body influence the torque 1
  dVector3 f2;  //  joints in the body influence the power of 2
  dVector3 t2;  //  joints in the body influence the torque 2
  ) 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 1kg. So, the force along z-axis should be 9.8 [N] in thoretically. 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.

The sample program, sample11.cpp,  can be downloaded from here.

PLAIN TEXT
C++:

// sample11.cpp  by Kosei Demura  2006-2008
#include "ode/ode.h"
#include "drawstuff/drawstuff.h"
 
static dWorldID world;
static dSpaceID space;
static dGeomID  ground;
static dJointID fixed;
static dJointGroupID contactgroup;
dJointFeedback *feedback = new dJointFeedback;
dsFunctions fn;
 
typedef struct {
  dBodyID body;
  dGeomID geom;
  dReal   radius,length,width,height,mass;
} myLink;
myLink box,sensor;
 
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  static int MAX_CONTACTS = 10;
  int i;
 
  dBodyID b1 = dGeomGetBody(o1);
  dBodyID b2 = dGeomGetBody(o2);
  if (b1 && b2 && dAreConnected (b1,b2)) return;
 
  dContact contact[MAX_CONTACTS];   //
  int numc = dCollide(o1,o2,MAX_CONTACTS,&contact[0].geom,
  sizeof(dContact));
  if (numc> 0) {
    for (i=0; i <numc; i++) {
       contact[i].surface.mode  =  dContactSoftCFM | dContactSoftERP;
       contact[i].surface.mu       = dInfinity;　　
       contact[i].surface.soft_cfm = 1e-8;
       contact[i].surface.soft_erp = 1.0;
       dJointID c = dJointCreateContact(world,contactgroup,&contact[i]);
       dJointAttach (c,dGeomGetBody(contact[i].geom.g1),
       dGeomGetBody(contact[i].geom.g2));
    }
  }
}
 
static void simLoop (int pause)
{
  static int steps = 0;
 
  dSpaceCollide(space,0,&nearCallback);
  dWorldStep(world,0.01);
  dJointGroupEmpty(contactgroup);
 
  feedback = dJointGetFeedback(fixed);　// Get force and torque of the joint
  printf("%5d Force fx=%6.2f ",steps++,feedback->f1[0]);　// x axis
  printf("fy=%6.2f ",feedback->f1[1]);　　　　　　　　　　　// y axis
  printf("fz=%6.2f \n",feedback->f1[2]);　　　　　　　　　　// z axis
 
  // Draw a box
  dsSetColor(1.0,0.0,0.0);
  dReal sides1[] = {box.length,box.width,box.height};
  dsDrawBoxD(dBodyGetPosition(box.body),
  dBodyGetRotation(box.body),sides1);
 
  // Draw a sensor
  dsSetColor(0.0,0.0,1.0);
  dReal sides2[] = {sensor.length,sensor.width,sensor.height};
  dsDrawBoxD(dBodyGetPosition(sensor.body),
  dBodyGetRotation(sensor.body),sides2);
}
 
void start()
{
  static float xyz[3] = {0.0,-3.0,1.0};
  static float hpr[3] = {90.0,0.0,0.0};
  dsSetViewpoint (xyz,hpr);
}
 
void  setDrawStuff()
{
  fn.version = DS_VERSION;
  fn.start   = &start;
  fn.step    = &simLoop;
  fn.command = NULL;
  fn.stop    = NULL;
  fn.path_to_textures = "../../drawstuff/textures";
}
 
int main (int argc, char **argv)
{
  setDrawStuff();
  dInitODE();
  world = dWorldCreate();
  space = dHashSpaceCreate(0);
  contactgroup = dJointGroupCreate(0);
  dWorldSetGravity(world,0,0,-9.8);
 
  ground = dCreatePlane(space,0,0,1,0);
 
  dMass m1;
  dReal x0 = 0.0, y0 = 0.0, z0 = 0.0;
 
  // A sensor (lower box)
  sensor.length = 0.2;  sensor.width  = 0.2;
  sensor.height = 0.2;  sensor.mass   = 1.0;
  sensor.body   = dBodyCreate(world);
  dMassSetZero(&m1);
  dMassSetBoxTotal(&m1,sensor.mass,sensor.length,sensor.width,sensor.height);
  dBodySetMass(sensor.body,&m1);
  dBodySetPosition(sensor.body, x0, y0, 0.5 * sensor.height + z0);
 
  sensor.geom = dCreateBox(space,sensor.length,sensor.width,sensor.height);
  dGeomSetBody(sensor.geom,sensor.body);
 
  // The upper box
  box.length = 0.2;  box.width  = 0.2;
  box.height = 0.2;  box.mass   = 1.0;
  box.body   = dBodyCreate(world);
  dMassSetZero(&m1);
  dMassSetBoxTotal(&m1,box.mass,box.length,box.width,box.height);
  dBodySetMass(box.body,&m1);
  dBodySetPosition(box.body, x0, y0, sensor.height + 0.5 * box.height + z0);
 
  box.geom = dCreateBox(space,box.length,box.width,box.height);
  dGeomSetBody(box.geom,box.body);
 
  // A fixed joint
  fixed = dJointCreateFixed(world,0);
  dJointAttach(fixed,box.body,sensor.body);
  dJointSetFixed(fixed);
 
  // Set a joint to get information about force and torque
  dJointSetFeedback(fixed,feedback);
 
  dsSimulationLoop(argc,argv,352,288,&fn);
  dWorldDestroy(world);
  dCloseODE();
  return 0;
} 



That's all. See you !

demu