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