[ODE] motor wierdness

[Darken, Chris]

I was playing around with Gene's biped code this weekend and noticed
something strange.  I don't know that this is a serious issue in any way,
but I thought I would mention it in case anyone is interested.

This wierdness happens when driving motors into their stops when the joint
is set fairly soft.   ODE moves the joint past the stop (as it should, since
the motor is trying to maintain a set velocity with a large max force and
the joint is soft).  The wierd thing is that when the stop is hit, the
angular velocity *increases*.  This looks very strange, to say the least,
and maybe something needs improvement.  Hacked-up example code below.

Chris




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

#define Pi 3.14159

#ifdef MSVC
#pragma warning(disable:4244 4305)  // for VC++, no precision loss
complaints
#endif

// select correct drawing functions

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

// global constants
//const dReal ERP = 0.2;
//const dReal CFM = 0.05;
const dReal ERP = 0.2;
const dReal CFM = 0.05;
const dReal HEAD = 0.12;
const dReal TORSO[3] = {0.1,0.1,.80}; //0.7	// length, width height
const dReal STARTZ = 5.0;

// environment setup
static dWorldID world;
static dGeomID ground, geom_group;
static dJointGroupID contactgroup;
static dSpaceID space;
static dBodyID body[11],head;	// 11 body parts + head
static dGeomID box[11];	 // 11 geometries for body parts
//static dGeomID ground_box[10];
static dJointID joint[10],headj;  // 10 joints + fixed joint for head
static dJointID lmotor[2];
static dJointID amotor[2];
static dJointID head_ball;
static dJointID head_motor;

static dJointID hook;

// Collision call back function here
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
}

// start simulation - set viewpoint
static void start()
{
  static float xyz[3] = {0.8317f,-0.9817f,4.8000f};
  static float hpr[3] = {121.0000f,-27.5000f,0.0000f};
  dsSetViewpoint (xyz,hpr);
}


// called when a key pressed
static void command (int cmd)
{
}

// run Simulation
static void simLoop(int pause)
{
  static int itime = 0;
  static int vsign = 1;
	dSpaceCollide (space,0,&nearCallback);
	dWorldStep (world,0.005);
	// remove all contact joints
	dJointGroupEmpty (contactgroup);

	dsSetColor (0,1,1);
	dsSetTexture (DS_WOOD); // DS_WOOD
  
	// draw components
	dsDrawSphere(dBodyGetPosition(head),dBodyGetRotation(head),HEAD);
	dsSetColor (1,0,1);
	
dsDrawBox(dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),TORSO);

	/*
	if ( (itime++) % 40  == 0 ) {
	  vsign *= -1;
	  dJointSetAMotorParam(head_motor,dParamVel3,vsign*10);
	  printf("Change!\n");
	}
	*/


}

static void init(void) 
{
	dMass m;

	for (int i=0;i<11;i++) 
		body[i] = dBodyCreate(world);
	head = dBodyCreate(world);	
	dBodySetPosition (head,0,0,STARTZ+TORSO[2]/2+HEAD);
	dBodySetPosition (body[0],0,0,STARTZ);	// torso

	// head
  dMassSetSphere(&m,1,HEAD);
  dBodySetMass(head, &m);

  // torso
  dMassSetBox (&m,1,TORSO[0],TORSO[1],TORSO[2]);	// torso
  dMassAdjust (&m,1);	// torso weight
  dBodySetMass (body[0],&m);
  box[0] = dCreateBox (space,TORSO[0],TORSO[1],TORSO[2]);
  dGeomSetBody (box[0],body[0]);

	// Create Joints

	// hook
	hook = dJointCreateFixed (world, 0);
	dJointAttach (hook, 0, head);
	dJointSetFixed (hook);

	// head joint
	head_ball = dJointCreateBall(world,0);
	dJointAttach(head_ball,body[0],head);
	dJointSetBallAnchor(head_ball,0,0,STARTZ+TORSO[2]/2);
	head_motor = dJointCreateAMotor(world,0);
	dJointAttach(head_motor,body[0],head);

	const float stop = Pi/2;
	//	const float stop_cfm = 0.01;
	//	const float stop_erp = 0.8;
	const float stop_cfm = 0.05;
	const float stop_erp = 0.2;
	dJointSetAMotorMode(head_motor,dAMotorEuler);
	dJointSetAMotorAxis(head_motor,0,1,0,0,1);
	dJointSetAMotorAxis(head_motor,2,2,0,1,0);
	dJointSetAMotorParam(head_motor,dParamLoStop,-stop);  // twist
	dJointSetAMotorParam(head_motor,dParamHiStop,stop);
	dJointSetAMotorParam(head_motor,dParamVel,0);
	dJointSetAMotorParam(head_motor,dParamFMax,0);
	dJointSetAMotorParam(head_motor,dParamLoStop2,-stop);  // back
	dJointSetAMotorParam(head_motor,dParamHiStop2,stop);   // front
	dJointSetAMotorParam(head_motor,dParamVel2,0);
	dJointSetAMotorParam(head_motor,dParamFMax2,0);
	dJointSetAMotorParam(head_motor,dParamLoStop3,-stop);  // back
	dJointSetAMotorParam(head_motor,dParamHiStop3,stop);   // front
	dJointSetAMotorParam(head_motor,dParamVel3,1);
	dJointSetAMotorParam(head_motor,dParamFMax3,1000);
	dJointSetAMotorParam(head_motor,dParamStopERP,stop_erp);
	dJointSetAMotorParam(head_motor,dParamStopCFM,stop_cfm);
	dJointSetAMotorParam(head_motor,dParamStopERP2,stop_erp);
	dJointSetAMotorParam(head_motor,dParamStopCFM2,stop_cfm);
	dJointSetAMotorParam(head_motor,dParamStopERP3,stop_erp);
	dJointSetAMotorParam(head_motor,dParamStopCFM3,stop_cfm); 
	dJointSetAMotorParam(head_motor,dParamBounce,0); 
	dJointSetAMotorParam(head_motor,dParamBounce2,0); 
	dJointSetAMotorParam(head_motor,dParamBounce3,0); 

}

// main program
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 = "../../drawstuff/textures";
	
	// create world (setup environment)
	world = dWorldCreate();		// create ODE world
	space = dHashSpaceCreate();
    contactgroup = dJointGroupCreate (0);
	dWorldSetGravity (world,0,0,-1.50);	// set gravity (x,y,z)
	dWorldSetERP (world,ERP);			// global ERP
	dWorldSetCFM (world,CFM);			// global CFM
	ground = dCreatePlane(space,0,0,1,0);
	init();

	// Start Simulation
	dsSimulationLoop (argc,argv,640,480,&fn);	// 640 x 480 OpenGL
display

	// Destroy World (end program)	
    dJointGroupDestroy (contactgroup);
	dSpaceDestroy (space);
	dWorldDestroy(world);
	return 0;
}