@Override
  public void computeTransform(RigidBodyTransform currXform) {
    update();
    CameraMountInterface cameraMount = getCameraMount();
    if (isMounted() && (cameraMount != null)) {
      cameraMount.getTransformToCamera(currXform);

      return;
    }

    positionOffset.set(getCamX(), getCamY(), getCamZ());
    xAxis.set(getFixX(), getFixY(), getFixZ());

    fixPointNode.translateTo(getFixX(), getFixY(), getFixZ());

    xAxis.sub(positionOffset);
    xAxis.normalize();
    zAxis.set(0.0, 0.0, 1.0);
    yAxis.cross(zAxis, xAxis);
    zAxis.cross(xAxis, yAxis);

    rotationMatrix.setColumn(0, xAxis);
    rotationMatrix.setColumn(1, yAxis);
    rotationMatrix.setColumn(2, zAxis);

    currXform.setRotationAndZeroTranslation(rotationMatrix);
    currXform.setTranslation(positionOffset);
    currXform.normalizeRotationPart();
  }
  private Matrix3d computeDesiredFootRotation(
      double angleToDestination, RobotSide swingLegSide, ReferenceFrame supportFootFrame) {
    RigidBodyTransform supportFootToWorldTransform =
        supportFootFrame.getTransformToDesiredFrame(ReferenceFrame.getWorldFrame());
    Matrix3d supportFootToWorldRotation = new Matrix3d();
    supportFootToWorldTransform.get(supportFootToWorldRotation);

    double maxTurnInAngle = 0.25;
    double maxTurnOutAngle = 0.4;

    double amountToYaw;
    switch (blindWalkingDirection.getEnumValue()) {
      case BACKWARD:
        {
          amountToYaw = AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI);
          break;
        }
      case LEFT:
        {
          amountToYaw =
              AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI / 2.0);
          break;
        }
      case RIGHT:
        {
          amountToYaw =
              AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, -Math.PI / 2.0);
          break;
        }
      case FORWARD:
        {
          amountToYaw = angleToDestination;
          break;
        }
      default:
        {
          throw new RuntimeException("Shouldn't get here!");
        }
    }

    if (swingLegSide == RobotSide.LEFT) {
      amountToYaw = MathTools.clipToMinMax(amountToYaw, -maxTurnInAngle, maxTurnOutAngle);

    } else {
      amountToYaw = MathTools.clipToMinMax(amountToYaw, -maxTurnOutAngle, maxTurnInAngle);
    }

    Matrix3d yawRotation = new Matrix3d();
    yawRotation.rotZ(amountToYaw);

    Matrix3d ret = new Matrix3d();
    ret.mul(yawRotation, supportFootToWorldRotation);

    return ret;
  }
Пример #3
0
  /**
   * Rotate the oriented bounding box of the 3D image about the specified axis with the specified
   * angle.
   *
   * @param transform The transform and its matrix by which to rotate the image.
   */
  public void rotateFrameBy(Transform3D transform) {

    double rY, rX, rZ;
    double sinrX, sinrY, sinrZ, cosrX, cosrY, cosrZ;
    Matrix3d matrix = new Matrix3d();

    transform.get(matrix);

    rY = -Math.asin(matrix.m02);

    if (Math.cos(rY) != 0) {
      rX = -Math.atan2(matrix.m12, matrix.m22);
      rZ = Math.atan2(matrix.m01, matrix.m00);
    } else {
      rX = -Math.atan2(matrix.m10, matrix.m11);
      rZ = 0;
    }

    cosrX = Math.cos(rX);
    sinrX = Math.sin(rX);
    cosrY = Math.cos(rY);
    sinrY = Math.sin(rY);
    cosrZ = Math.cos(rZ);
    sinrZ = Math.sin(rZ);

    matrix.m00 = cosrZ * cosrY;
    matrix.m01 = -sinrZ * cosrY;
    matrix.m02 = sinrY;

    matrix.m10 = (cosrZ * sinrY * sinrX) + (sinrZ * cosrX);
    matrix.m11 = (-sinrZ * sinrY * sinrX) + (cosrZ * cosrX);
    matrix.m12 = -cosrY * sinrX;

    matrix.m20 = (-cosrZ * sinrY * cosrX) + (sinrZ * sinrX);
    matrix.m21 = (sinrZ * sinrY * cosrX) + (cosrZ * sinrX);
    matrix.m22 = cosrY * cosrX;

    m_kRotate.set(matrix);
    m_akAxis[0] = new Vector3f(1.0f, 0.0f, 0.0f);
    m_akAxis[1] = new Vector3f(0.0f, 1.0f, 0.0f);
    m_akAxis[2] = new Vector3f(0.0f, 0.0f, 1.0f);

    for (int i = 0; i < 3; i++) {
      m_kRotate.transform(m_akAxis[i]);
    }

    orthonormalize(m_akAxis);

    for (int i = 0; i < 3; i++) {
      setAxis(i, m_akAxis[i]);
    }
  }
  @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);
  }
 protected void jointDependentSetAndGetRotation(Matrix3d Rh_i) {
   tempOrientation2.set(
       owner.q_qx.getDoubleValue(),
       owner.q_qy.getDoubleValue(),
       owner.q_qz.getDoubleValue(),
       owner.q_qs.getDoubleValue());
   Rh_i.set(tempOrientation2);
 }
  public void setRotation(Matrix3d rotation) {
    double r11, r12, r13, r21, r22, r23, r31, r32, r33;

    r11 = rotation.getM00();
    r12 = rotation.getM01();
    r13 = rotation.getM02();
    r21 = rotation.getM10();
    r22 = rotation.getM11();
    r23 = rotation.getM12();
    r31 = rotation.getM20();
    r32 = rotation.getM21();
    r33 = rotation.getM22();
    q_qs.set(Math.sqrt((1.0 + r11 + r22 + r33) / 4.0));
    q_qx.set((r32 - r23) / (4.0 * q_qs.getDoubleValue()));
    q_qy.set((r13 - r31) / (4.0 * q_qs.getDoubleValue()));
    q_qz.set((r21 - r12) / (4.0 * q_qs.getDoubleValue()));
  }
  /**
   * resolve
   *
   * <p>マウスの動きから生成された T3D を逆運動学サーバを使って 各ジョイントの動きに直し設定する
   *
   * @param transform
   * @return
   */
  public boolean resolve(Transform3D transform) {
    _setLinkStatus(robot_.getName());

    Matrix3d m3d = new Matrix3d();
    Vector3d v3d = new Vector3d();
    transform.get(m3d, v3d);

    v3d.get(tr.p);
    for (int i = 0; i < 3; i++) {
      for (int j = 0; j < 3; j++) {
        tr.R[3 * i + j] = m3d.getElement(i, j);
      }
    }
    try {
      if (robot_ == null || from_ == null || to_ == null) return false;

      if (!integrator_.calcCharacterInverseKinematics(
          robot_.getName(), from_.getName(), to_.getName(), tr)) {
        System.out.println("ik failed.");
        robot_.calcForwardKinematics();
        return false;
      }
      ;
    } catch (Exception e) {
      e.printStackTrace();
    }

    DblSequenceHolder v = new DblSequenceHolder();
    integrator_.getCharacterAllLinkData(robot_.getName(), LinkDataType.JOINT_VALUE, v);
    robot_.setJointValues(v.value);
    robot_.setJointValuesWithinLimit();
    robot_.updateInitialJointValues();
    _setRootJoint(robot_);
    robot_.calcForwardKinematics();

    return true;
  }
  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());
  }
  private void computeOrientationStateOutputBlock() {
    estimationFrame.getTransformToDesiredFrame(tempTransform, ReferenceFrame.getWorldFrame());
    tempTransform.getRotation(rotationFromEstimationToWorld);

    tempCenterOfMassPosition.setIncludingFrame(centerOfMassPositionPort.getData());
    tempCenterOfMassPosition.changeFrame(estimationFrame);

    ReferenceFrame referenceFrame =
        referenceFrameMap.getFrameByName(
            pointPositionMeasurementInputPort.getData().getBodyFixedReferenceFrameName());
    tempFramePoint.setIncludingFrame(
        referenceFrame,
        pointPositionMeasurementInputPort.getData().getMeasurementPointInBodyFrame());

    tempFramePoint.changeFrame(estimationFrame);
    tempFramePoint.sub(tempCenterOfMassPosition);
    tempFramePoint.scale(-1.0);

    MatrixTools.toTildeForm(tempMatrix, tempFramePoint.getPoint());
    tempMatrix.mul(rotationFromEstimationToWorld, tempMatrix);
    MatrixTools.setDenseMatrixFromMatrix3d(0, 0, tempMatrix, getOutputMatrixBlock(orientationPort));
  }
Пример #12
0
 public Matrix3d getViewMatrix(int index) {
   Matrix3d m = new Matrix3d();
   switch (index) {
     case 0:
       m.setIdentity(); // C4 vertex-centered
       break;
     case 1:
       m.rotX(-0.5 * TETRAHEDRAL_ANGLE); // C3 face-centered  2.0*Math.PI/3
       Matrix3d m1 = new Matrix3d();
       m1.rotZ(Math.PI / 4);
       m.mul(m1);
       break;
     case 2:
       m.rotY(Math.PI / 4); // side face-centered
       break;
     default:
       throw new IllegalArgumentException("getViewMatrix: index out of range:" + index);
   }
   return m;
 }
Пример #13
0
 @Override
 public void convertToAbsolutePosition(Vector3d point) {
   M.transform(point);
   point.add(origin);
 }
Пример #14
0
 @Override
 public boolean isValid() {
   return M.determinant() == 1;
 }
Пример #15
0
 public IfcPosition3d(Vector3d origin) {
   this.origin = origin;
   M = new Matrix3d();
   M.setIdentity();
 }