[ODE] Initial position

[Don Burns]

HI Martin,

I'm using doubles.

On Wed, 21 Apr 2004, Martin C. Martin wrote:

> Don Burns wrote:
> 
> > My previous question regarding using joints with composite geometry seems 
> > to have stumped the jury,
> 
> Quite the opposite: the problem is most likely in your nearCallback, 
> which someone asked you to post to the list, but you never did.  That 
> the people on this list are reluctant to spend their spare time surfing 
> to your site and digging through your code for the one function they 
> asked for, hardly counts as "stumpping the jury."
>

My bad.  Here:

///////////////////////////////////////////////////////////////////////

static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  int i,n;
                                                                                                                            
  // only collide things with the ground
  int g1 = (o1 == ground || o1 == ground_box);
  int g2 = (o2 == ground || o2 == ground_box);
  if (!(g1 ^ g2)) return;
                                                                                                                            
  const int N = 10;
  dContact contact[N];
  n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
  if (n > 0) {
    for (i=0; i<n; i++) {
      contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
    dContactSoftERP | dContactSoftCFM | dContactApprox1;
      contact[i].surface.mu = dInfinity;
      contact[i].surface.slip1 = 0.1;
      contact[i].surface.slip2 = 0.1;
      //contact[i].surface.soft_erp = 0.5;
      //contact[i].surface.soft_cfm = 0.3;
      contact[i].surface.soft_erp = 0.0001;
      contact[i].surface.soft_cfm = 0.0001;
      dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
      dJointAttach (c,
            dGeomGetBody(contact[i].geom.g1),
            dGeomGetBody(contact[i].geom.g2));
    }
  }
}
///////////////////////////////////////////////////////////////////////

... which is mostly cut-n-paste from test_buggy.cpp, an example that works
well, which is why I didn't suspect it as being the problem.  If you can 
show me what is wrong with this code I'd be very appreciative.
 
> > When I'm using a simple world (single big plane) and placing my model at
> > 0,0,0, things behave nicely.  When I take my model and put it into a real
> > world (position is now 16068.0, 7820.0, 25.5, with heading 160.0) the
> > model flings itself into wild gyrations and produces NANs after about 20
> > frames.
> > 
> > Is there a trick to initializing (or re-initializing) the position and 
> > attitude of a rigid body before doing a dWorldStep() ?
> 
> Did you make sure to translate all bodies and joints by the same amount? 
>   Also, an orientation is either three euler angles (in radians), the 
> four numbers of a quaternion, or a 3x3 matrix.  What do you mean by 
> "with heading of 160.0"?
>

Sorry, I'm using terminology common to flight sims.  The eulers are pitch,
roll and heading. "with heading of 160"  means a heading of 160 degrees.  
I'm orienting with a dMatrix3, which, of course, uses direction cosines.

Since posting I realized that I was not translating or orienting all
bodies associated with the object, so I've fixed that such that all bodies 
are translated and oriented.  How do I translate and orient joints?

The behavior is much better now, and works nicely if I leave the 
orientation with the identity matrix.  However, any other orientation 
still seems to spin the model crazily upon reset.

It may be interesting to note that I had this all working well previously 
when I was not using joints.

Thanks,

-don