Example #1
0
//! This function is called once per simulation step, allowing the
//! constraint to be "motorized" according to some response.
void OscUniversalODE::simulationCallback()
{
    ODEConstraint& me = *static_cast<ODEConstraint*>(special());

    dReal angle1 = dJointGetUniversalAngle1(me.joint());
    dReal rate1 = dJointGetUniversalAngle1Rate(me.joint());

    dReal addtorque1 =
        - m_response->m_stiffness.m_value*angle1
        - m_response->m_damping.m_value*rate1;

    dReal angle2 = dJointGetUniversalAngle2(me.joint());
    dReal rate2 = dJointGetUniversalAngle2Rate(me.joint());

    dReal addtorque2 =
        - m_response->m_stiffness.m_value*angle2
        - m_response->m_damping.m_value*rate2;

    m_torque1.m_value = addtorque1;
    m_torque2.m_value = addtorque2;

    dJointAddUniversalTorques(me.joint(), addtorque1, addtorque2);
}
dReal doStuffAndGetError (int n)
{
  switch (n) {

  // ********** fixed joint

  case 0: {			// 2 body
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    // check the orientations are the same
    const dReal *R1 = dBodyGetRotation (body[0]);
    const dReal *R2 = dBodyGetRotation (body[1]);
    dReal err1 = dMaxDifference (R1,R2,3,3);
    // check the body offset is correct
    dVector3 p,pp;
    const dReal *p1 = dBodyGetPosition (body[0]);
    const dReal *p2 = dBodyGetPosition (body[1]);
    for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
    dMULTIPLY1_331 (pp,R1,p);
    pp[0] += 0.5;
    pp[1] += 0.5;
    return (err1 + length (pp)) * 300;
  }

  case 1: {			// 1 body to static env
    addOscillatingTorque (0.1);

    // check the orientation is the identity
    dReal err1 = cmpIdentity (dBodyGetRotation (body[0]));

    // check the body offset is correct
    dVector3 p;
    const dReal *p1 = dBodyGetPosition (body[0]);
    for (int i=0; i<3; i++) p[i] = p1[i];
    p[0] -= 0.25;
    p[1] -= 0.25;
    p[2] -= 1;
    return (err1 + length (p)) * 1e6;
  }

  case 2: {			// 2 body
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    // check the body offset is correct
    // Should really check body rotation too.  Oh well.
    const dReal *R1 = dBodyGetRotation (body[0]);
    dVector3 p,pp;
    const dReal *p1 = dBodyGetPosition (body[0]);
    const dReal *p2 = dBodyGetPosition (body[1]);
    for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
    dMULTIPLY1_331 (pp,R1,p);
    pp[0] += 0.5;
    pp[1] += 0.5;
    return length(pp) * 300;
  }

  case 3: {			// 1 body to static env with relative rotation
    addOscillatingTorque (0.1);

    // check the body offset is correct
    dVector3 p;
    const dReal *p1 = dBodyGetPosition (body[0]);
    for (int i=0; i<3; i++) p[i] = p1[i];
    p[0] -= 0.25;
    p[1] -= 0.25;
    p[2] -= 1;
    return  length (p) * 1e6;
  }


  // ********** hinge joint

  case 200:			// 2 body
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    return dInfinity;

  case 220:			// hinge angle polarity test
    dBodyAddTorque (body[0],0,0,0.01);
    dBodyAddTorque (body[1],0,0,-0.01);
    if (iteration == 40) {
      dReal a = dJointGetHingeAngle (joint);
      if (a > 0.5 && a < 1) return 0; else return 10;
    }
    return 0;

  case 221: {			// hinge angle rate test
    static dReal last_angle = 0;
    dBodyAddTorque (body[0],0,0,0.01);
    dBodyAddTorque (body[1],0,0,-0.01);
    dReal a = dJointGetHingeAngle (joint);
    dReal r = dJointGetHingeAngleRate (joint);
    dReal er = (a-last_angle)/STEPSIZE;		// estimated rate
    last_angle = a;
    return fabs(r-er) * 4e4;
  }

  case 230:			// hinge motor rate (and polarity) test
  case 231: {			// ...with stops
    static dReal a = 0;
    dReal r = dJointGetHingeAngleRate (joint);
    dReal err = fabs (cos(a) - r);
    if (a==0) err = 0;
    a += 0.03;
    dJointSetHingeParam (joint,dParamVel,cos(a));
    if (n==231) return dInfinity;
    return err * 1e6;
  }

  // ********** slider joint

  case 300:			// 2 body
    addOscillatingTorque (0.05);
    dampRotationalMotion (0.1);
    addSpringForce (0.5);
    return dInfinity;

  case 320:			// slider angle polarity test
    dBodyAddForce (body[0],0,0,0.1);
    dBodyAddForce (body[1],0,0,-0.1);
    if (iteration == 40) {
      dReal a = dJointGetSliderPosition (joint);
      if (a > 0.2 && a < 0.5) return 0; else return 10;
      return a;
    }
    return 0;

  case 321: {			// slider angle rate test
    static dReal last_pos = 0;
    dBodyAddForce (body[0],0,0,0.1);
    dBodyAddForce (body[1],0,0,-0.1);
    dReal p = dJointGetSliderPosition (joint);
    dReal r = dJointGetSliderPositionRate (joint);
    dReal er = (p-last_pos)/STEPSIZE;	// estimated rate (almost exact)
    last_pos = p;
    return fabs(r-er) * 1e9;
  }

  case 330:			// slider motor rate (and polarity) test
  case 331: {			// ...with stops
    static dReal a = 0;
    dReal r = dJointGetSliderPositionRate (joint);
    dReal err = fabs (0.7*cos(a) - r);
    if (a < 0.04) err = 0;
    a += 0.03;
    dJointSetSliderParam (joint,dParamVel,0.7*cos(a));
    if (n==331) return dInfinity;
    return err * 1e6;
  }

  // ********** hinge-2 joint

  case 420:			// hinge-2 steering angle polarity test
    dBodyAddTorque (body[0],0,0,0.01);
    dBodyAddTorque (body[1],0,0,-0.01);
    if (iteration == 40) {
      dReal a = dJointGetHinge2Angle1 (joint);
      if (a > 0.5 && a < 0.6) return 0; else return 10;
    }
    return 0;

  case 421: {			// hinge-2 steering angle rate test
    static dReal last_angle = 0;
    dBodyAddTorque (body[0],0,0,0.01);
    dBodyAddTorque (body[1],0,0,-0.01);
    dReal a = dJointGetHinge2Angle1 (joint);
    dReal r = dJointGetHinge2Angle1Rate (joint);
    dReal er = (a-last_angle)/STEPSIZE;		// estimated rate
    last_angle = a;
    return fabs(r-er)*2e4;
  }

  case 430:			// hinge 2 steering motor rate (+polarity) test
  case 431: {			// ...with stops
    static dReal a = 0;
    dReal r = dJointGetHinge2Angle1Rate (joint);
    dReal err = fabs (cos(a) - r);
    if (a==0) err = 0;
    a += 0.03;
    dJointSetHinge2Param (joint,dParamVel,cos(a));
    if (n==431) return dInfinity;
    return err * 1e6;
  }

  case 432: {			// hinge 2 wheel motor rate (+polarity) test
    static dReal a = 0;
    dReal r = dJointGetHinge2Angle2Rate (joint);
    dReal err = fabs (cos(a) - r);
    if (a==0) err = 0;
    a += 0.03;
    dJointSetHinge2Param (joint,dParamVel2,cos(a));
    return err * 1e6;
  }

  // ********** angular motor joint

  case 600: {			// test euler angle calculations
    // desired euler angles from last iteration
    static dReal a1,a2,a3;

    // find actual euler angles
    dReal aa1 = dJointGetAMotorAngle (joint,0);
    dReal aa2 = dJointGetAMotorAngle (joint,1);
    dReal aa3 = dJointGetAMotorAngle (joint,2);
    // printf ("actual  = %.4f %.4f %.4f\n\n",aa1,aa2,aa3);

    dReal err = dInfinity;
    if (iteration > 0) {
      err = dFabs(aa1-a1) + dFabs(aa2-a2) + dFabs(aa3-a3);
      err *= 1e10;
    }

    // get random base rotation for both bodies
    dMatrix3 Rbase;
    dRFromAxisAndAngle (Rbase, 3*(dRandReal()-0.5), 3*(dRandReal()-0.5),
			3*(dRandReal()-0.5), 3*(dRandReal()-0.5));
    dBodySetRotation (body[0],Rbase);

    // rotate body 2 by random euler angles w.r.t. body 1
    a1 = 3.14 * 2 * (dRandReal()-0.5);
    a2 = 1.57 * 2 * (dRandReal()-0.5);
    a3 = 3.14 * 2 * (dRandReal()-0.5);
    dMatrix3 R1,R2,R3,Rtmp1,Rtmp2;
    dRFromAxisAndAngle (R1,0,0,1,-a1);
    dRFromAxisAndAngle (R2,0,1,0,a2);
    dRFromAxisAndAngle (R3,1,0,0,-a3);
    dMultiply0 (Rtmp1,R2,R3,3,3,3);
    dMultiply0 (Rtmp2,R1,Rtmp1,3,3,3);
    dMultiply0 (Rtmp1,Rbase,Rtmp2,3,3,3);
    dBodySetRotation (body[1],Rtmp1);
    // printf ("desired = %.4f %.4f %.4f\n",a1,a2,a3);

    return err;
  }

  // ********** universal joint

  case 700: {		// 2 body: joint constraint
    dVector3 ax1, ax2;

    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    dJointGetUniversalAxis1(joint, ax1);
    dJointGetUniversalAxis2(joint, ax2);
    return fabs(10*dDOT(ax1, ax2));
  }

  case 701: {		// 2 body: angle 1 rate
    static dReal last_angle = 0;
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle1(joint);
    dReal r = dJointGetUniversalAngle1Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    // I'm not sure why the error is so large here.
    return fabs(r - er) * 1e1;
  }

  case 702: {		// 2 body: angle 2 rate
    static dReal last_angle = 0;
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle2(joint);
    dReal r = dJointGetUniversalAngle2Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    // I'm not sure why the error is so large here.
    return fabs(r - er) * 1e1;
  }

  case 720: {		// universal transmit torque test: constraint error
    dVector3 ax1, ax2;
    addOscillatingTorqueAbout (0.1, 1, 1, 0);
    dampRotationalMotion (0.1);
    dJointGetUniversalAxis1(joint, ax1);
    dJointGetUniversalAxis2(joint, ax2);
    return fabs(10*dDOT(ax1, ax2));
  }

  case 721: {		// universal transmit torque test: angle1 rate
    static dReal last_angle = 0;
    addOscillatingTorqueAbout (0.1, 1, 1, 0);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle1(joint);
    dReal r = dJointGetUniversalAngle1Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e10;
  }

  case 722: {		// universal transmit torque test: angle2 rate
    static dReal last_angle = 0;
    addOscillatingTorqueAbout (0.1, 1, 1, 0);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle2(joint);
    dReal r = dJointGetUniversalAngle2Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e10;
  }

  case 730:{
    dVector3 ax1, ax2;
    dJointGetUniversalAxis1(joint, ax1);
    dJointGetUniversalAxis2(joint, ax2);
    addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
    dampRotationalMotion (0.1);
    return fabs(10*dDOT(ax1, ax2));
  }

  case 731:{
    dVector3 ax1;
    static dReal last_angle = 0;
    dJointGetUniversalAxis1(joint, ax1);
    addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle1(joint);
    dReal r = dJointGetUniversalAngle1Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 2e3;
  }

  case 732:{
    dVector3 ax1;
    static dReal last_angle = 0;
    dJointGetUniversalAxis1(joint, ax1);
    addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle2(joint);
    dReal r = dJointGetUniversalAngle2Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e10;
  }

  case 740:{
    dVector3 ax1, ax2;
    dJointGetUniversalAxis1(joint, ax1);
    dJointGetUniversalAxis2(joint, ax2);
    addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
    dampRotationalMotion (0.1);
    return fabs(10*dDOT(ax1, ax2));
  }

  case 741:{
    dVector3 ax2;
    static dReal last_angle = 0;
    dJointGetUniversalAxis2(joint, ax2);
    addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle1(joint);
    dReal r = dJointGetUniversalAngle1Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e10;
  }

  case 742:{
    dVector3 ax2;
    static dReal last_angle = 0;
    dJointGetUniversalAxis2(joint, ax2);
    addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle2(joint);
    dReal r = dJointGetUniversalAngle2Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e4;
  }
  }

  return dInfinity;
}