@ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout = 30000)
  public void testSingleRigidBodyRotation() {
    Random random = new Random(1766L);

    RigidBody elevator = new RigidBody("elevator", world);
    Vector3d jointAxis = RandomTools.generateRandomVector(random);
    jointAxis.normalize();
    RigidBodyTransform transformToParent = new RigidBodyTransform();
    transformToParent.setIdentity();
    RevoluteJoint joint =
        ScrewTools.addRevoluteJoint("joint", elevator, transformToParent, jointAxis);
    RigidBody body =
        ScrewTools.addRigidBody(
            "body",
            joint,
            RandomTools.generateRandomDiagonalMatrix3d(random),
            random.nextDouble(),
            new Vector3d());

    joint.setQ(random.nextDouble());
    joint.setQd(random.nextDouble());

    Momentum momentum = computeMomentum(elevator, world);

    momentum.changeFrame(world);
    FrameVector linearMomentum =
        new FrameVector(momentum.getExpressedInFrame(), momentum.getLinearPartCopy());
    FrameVector angularMomentum =
        new FrameVector(momentum.getExpressedInFrame(), momentum.getAngularPartCopy());

    FrameVector linearMomentumCheck = new FrameVector(world);
    Matrix3d inertia = body.getInertia().getMassMomentOfInertiaPartCopy();
    Vector3d angularMomentumCheckVector = new Vector3d(jointAxis);
    angularMomentumCheckVector.scale(joint.getQd());
    inertia.transform(angularMomentumCheckVector);
    FrameVector angularMomentumCheck =
        new FrameVector(body.getInertia().getExpressedInFrame(), angularMomentumCheckVector);
    angularMomentumCheck.changeFrame(world);

    double epsilon = 1e-9;
    JUnitTools.assertTuple3dEquals(
        linearMomentumCheck.getVector(), linearMomentum.getVector(), epsilon);
    JUnitTools.assertTuple3dEquals(
        angularMomentumCheck.getVector(), angularMomentum.getVector(), epsilon);
    assertTrue(angularMomentum.length() > epsilon);
  }
  private void computeDerivativeTerm(
      FrameVector desiredAngularVelocity, FrameVector currentAngularVelocity) {
    desiredAngularVelocity.changeFrame(bodyFrame);
    currentAngularVelocity.changeFrame(bodyFrame);

    derivativeTerm.sub(desiredAngularVelocity, currentAngularVelocity);

    // Limit the maximum velocity error considered for control action
    double maximumVelocityError = gains.getMaximumDerivativeError();
    double velocityErrorMagnitude = derivativeTerm.length();
    if (velocityErrorMagnitude > maximumVelocityError) {
      derivativeTerm.scale(maximumVelocityError / velocityErrorMagnitude);
    }

    velocityError.set(derivativeTerm);
    derivativeGainMatrix.transform(derivativeTerm.getVector());
  }
  private void computeProportionalTerm(FrameOrientation desiredOrientation) {
    desiredOrientation.changeFrame(bodyFrame);
    desiredOrientation.getQuaternion(errorQuaternion);
    errorAngleAxis.set(errorQuaternion);
    errorAngleAxis.setAngle(AngleTools.trimAngleMinusPiToPi(errorAngleAxis.getAngle()));

    // Limit the maximum position error considered for control action
    double maximumError = gains.getMaximumProportionalError();
    if (errorAngleAxis.getAngle() > maximumError) {
      errorAngleAxis.setAngle(Math.signum(errorAngleAxis.getAngle()) * maximumError);
    }

    proportionalTerm.set(errorAngleAxis.getX(), errorAngleAxis.getY(), errorAngleAxis.getZ());
    proportionalTerm.scale(errorAngleAxis.getAngle());
    rotationErrorInBody.set(proportionalTerm);

    proportionalGainMatrix.transform(proportionalTerm.getVector());
  }
  private void computeIntegralTerm() {
    if (gains.getMaximumIntegralError() < 1e-5) {
      integralTerm.setToZero(bodyFrame);
      return;
    }

    double integratedErrorAngle = errorAngleAxis.getAngle() * dt;
    double errorIntegratedX = errorAngleAxis.getX() * integratedErrorAngle;
    double errorIntegratedY = errorAngleAxis.getY() * integratedErrorAngle;
    double errorIntegratedZ = errorAngleAxis.getZ() * integratedErrorAngle;
    rotationErrorCumulated.add(errorIntegratedX, errorIntegratedY, errorIntegratedZ);

    double errorMagnitude = rotationErrorCumulated.length();
    if (errorMagnitude > gains.getMaximumIntegralError()) {
      rotationErrorCumulated.scale(gains.getMaximumIntegralError() / errorMagnitude);
    }

    rotationErrorCumulated.getFrameTuple(integralTerm);
    integralGainMatrix.transform(integralTerm.getVector());
  }
Ejemplo n.º 5
0
 @Override
 public void convertToAbsolutePosition(Vector3d point) {
   M.transform(point);
   point.add(origin);
 }