[ODE] AMotor axes from object space

[Jason Mallios]

I'm animating a skeleton from motion capture (MOCAP) data that gives me a
joint position and direction vectors for the two bones it connects in an
initial pose.  The initial pose can be thought of as angles of zero
radians about three joint-space axes.

My skeleton file also defines a local coordinate system for each bone with
Y pointing toward the joint connecting the bone to its parent.  So, of
two connected bones, one (the parent) has a +Y pointing away from the
joint and one (the child) has a +Y pointing to the joint.  The file also
defines stops along the local coordinate axes for every child's joint with
its parent.

I've set ball and sockets for each joint in the skeleton and I want to set
AMotors to the two bodies of each ball and socket joint.  I think I want
to do the following, but the results are either unstable or inaccurate.

// b1 = parent dBodyID, b2 = child dBodyID
// x_axis = child's local x
// y_axis = child's local y
// z_axis = child's local z
// anchor = joint position, where child and parent meet

// BALL JOINT
dJointID ball_joint = dJointCreateBall(world,0);
dJointAttach(ball_joint,b1,b2);
dJointSetBallAnchor(ball_joint,anchor[0], anchor[1], anchor[2]);

// MOTOR
dJointID motor = dJointCreateAMotor(world,0);
dJointAttach(motor,b1,b2);
dJointSetAMotorNumAxes (motor, 3);
dJointSetAMotorMode (motor, dAMotorUser);

// MOTOR AXES BASED ON ODE MANUAL 7.3.8, ALL SET TO CHILD BODY
dJointSetAMotorAxis (motor, 0, 2, z_axis[0], z_axis[1], z_axis[2]);
dJointSetAMotorAxis (motor, 1, 2, x_axis[0], x_axis[1], x_axis[2]);
dJointSetAMotorAxis (motor, 2, 2, y_axis[0], y_axis[1], y_axis[2]);

// INITIAL MOTOR ANGLES
dJointSetAMotorAngle(j_ids.last(), 0, 0);
dJointSetAMotorAngle(j_ids.last(), 1, 0);
dJointSetAMotorAngle(j_ids.last(), 2, 0);

// STOPS
// x_lo = child's low limit along it's local x in radians
// x_hi = child's high limit along it's local x in radians
// y_lo = child's low limit along it's local y in radians
// y_hi = child's high limit along it's local y in radians
// z_lo = child's low limit along it's local z in radians
// z_hi = child's high limit along it's local z in radians
dJointSetAMotorParam(j_ids.last(), dParamLoStop, z_lo);
dJointSetAMotorParam(j_ids.last(), dParamHiStop, z_hi);
dJointSetAMotorParam(j_ids.last(), dParamVel, 0);
dJointSetAMotorParam(j_ids.last(), dParamFMax, 0.1);
dJointSetAMotorParam(j_ids.last(), dParamLoStop2, x_lo);
dJointSetAMotorParam(j_ids.last(), dParamHiStop2, x_hi);
dJointSetAMotorParam(j_ids.last(), dParamVel2, 0);
dJointSetAMotorParam(j_ids.last(), dParamFMax2, 0.1);
dJointSetAMotorParam(j_ids.last(), dParamLoStop3, y_lo);
dJointSetAMotorParam(j_ids.last(), dParamHiStop3, y_hi);
dJointSetAMotorParam(j_ids.last(), dParamVel3, 0);
dJointSetAMotorParam(j_ids.last(), dParamFMax3, 0.1);

Any ideas why this wouldn't work as expected?  Am I attaching the axes to
the correct bodies?  It was unclear from the documentation if axes were
required to be set to both bodies.  I found dAMotorEuler to be even less
predictable.

Thanks,
Jason Mallios