Java FramePose2d.getPositionDistanceの例

コード例 #1

ファイル: DRCWalkToGoalBehaviorTest.java プロジェクト: ihmcrobotics/ihmc-open-robotics-software

  private void assertPosesAreWithinThresholds(FramePose2d framePose1, FramePose2d framePose2) {
    double positionDistance = framePose1.getPositionDistance(framePose2);
    double orientationDistance = framePose1.getOrientationDistance(framePose2);

    if (DEBUG) {
      PrintTools.debug(this, " desired Midfeet Pose :\n" + framePose1 + "\n");
      PrintTools.debug(this, " actual Midfeet Pose :\n" + framePose2 + "\n");

      PrintTools.debug(this, " positionDistance = " + positionDistance);
      PrintTools.debug(this, " orientationDistance = " + orientationDistance);
    }

    assertEquals(0.0, positionDistance, POSITION_THRESHOLD);
    assertEquals(0.0, orientationDistance, ORIENTATION_THRESHOLD);
  }