11. Force and Torque

 

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 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.

    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 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.

The sample program, sample11.cpp (sample11-110105.zip),  can be downloaded from here.

// sample11.cpp  by Kosei Demura  2006-2008
#include 
#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

スポンサーリンク
シェアする
demura.netをフォローする
タイトルとURLをコピーしました