[ODE] Problem with dJointGetHingeAngleRate in main simulation loop?

[Dave Ellison]

Hi, 

The second time round the main simulation loop, dJointGetHingeAngleRate seems 
to take a sudden jump in value. I am trying out code for a simple pendulum, of 
length l, with l = g = 9.81, with theta0 = 0.1 and thetadot = 1.0 (theta 
measured from the vertical). The pivot is at height 12 above the plane. The 
period is thus 2Pi and the amplitude about 1.5 radians.

When I run this, the output is:

0.00sec:  angle= 0.100  angledot= 1.000
0.01sec:  angle= 0.100  angledot= 0.001

^< sudden jump here> ^

0.02sec:  angle= 0.100  angledot= 0.002
0.03sec:  angle= 0.100  angledot= 0.003
0.04sec:  angle= 0.100  angledot= 0.004


I have checked the value  before entering the loop and it is as I expect.

Relevant code is:

static void simLoop (int pause)
{
	double h = 0.01; // integration steps
	double elapsedT = dsElapsedTime();
	int nrofsteps = (int) ceilf(elapsedT/h);
	for (int i=0; i<nrofsteps && !pause; i++){

		const dReal angle = dJointGetHingeAngle(pivot);
		const dReal angledot = dJointGetHingeAngleRate(pivot);

		if (nrofsteps%1 == 0)
		{	
			printf( "%1.2fsec:  angle=%6.3f  angledot=%6.3f  \n",
				totalTime,   angle + th0, angledot); 
		}

		dWorldStep (world, h);
		totalTime += h;
	}
	// Do drawing

	// Get Body Positions

	const dReal *BPos1 = dBodyGetPosition(pedulumBob);
	float x1 = BPos1[0]; float y1 = BPos1[1]; float z1 = BPos1[2];

	const dReal *SRot1 = dBodyGetRotation(pedulumBob);
	float posPend[3] = {0.0, 0.0, pivotH};

	dsDrawSphere(BPos1, SRot1, 0.05);

	dsDrawLine(posPend, BPos1);

with set up, in main:

	world = dWorldCreate();

	dWorldSetGravity (world,0,0,-g);

	//  Create pedulumBob
	pedulumBob = dBodyCreate (world);

	//Position of pendulum bob
	dBodySetPosition (pedulumBob, l * sin(th0), 0.0, pivotH - l * cos
(th0));
	dBodySetAngularVel(pedulumBob, 0.0, thDot0, 0.0);

	const dReal *pos = dBodyGetPosition(pedulumBob);
	printf( "x=%6.3f y=%6.3f z=%6.3f  \n", pos[0], pos[1], pos[2]); 

	dMassSetSphere (&m, 2500.0, 0.05);
	dMassAdjust (&m,1);
	dBodySetMass (pedulumBob,&m);

	pivot = dJointCreateHinge (world, 0);
	dJointAttach(pivot, pedulumBob, 0);

	dJointSetHingeAnchor (pivot, 0, 0, pivotH);
	dJointSetHingeAxis (pivot, 0, 1, 0);

	const dReal angle = dJointGetHingeAngle(pivot);
	const dReal angledot = dJointGetHingeAngleRate(pivot);

	printf( "angle=%6.3f  angledot=%6.3f  \n",
		angle + th0, angledot); 
	// run simulation
	dsSimulationLoop (argc,argv,352,288,&fn);
	//   dsSimulationLoop (1,-pause,352,288,&fn);

	dWorldDestroy (world);

	return 0;
}


Am I doing something wrong or is there a bug?

Dave