This page was maily translated from http://demura.net/archives/9ode by Babel Fish Translation.
As been in the question from Jimmy, as for the command which inspects torque and the power which depend on the joint it is net easy to find on the ODE user manual.
With ODE the torque and the like which falls on the joint cannot be acquired immediately. In order to acquire, after appointing the joint with dJointSetFeedback, you acquire information with dJointGetFeedback. As for this is in order to improve performance. It is not the case that power and the torque which fall on all joints always are necessary don’t you think?.
- Void dJointSetFeedback (dJointID and dJointFeedback *);
The joint which acquires the information of power and torque is appointed with dJointID. The dJointFeedback structure is as follows.typedef struct dJointFeedback {
dVector3 f1; // The power which the joint has caused to body 1
dVector3 t1; // The torque which the joint has caused to body 1
dVector3 f2; // The power which the joint has caused to body 2
dVector3 t2; / / The torque which the joint has caused to body 2
} dJointFeedback; - dJointFeedback *dJointGetFeedback (dJointID);
Information of power and torque of the joint which it is designated with dJointID is acquired.
Next, the sample program which used this API is introduced. The box is raised the fixed joint (Fixed Joint) with the land (static object), the stick and the box where the weight has been attached with the fixed joint which becomes the sensor there are connected. It is the program which indicates the power which depends on the sensor. You can ignore the weight of the stick almost with 0.0001kg. It meaning that mass of the weight is 1kg, torque of the around x axis if it is 9.8Nm, is good reason. With my environment according to theory it had started hitting the value of 9.8.
[code]
// torque.cpp by Kosei Demura 2006-12-05
#include
#include
#ifdef dDOUBLE
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawSphere dsDrawSphereD
#endif
static dWorldID world;
static dSpaceID space;
static dGeomID ground;
static dJointID sensor,fixed[2];
static dJointGroupID contactgroup;
dJointFeedback *feedback = new dJointFeedback;
dsFunctions fn;
typedef struct {
dBodyID body;
dGeomID geom;
dReal radius,length,width,height,mass;
} myLink;
myLink stick, ball, box;
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
static int MAX_CONTACTS = 10;
int i;
// exit without doing anything if the two bodies are connected by a joint
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnected(b1,b2)) return;
dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
int numc = dCollide(o1,o2,MAX_CONTACTS,&contact[0].geom,
sizeof(dContact));
if (numc > 0) {
for (i=0; 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(sensor);
printf(“%5d Torque Tx=%6.2f “,steps++,feedback->t1[0]);
printf(“Ty=%6.2f “,feedback->t1[1]);
printf(“Tz=%6.2f \n”,feedback->t1[2]);
// draw a box
dsSetColor(1.3,1.3,0.0);
dReal sides1[] = {box.length,box.width,box.height};
dsDrawBoxD(dBodyGetPosition(box.body),
dBodyGetRotation(box.body),sides1);
// draw a ball
dsSetColor(1.3,0.0,0.0);
dsDrawSphere(dBodyGetPosition(ball.body),
dBodyGetRotation(ball.body),ball.radius);
// draw a stick
dsSetColor(0.0,0.0,1.3);
dsDrawCylinder(dBodyGetPosition(stick.body),
dBodyGetRotation(stick.body),stick.length, stick.radius);
}
void start()
{
static float xyz[3] = {3.0,0.0,1.0};
static float hpr[3] = {180.0,0.0,0.0};
dsSetViewpoint (xyz,hpr);
dsSetSphereQuality(3);
}
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();
world = dWorldCreate();
space = dHashSpaceCreate(0);
contactgroup = dJointGroupCreate(0);
dWorldSetGravity(world,0,0,-9.8);
dWorldSetERP(world,1.0);
// Create a ground
ground = dCreatePlane(space,0,0,1,0);
dMass m1;
dReal x0 = 0.0, y0 = 0.0, z0 = 0.5;
dReal x1 = 0.0, y1 = 0.5, z1 = 1.0;
dReal x2 = 0.0, y2 = 1.0, z2 = 1.0;
dMatrix3 R;
// box
box.length = 0.1; box.width = 0.1;
box.height = 1.0; 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, z0);
box.geom = dCreateBox(space,box.length,box.width,box.height);
dGeomSetBody(box.geom,box.body);
// stick
stick.length = 1.0; stick.radius = 0.01;
stick.mass = 0.0001;
stick.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetCylinderTotal(&m1,stick.mass,3, stick.radius, stick.length);
dBodySetMass(stick.body,&m1);
dBodySetPosition(stick.body, x1, y1, z1);
dRFromAxisAndAngle(R, 1, 0, 0, M_PI/2);
dBodySetRotation(stick.body, R);
stick.geom = dCreateCylinder(space,stick.radius,stick.length);
dGeomSetBody(stick.geom,stick.body);
// ball
ball.radius = 0.1;
ball.mass = 1.0;
ball.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetSphereTotal(&m1,ball.mass,ball.radius);
dBodySetMass(ball.body,&m1);
dBodySetPosition(ball.body, x2, y2, z2);
ball.geom = dCreateSphere(space,ball.radius);
dGeomSetBody(ball.geom,ball.body);
// fixed joint
fixed[0] = dJointCreateFixed(world,0);
dJointAttach(fixed[0],0, box.body);
dJointSetFixed(fixed[0]);
fixed[1] = dJointCreateFixed(world,0);
dJointAttach(fixed[1],stick.body,ball.body);
dJointSetFixed(fixed[1]);
sensor = dJointCreateFixed(world,0);
dJointAttach(sensor,box.body, stick.body);
dJointSetFixed(sensor);
// set feed back
dJointSetFeedback(sensor,feedback);
dsSimulationLoop(argc,argv,640, 480,&fn);
dWorldDestroy(world);
return 0;
}
[/code]
コメント