FW: [ODE] Joint feedback in quickstep

[Jaroslav Sinecky]

I sent a post with quickstep.cpp attached to the list yesterday but it
didn't make it through because it's too big and is waiting for moderator
approval.  Since I don't know if this would happen, I did the most simple
thing ... put the file on the following link ...

http://home.tiscali.cz/pusher/quickstep.cpp

(I agree, probably much better than sending al the KBytes to every single
user on the list)

read bellow ...

> -----Original Message-----
> From: Jaroslav Sinecky [mailto:jsinecky at tiscali.cz]
> Sent: Wednesday, June 08, 2005 6:52 PM
> To: ode at q12.org
> Subject: FW: [ODE] Joint feedback in quickstep
>
>
> Well, as noone called me mad so far and being unpatient I went
> and really dig in the quickstep code. Misteriously it seems to
> work now as I would expect that is I get more reasonable
> constraint forces in the feedbacks and the simulation runs more o
> less similar as with dWorldStep. (Apart of bit more unstable
> behaviour which is not surprising especialy if I have couple of
> kinematic loops in there).
>
> I attach modified quickstep.cpp so it would be nice, in case you
> don't find completly mistaken what I'm doing, if someone more
> familiar with ode code could have look at it to see if it seems
> correct and then suggest what to do next ... like create some
> patch or stuff. (Sorry, I got bit lost in the discussion about
> patches and unstable branch)
>
> Cheers!
> Jaroslav
>
> > -----Original Message-----
> > From: ode-bounces at q12.org [mailto:ode-bounces at q12.org]On Behalf
> > Of Jaroslav Sinecky
> > Sent: Wednesday, June 08, 2005 8:43 AM
> > To: ode at q12.org
> > Subject: [ODE] Joint feedback in quickstep
> >
> >
> > 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));
> > 				}
> > 			}
> > 		}
> > 	}
> > }