[ODE] Joint feedback in quickstep

[Jaroslav Sinecky]

Hello all!

I see myself obliged to change to QuickStep to keep my simulation
interactive, but the joint feedback forces that I get now are no longer what
expected (it was ok with dWorldStep).
After some meditating over ODE concept equations (once again) and studying
the code I really think there is a problem. It seems to me I actually get
feedback forces for each body but summed from all joints that attaches the
body instead of just from one concrete joint.

Is it possible? Noone had problems using joint feedback with quickstep so
far?

I suppose it should be possible to get correct constraint force (same as in
step or stepfast code) by multiplying the part of lambda vector related to
the joint in question with respective block of J' matrix. But before I start
digging in the code I wanted to ask here, because maybe there's something
I'm doing/thinking wrong.

Thanks,
Jaroslav


ps. I'm refering mainly to this part of quickstep.cpp:

// if joint feedback is requested, compute the constraint force.
// BUT: cforce is inv(M)*J'*lambda, whereas we want just J'*lambda,
// so we must compute M*cforce.
// @@@ if any joint has a feedback request we compute the entire
//     adjusted cforce, which is not the most efficient way to do it.
for (j=0; j<nj; j++) {
	if (joint[j]->feedback) {
		// compute adjusted cforce
		for (i=0; i<nb; i++) {
			dReal k = body[i]->mass.mass;
			cforce [i*6+0] *= k;
			cforce [i*6+1] *= k;
			cforce [i*6+2] *= k;
			dVector3 tmp;
			dMULTIPLY0_331 (tmp, I + 12*i, cforce + i*6 + 3);
			cforce [i*6+3] = tmp[0];
			cforce [i*6+4] = tmp[1];
			cforce [i*6+5] = tmp[2];
		}
		// compute feedback for this and all remaining joints
		for (; j<nj; j++) {
			dJointFeedback *fb = joint[j]->feedback;
			if (fb) {
				int b1 = joint[j]->node[0].body->tag;
				memcpy (fb->f1,cforce+b1*6,3*sizeof(dReal));
				memcpy (fb->t1,cforce+b1*6+3,3*sizeof(dReal));
				if (joint[j]->node[1].body) {
					int b2 = joint[j]->node[1].body->tag;
					memcpy (fb->f2,cforce+b2*6,3*sizeof(dReal));
					memcpy (fb->t2,cforce+b2*6+3,3*sizeof(dReal));
				}
			}
		}
	}
}