示例#1
0
void UniversalJoint::computeValue(double& value, int portId)
{
  if(portId == AXIS1_JOINT)
  {
    value = (dJointGetUniversalAngle1(physicalJoint) * 180.0 / M_PI);
  }
  else if(portId == AXIS1_MOTOR)
  {
    if(axis1->motor->getMotorType() == VELOCITY)
      value = (dJointGetUniversalParam(physicalJoint, dParamVel));
    else
      value = (dJointGetUniversalAngle1(physicalJoint) * 180.0 / M_PI);
  }
  else if(portId == AXIS2_JOINT)
  {
    value = (dJointGetUniversalAngle2(physicalJoint) * 180.0 / M_PI);
  }
  else if(portId == AXIS2_MOTOR)
  {
    if(axis2->motor->getMotorType() == VELOCITY)
      value = (dJointGetUniversalParam(physicalJoint, dParamVel2));
    else
      value = (dJointGetUniversalAngle2(physicalJoint) * 180.0 / M_PI);
  }
}
示例#2
0
文件: Universal.c 项目: RONNCC/pysoy
gfloat* soy_joints_universal_get_angle (soyjointsUniversal* self, int* result_length1) {
	gfloat* result;
	struct dxJoint* _tmp0_;
	dReal _tmp1_ = 0.0;
	struct dxJoint* _tmp2_;
	dReal _tmp3_ = 0.0;
	gfloat* _tmp4_ = NULL;
	gfloat* _tmp5_;
	gint _tmp5__length1;
	g_return_val_if_fail (self != NULL, NULL);
	_tmp0_ = ((soyjointsJoint*) self)->joint;
	_tmp1_ = dJointGetUniversalAngle1 ((struct dxJoint*) _tmp0_);
	_tmp2_ = ((soyjointsJoint*) self)->joint;
	_tmp3_ = dJointGetUniversalAngle2 ((struct dxJoint*) _tmp2_);
	_tmp4_ = g_new0 (gfloat, 2);
	_tmp4_[0] = (gfloat) _tmp1_;
	_tmp4_[1] = (gfloat) _tmp3_;
	_tmp5_ = _tmp4_;
	_tmp5__length1 = 2;
	if (result_length1) {
		*result_length1 = _tmp5__length1;
	}
	result = _tmp5_;
	return result;
}
示例#3
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);
}
示例#4
0
void UniversalJoint::act(bool initial)
{
  if(!initial)
  {
    if(axis1->motor)
    {
      if(axis1->motor->getMotorType() == VELOCITY)
      {
        dReal desiredVelocity(dReal(axis1->motor->getDesiredVelocity()));
        dJointSetUniversalParam(physicalJoint, dParamFMax, dReal(axis1->motor->getMaxForce()));
        dJointSetUniversalParam(physicalJoint, dParamVel, desiredVelocity);
      }
      else if(axis1->motor->getMotorType() == ANGULAR)
      {
        double controllerOutput = axis1->motor->getControllerOutput(dJointGetUniversalAngle1(physicalJoint));
        dJointSetUniversalParam(physicalJoint, dParamFMax, dReal(axis1->motor->getMaxForce()));
        dJointSetUniversalParam(physicalJoint, dParamVel, dReal(controllerOutput));
      }
    }

    if(axis2->motor)
    {
      if(axis2->motor->getMotorType() == VELOCITY)
      {
        dReal desiredVelocity(dReal(axis2->motor->getDesiredVelocity()));
        dJointSetUniversalParam(physicalJoint, dParamFMax2, dReal(axis2->motor->getMaxForce()));
        dJointSetUniversalParam(physicalJoint, dParamVel2, desiredVelocity);
      }
      else if(axis2->motor->getMotorType() == ANGULAR)
      {
        double controllerOutput = axis2->motor->getControllerOutput(dJointGetUniversalAngle2(physicalJoint));
        dJointSetUniversalParam(physicalJoint, dParamFMax2, dReal(axis2->motor->getMaxForce()));
        dJointSetUniversalParam(physicalJoint, dParamVel2, dReal(controllerOutput));
      }
    }
  }
}
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;
}