[ODE] Bug in ODE !

[Marco Vienup]

Hi,
since some days I try to understand the results from the dJointGetFeedback
function.
I set up a small example at the end of the mail. I would be very amused if
someone
could take a look on it !
At the moment I am pretty sure that ode calculates the torques wrong or sets
the wrong
values in the struct. The torques oscillate between positive and negative
values, even
if there is no motion in the simulation ! And the values are definite wrong.
Maybe I am
just to stupid to understand it. But I hope someone here is able to
interpret the results.

Thanks
Marco



#define DENSITY (5.0)		// density of all objects
#define GPB 3			// maximum number of geometries per body
#define MAX_CONTACTS 4		// maximum number of contact points per body


static dWorldID world;
static dSpaceID space;
static dJointGroupID contactgroup;


static dBodyID bodyA;
static dGeomID geomA;
static dBodyID bodyB;
static dGeomID geomB;
static dJointID hinge;
static dJointFeedback feedback;

static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  int i;
  // if (o1->body && o2->body) return;

  // exit without doing anything if the two bodies are connected by a joint
  dBodyID b1 = dGeomGetBody(o1);
  dBodyID b2 = dGeomGetBody(o2);
  if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;

  dContact contact[MAX_CONTACTS];   // up to MAX_CONTACTS contacts per
box-box
  for (i=0; i<MAX_CONTACTS; i++) {
    contact[i].surface.mode = dContactBounce | dContactSoftCFM;
    contact[i].surface.mu = dInfinity;
    contact[i].surface.mu2 = 0;
    contact[i].surface.bounce = 0.1;
    contact[i].surface.bounce_vel = 0.1;
    contact[i].surface.soft_cfm = 0.01;
  }
  if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
			   sizeof(dContact))) {
    dMatrix3 RI;
    dRSetIdentity (RI);
    const dReal ss[3] = {0.02,0.02,0.02};
    for (i=0; i<numc; i++) {
      dJointID c = dJointCreateContact (world,contactgroup,contact+i);
      dJointAttach (c,b1,b2);
      dsDrawBox (contact[i].geom.pos,RI,ss);
    }
  }
}

static void start()
{

}

static void command (int cmd)
{

}

void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
	pos=dGeomGetPosition(geomA);
    R  =dGeomGetRotation(geomA);

	dVector3 sides;
    dGeomBoxGetLengths(geomA, sides);
    dsDrawBox(pos, R, sides);

	pos=dGeomGetPosition(geomB);
    R  =dGeomGetRotation(geomB);

    dGeomBoxGetLengths(geomB, sides);
    dsDrawBox(pos, R, sides);
}

static void simLoop (int pause)
{
	dSpaceCollide (space,0,&nearCallback);

	dWorldStep (world, 0.001);

	//Calculate hinge velocity
	dReal Gain = 0.1;
	dReal TruePosition = dJointGetHingeAngle(hinge);
	dReal DesiredPosition = 0;
	dReal Error = TruePosition - DesiredPosition;

	dReal DesiredVelocity = Error * Gain;

	//Set parameter
	dJointSetHingeParam(hinge, dParamFMax, 100.0);
	dJointSetHingeParam(hinge, dParamVel, -DesiredVelocity);

	//Read feedback
	dJointFeedback* fb=dJointGetFeedback(hinge);

	//Get hinge axis
	dVector3 axis;
	dJointGetHingeAxis(hinge, axis);

	//Calculate torque
	double torque1=(fb->t1[0]*axis[0])+(fb->t1[1]*axis[1])+(fb->t1[2]*axis[2]);
	double torque2=(fb->t2[0]*axis[0])+(fb->t2[1]*axis[1])+(fb->t2[2]*axis[2]);

	printf("t1: %f t2: %f v:%f\n", torque1, torque2, DesiredVelocity);

  // remove all contact joints
    dJointGroupEmpty (contactgroup);

    drawGeom (0,0,0,false);
}

int main (int argc, char **argv)
{
  // setup pointers to drawstuff callback functions
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = &command;
  fn.stop = 0;
  fn.path_to_textures = "D:/ode-0.5/drawstuff/textures";

  // create world
  world = dWorldCreate();
  space = dHashSpaceCreate (0);
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world, 0, 0, -1.0);
  dWorldSetCFM (world,1e-5);
  dCreatePlane (space,0,0,1,0);


  //Create Bodys
  bodyA = dBodyCreate(world);
  bodyB = dBodyCreate(world);

  //Set Position
  dBodySetPosition(bodyA, 0.0, 0.0, 1.5);
  dBodySetPosition(bodyB, 0.0, 1.0, 2.5);

  //Set Rotation
  dMatrix3 R;
  dRFromAxisAndAngle(R, 0.0, 0.0, 0.0, 0.0);
  dBodySetRotation(bodyA, R);
  dBodySetRotation(bodyB, R);

  //Create Mass Box
  dMass m;
  dMassSetBox(&m, DENSITY, 1.0, 1.0, 1.0);

  //Create Geom
  geomA=dCreateBox(space, 1.0, 1.0, 1.0);
  geomB=dCreateBox(space, 1.0, 1.0, 1.0);

  //Add Geom and Body
  dGeomSetBody(geomA, bodyA);
  dGeomSetBody(geomB, bodyB);

  dBodySetMass(bodyA, &m);
  dBodySetMass(bodyB, &m);

  //Reset Mass
  dBodyGetMass(bodyA, &m);
  dMassAdjust(&m, 10.0);
  dBodySetMass(bodyA, &m);

  dBodyGetMass(bodyB, &m);
  dMassAdjust(&m, 1.0);
  dBodySetMass(bodyB, &m);

  //Create Hinge
  hinge=dJointCreateHinge(world, 0);
  dJointAttach(hinge, bodyA, bodyB);
  dJointSetHingeAnchor(hinge, 0, 0.5, 2.0);
  dJointSetHingeAxis(hinge, 1.0, 0, 0);

  //Set Feedback pointer
  dJointSetFeedback(hinge, &feedback);

  // run simulation
  dsSimulationLoop (argc,argv,352,288,&fn);

  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);

  return 0;
}


---
Marco Vienup
Tel: 0177/7963323
Mail: Marco at Vienup.de