[ODE] Instability problem

javilon pirilon pirilon at lycos.com
Sat Apr 6 03:21:02 2002
Previous message: [ODE] ODE / Mac OS X Crash
Next message: [ODE] dBodySetFiniteRotationAxis usage
Messages sorted by: [ date ] [ thread ] [ subject ] [ author ]
I have an instability problem that looks like a problem in ode. The structure I have coded behaves ok when the simulation starts. When it rotates and reaches a given angle measured against the worl, the aMotor joint that works in the knee of the legs of the structure starts misbehaving.

I have tried changing the ERP and CFM as per de docs, to no avail.

The code is quite small and simple, so maybe somebody could have a look to it.

Here is the code:

----------------------------------------------------------------------------

#include <ode/ode.h>
#include <drawstuff/drawstuff.h>

// select correct drawing functions
// select correct drawing functions

#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCappedCylinder dsDrawCappedCylinderD
#endif
//

#define SIDE (0.5f)	// side length of a box - don't change this
#define MASS (1.0)	// mass of a box
#define STEPSIZE 0.05

#define LEG_LENGTH (1.5*SIDE)
#define LEG_RADIUS (0.05*SIDE)
#define FOOT_LENGTH (2*SIDE)
#define FOOT_RADIUS (0.1*SIDE)
#define STRIDE_LENGTH (0.3)
#define CYCLE 200

// dynamics objects
static dWorldID world;
static dSpaceID space;   //geometry space
static dJointGroupID contactgroup;

static dBodyID body;

static dBodyID leg[6];
static dJointID legJoint[6];
static dJointID aMotorJoint[6];

static dBodyID foot[6];
static dJointID footMotor[6];
static dGeomID footGeometry[6];
static dJointID footJoint[6];

static long int time=0;


void dBodyToWorld (dReal *posWorld,  dBodyID bodyID, dReal *posBody){

	// rotate point
	dMULTIPLY0_331 (posWorld, dBodyGetRotation(bodyID), posBody);

	// add center of mass
	const dReal* pos = dBodyGetPosition(bodyID);
	posWorld[0] += pos[0];
	posWorld[1] += pos[1];
	posWorld[2] += pos[2];
}

dReal generateCycle(dReal phase){
	dReal angle = 2 * M_PI * (dReal)time / CYCLE;
	return sin( angle + phase);
}


// this is called by dSpaceCollide when two objects in space are
// potentially colliding.

static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  dBodyID b1 = dGeomGetBody(o1);
  dBodyID b2 = dGeomGetBody(o2);

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


// applyAngularForce will try to get the joint to move to the given angle.
void applyAngularForce(dJointID jointID, dJointID motorID, dReal expectedAngle){
	dReal actualAngle = dJointGetHingeAngle(jointID);
	if ( (expectedAngle - actualAngle > M_PI/2) || (expectedAngle - actualAngle < -M_PI/2) ){
		dJointSetAMotorParam(motorID, dParamVel, 1);	
	}
	else{
		dReal requiredAngularSpeed = 10* (expectedAngle - actualAngle);
		if ((requiredAngularSpeed > .01) || (requiredAngularSpeed < .01)){
			dJointSetAMotorParam(motorID, dParamVel, requiredAngularSpeed);	
		}
		//printf("actualAngle = %f  expectedAngle = %f  requiredAngularSpeed = %f \n", actualAngle, expectedAngle, requiredAngularSpeed);
		//if (jointID==footJoint[0]) printf("%d, %f, %f, %f \n", time, actualAngle, expectedAngle, requiredAngularSpeed);
	}
}


void forces(dReal ks, int i)
{
	dReal legExpectedAngle = STRIDE_LENGTH * generateCycle((i % 3) * M_PI/3);
	if (i<3) legExpectedAngle = -legExpectedAngle;
	
	if (i==0) legExpectedAngle -= M_PI/5;
	if (i==1) legExpectedAngle += M_PI/5;
	if (i==3) legExpectedAngle += M_PI/5;
	if (i==4) legExpectedAngle -= M_PI/5;
	
	applyAngularForce(legJoint[i], aMotorJoint[i], legExpectedAngle);	
	
	dReal footExpectedAngle = .1 * generateCycle((i % 3) * M_PI/6 + M_PI/2);

	if (i<3) applyAngularForce(footJoint[i], footMotor[i], footExpectedAngle + M_PI/8);
	if (i>2) applyAngularForce(footJoint[i], footMotor[i], footExpectedAngle - M_PI/8);

//	if (i<3) applyAngularForce(footJoint[i], footMotor[i], 0 + M_PI/8);
//	if (i>2) applyAngularForce(footJoint[i], footMotor[i], 0 - M_PI/8);

	
	//dump legs with air friction?
	const dReal* vel = dBodyGetLinearVel(foot[i]);
	dBodyAddForce(foot[i], -.1*vel[0], -.1*vel[1], -.1*vel[2]); 
}

dReal doStuffAndGetError ()
{
	for (int i=0;i<6;i++){
			forces(10, i);
	}
	return 0;
}

//****************************************************************************
// test world construction and utilities

void constructWorldForTest ()
{
//  dReal gravity = -0.05f;
  dReal gravity = -0.00f;  
	
	
	
	// create world
  world = dWorldCreate();
  space = dHashSpaceCreate();
  dWorldSetERP (world,0.2);
  dWorldSetCFM (world,1e-6);
  dCreatePlane (space,0,0,1,0); //create ground geometry
  dWorldSetGravity (world,0,0,gravity);

  dMass m;
  dMassSetBox (&m,10,SIDE,SIDE,SIDE);
  dMassAdjust (&m,10*MASS);

  body = dBodyCreate (world);
  dBodySetMass (body,&m);
  dBodySetPosition (body, 0, 0, 2);
  dQuaternion q;
  dQFromAxisAndAngle (q,0,0,0,0);
  dBodySetQuaternion (body,q);

	int i;

	// legs
	for (i=0; i<6; i++){
		dMass mCylinder;
		dMassSetCappedCylinder (&mCylinder, 0.1, 3,	LEG_RADIUS, LEG_LENGTH);
		dMassAdjust (&mCylinder,MASS);
		// legs
		leg[i] = dBodyCreate (world);
		dBodySetMass (leg[i], &mCylinder);
		dQuaternion qCylinder;
		dVector3 p0;
		switch (i){
			case 0:
				dBodySetPosition (leg[i], SIDE, SIDE/2, 2); 
				p0[0]=SIDE/2;
				p0[1]=SIDE/2;
				p0[2]=-SIDE/2;
//old//				dQFromAxisAndAngle (qCylinder,-1,1,0,M_PI/4);
				dQFromAxisAndAngle (qCylinder,0,1,0,M_PI/4);
				break;
			case 1:
				dBodySetPosition (leg[i], SIDE, -SIDE/2, 2); 
				p0[0]=SIDE/2;
				p0[1]=-SIDE/2;
				p0[2]=-SIDE/2;
//				dQFromAxisAndAngle (qCylinder,1,1,0,M_PI/4);
				dQFromAxisAndAngle (qCylinder,0,1,0,M_PI/4);
				break;
			case 2:
				dBodySetPosition (leg[i], SIDE, 0, 2);
				p0[0]=SIDE/2;
				p0[1]=0;
				p0[2]=-SIDE/2;
				dQFromAxisAndAngle (qCylinder,0,1,0,M_PI/4);
				break;
			case 3:
				dBodySetPosition (leg[i], -SIDE, SIDE/2, 2);
				p0[0]=-SIDE/2;
				p0[1]=SIDE/2;
				p0[2]=-SIDE/2;
//				dQFromAxisAndAngle (qCylinder,-1,-1,0,M_PI/4);
				dQFromAxisAndAngle (qCylinder,0,-1,0,M_PI/4);
				break;
			case 4:
				dBodySetPosition (leg[i], -SIDE, -SIDE/2, 2);
				p0[0]=-SIDE/2;
				p0[1]=-SIDE/2;
				p0[2]=-SIDE/2;
//				dQFromAxisAndAngle (qCylinder,1,-1,0,M_PI/4);
				dQFromAxisAndAngle (qCylinder,0,-1,0,M_PI/4);
				break;
			case 5:
				dBodySetPosition (leg[i], -SIDE, 0, 2);
				p0[0]=-SIDE/2;
				p0[1]=0;
				p0[2]=-SIDE/2;
				dQFromAxisAndAngle (qCylinder,0,-1,0,M_PI/4);
				break;

		}
		dBodySetQuaternion (leg[i],qCylinder);

		dVector3 pos;
		dBodyToWorld(pos, body, p0);
		legJoint[i] = dJointCreateHinge (world, 0);
		dJointAttach (legJoint[i], body,leg[i]);
		dJointSetHingeAxis(legJoint[i], 0,0,1);
		dJointSetHingeAnchor (legJoint[i], pos[0],pos[1],pos[2]);

		aMotorJoint[i] = dJointCreateAMotor(world,0);
		dJointAttach (aMotorJoint[i],body,leg[i]);

		dJointSetAMotorNumAxes (aMotorJoint[i],1);
		dJointSetAMotorAxis (aMotorJoint[i],0,1, 0,0,1);
		dJointSetAMotorParam(aMotorJoint[i], dParamVel, 1);
		dJointSetAMotorParam(aMotorJoint[i], dParamFMax, 1);
		
	}
	
	
	// foot

	for (i=0; i<6; i++){
		dMass mCylinder;
		dMassSetCappedCylinder (&mCylinder, 1, 3,	FOOT_RADIUS, FOOT_LENGTH);
		dMassAdjust (&mCylinder,MASS);
		// foot
		foot[i] = dBodyCreate (world);
		dBodySetMass (foot[i], &mCylinder);
		dVector3 p0 = {0,0,FOOT_LENGTH/2};
		dVector3 pos ;

		dBodyToWorld(pos, leg[i], p0);
		dBodySetPosition (foot[i], pos[0], pos[1], pos[2]-FOOT_LENGTH/2);

		dQuaternion qCylinder;
		dQFromAxisAndAngle (qCylinder,0,0,0,0);
		dBodySetQuaternion (foot[i],qCylinder);

		footJoint[i] = dJointCreateHinge (world, 0);
		dJointAttach (footJoint[i], leg[i],foot[i]);
		dJointSetHingeAxis(footJoint[i], 0,1,0);
		dJointSetHingeAnchor (footJoint[i], pos[0],pos[1],pos[2]);

		footMotor[i] = dJointCreateAMotor(world,0);
		dJointAttach (footMotor[i],leg[i],foot[i]);

		dJointSetAMotorNumAxes (footMotor[i],1);
		dJointSetAMotorAxis (footMotor[i],0,1, 0,1,0);
		dJointSetAMotorParam(footMotor[i], dParamVel, 0);
		dJointSetAMotorParam(footMotor[i], dParamFMax, 1);
		
		// geometry for collision detection
		footGeometry[i] = dCreateCCylinder (space, FOOT_RADIUS, FOOT_LENGTH);
		dGeomSetBody (footGeometry[i], foot[i]);
		
	}

}

// start simulation - set viewpoint

static void start()
{
  static float xyz[3] = {8*1.0382f,-8*1.0811f,1.4700f};
  static float hpr[3] = {135.0000f,-0.1*19.5000f,0.0000f};
  dsSetViewpoint (xyz,hpr);
}


// simulation loop

static void simLoop (int pause)
{

  if (!pause) {
		time++;

    // do stuff for this test and check to see if the joint is behaving well
    doStuffAndGetError ();
		
		dSpaceCollide (space,0,&nearCallback);

    // take a step
    dWorldStep (world,STEPSIZE);
		
		dJointGroupEmpty (contactgroup);
  }

	dReal sides1[3] = {SIDE,SIDE,SIDE};
	dsSetTexture (DS_WOOD);
	dsSetColor (1,1,0);
	dsDrawBox (dBodyGetPosition(body),dBodyGetRotation(body),sides1);
	
	int i;
	for (i=0;i<6;i++){
		dsSetColor (1,1,0);
		dsDrawCappedCylinder (dBodyGetPosition(leg[i]), dBodyGetRotation(leg[i]), LEG_LENGTH, LEG_RADIUS);
		
	}
	for (i=0;i<6;i++){
		dsSetColor (1,1,0);
		dsDrawCappedCylinder (dBodyGetPosition(foot[i]), dBodyGetRotation(foot[i]), FOOT_LENGTH, FOOT_RADIUS);
	}
	
	for (i=0;i<6;i++){
		dVector3 footHingeAnchor;
		dJointGetHingeAnchor(footJoint[i], footHingeAnchor);
		dsDrawCappedCylinder (footHingeAnchor, dBodyGetRotation(foot[i]), .01, 1.5*FOOT_RADIUS);

		dVector3 legHingeAnchor;
		dJointGetHingeAnchor(legJoint[i], legHingeAnchor);
		dsDrawCappedCylinder (legHingeAnchor, dBodyGetRotation(leg[i]), .01, 1.5*FOOT_RADIUS);
	}
}

// called when a key pressed

static void command (int cmd)
{

}


//****************************************************************************
// conduct a specific test, and report the results

void doTest (int argc, char **argv)
{
	constructWorldForTest ();
  // 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 = "../../drawstuff/textures";

  contactgroup = dJointGroupCreate (0);
	
//  dsSimulationLoop (argc,argv,352,288,&fn);
  dsSimulationLoop (argc,argv,800,600,&fn);
	
  dJointGroupDestroy (contactgroup);
  dWorldDestroy (world); 
}


int main (int argc, char **argv)
{
  doTest (argc, argv);
  return 0;
}

----------------------------------------------------------------------------



See Dave Matthews Band live or win a signed guitar
http://r.lycos.com/r/bmgfly_mail_dmb/http://win.ipromotions.com/lycos_020201/splash.asp
Previous message: [ODE] ODE / Mac OS X Crash
Next message: [ODE] dBodySetFiniteRotationAxis usage
Messages sorted by: [ date ] [ thread ] [ subject ] [ author ]