us.ihmc.robotics.testing.YoVariableTestGoal.doubleWithinEpsilon()方法的使用及代码示例

x33g5p2x  于2022-02-05 转载在 其他  
字(13.7k)|赞(0)|评价(0)|浏览(74)

本文整理了Java中us.ihmc.robotics.testing.YoVariableTestGoal.doubleWithinEpsilon()方法的一些代码示例，展示了YoVariableTestGoal.doubleWithinEpsilon()的具体用法。这些代码示例主要来源于Github/Stackoverflow/Maven等平台，是从一些精选项目中提取出来的代码，具有较强的参考意义，能在一定程度帮忙到你。YoVariableTestGoal.doubleWithinEpsilon()方法的具体详情如下：
包路径：us.ihmc.robotics.testing.YoVariableTestGoal
类名称：YoVariableTestGoal
方法名：doubleWithinEpsilon

YoVariableTestGoal.doubleWithinEpsilon介绍

暂无

代码示例

代码示例来源：origin: us.ihmc/ihmc-robotics-toolkit

public static YoVariableTestGoal boundingBox(YoFramePoint3D yoFramePoint, Point3DReadOnly boxCenter, double boxEpsilon)
  {
   YoVariableTestGoal goalX = YoVariableTestGoal.doubleWithinEpsilon(yoFramePoint.getYoX(), boxCenter.getX(), boxEpsilon);
   YoVariableTestGoal goalY = YoVariableTestGoal.doubleWithinEpsilon(yoFramePoint.getYoY(), boxCenter.getY(), boxEpsilon);
   YoVariableTestGoal goalZ = YoVariableTestGoal.doubleWithinEpsilon(yoFramePoint.getYoZ(), boxCenter.getZ(), boxEpsilon);
   
   return new YoVariableTestGoal(yoFramePoint.getYoX(), yoFramePoint.getYoY(), yoFramePoint.getYoZ())
   {
     @Override
     public boolean currentlyMeetsGoal()
     {
      return goalX.currentlyMeetsGoal() && goalY.currentlyMeetsGoal() && goalZ.currentlyMeetsGoal();
     }

     @Override
     public String toString()
     {
      return "\n" + goalX.toString() + "\n" + goalY.toString() + "\n" + goalZ.toString();
     }
   };
  }
}

代码示例来源：origin: us.ihmc/ihmc-quadruped-robotics-test

private void testWalkingInASemiCircle(double endPhaseShift, double walkingSpeed, double angularVelocity)
{
 stepTeleopManager.setShiftPlanBasedOnStepAdjustment(false);
 QuadrupedTestBehaviors.readyXGait(conductor, variables, stepTeleopManager);
 double radius = Math.abs(walkingSpeed / angularVelocity);
 double expectedSemiCircleWalkTime = Math.PI / Math.abs(angularVelocity);
 stepTeleopManager.requestXGait();
 stepTeleopManager.getXGaitSettings().setEndPhaseShift(endPhaseShift);
 stepTeleopManager.setDesiredVelocity(walkingSpeed, 0.0, angularVelocity);
 conductor.addSustainGoal(QuadrupedTestGoals.notFallen(variables));
 conductor.addTimeLimit(variables.getYoTime(), expectedSemiCircleWalkTime * 1.5);
 conductor.addWaypointGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), Math.signum(angularVelocity) * Math.PI / 2, 0.1));
 conductor.addTerminalGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyX(), 0.0, 0.2));
 conductor.addTerminalGoal(YoVariableTestGoal.or(
    YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), -Math.PI, 0.2),
    YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), Math.PI, 0.2)));
 if(Math.signum(walkingSpeed) > 0.0)
 {
   conductor.addWaypointGoal(YoVariableTestGoal.doubleGreaterThan(variables.getRobotBodyX(), radius * walkingSpeed * 0.6));
 }
 else
 {
   conductor.addWaypointGoal(YoVariableTestGoal.doubleLessThan(variables.getRobotBodyX(), radius * walkingSpeed * 0.6));
 }
 conductor.simulate();
}

代码示例来源：origin: us.ihmc/ihmc-quadruped-robotics-test

private void standupPrecisely(double desiredCoMHeight) throws AssertionFailedError
{
 QuadrupedTestBehaviors.readyXGait(conductor, variables, stepTeleopManager);
 stepTeleopManager.setDesiredBodyHeight(desiredCoMHeight);
 conductor.addSustainGoal(QuadrupedTestGoals.notFallen(variables));
 conductor.addTerminalGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getCurrentHeightInWorld(), desiredCoMHeight, 0.01));
 conductor.simulate();
}

代码示例来源：origin: us.ihmc/ihmc-quadruped-robotics-test

conductor.addSustainGoal(QuadrupedTestGoals.notFallen(variables));
conductor.addTimeLimit(variables.getYoTime(), expectedSemiCircleWalkTime * 1.5);
conductor.addWaypointGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), Math.signum(angularVelocity) * Math.PI / 2, 0.1));
conductor.addTerminalGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyX(), 0.0, 0.2));
conductor.addTerminalGoal(YoVariableTestGoal.or(
   YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), -Math.PI, 0.2),
   YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), Math.PI, 0.2)));

代码示例来源：origin: us.ihmc/ihmc-quadruped-robotics-test

@ContinuousIntegrationAnnotations.ContinuousIntegrationTest(estimatedDuration = 370.6)
@Test(timeout = 1900000)
public void rotate720InPlaceRight() throws SimulationExceededMaximumTimeException, ControllerFailureException, IOException
{
 QuadrupedTestBehaviors.readyXGait(conductor, variables, stepTeleopManager);
 stepTeleopManager.requestXGait();
 stepTeleopManager.getXGaitSettings().setStanceWidth(0.35);
 conductor.addTerminalGoal(QuadrupedTestGoals.timeInFuture(variables, 1.0));
 conductor.simulate();
 stepTeleopManager.setDesiredVelocity(0.0, 0.0, -0.5);
 int numSpins = 2;
 for (int i = 0; i < numSpins; i++)
 {
   conductor.addSustainGoal(QuadrupedTestGoals.notFallen(variables));
   conductor.addSustainGoal(YoVariableTestGoal.doubleLessThan(variables.getYoTime(), variables.getYoTime().getDoubleValue() + 10.0));
   conductor.addWaypointGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), -Math.PI / 4.0, 1e-2));
   conductor.addTerminalGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), -Math.PI / 2.0, 1e-2));
   conductor.simulate();
   conductor.addSustainGoal(QuadrupedTestGoals.notFallen(variables));
   conductor.addSustainGoal(YoVariableTestGoal.doubleLessThan(variables.getYoTime(), variables.getYoTime().getDoubleValue() + 10.0));
   conductor.addWaypointGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), Math.PI / 4.0, 1e-2));
   conductor.addTerminalGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), 0.0, 1e-2));
   conductor.simulate();
 }
 conductor.concludeTesting();
}

代码示例来源：origin: us.ihmc/ihmc-quadruped-robotics-test

conductor.addWaypointGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), Math.PI / 4.0, 1e-2));
conductor.addTerminalGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), Math.PI / 2.0, 1e-2));
conductor.simulate();
conductor.addWaypointGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), -Math.PI / 4.0, 1e-2));
conductor.addTerminalGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), 0.0, 1e-2));
conductor.simulate();

代码示例来源：origin: us.ihmc/ihmc-quadruped-robotics-test

@ContinuousIntegrationTest(estimatedDuration = 74.7)
@Test(timeout = 370000)
public void testScriptedFlatGroundWalking() throws BlockingSimulationRunner.SimulationExceededMaximumTimeException
{
 QuadrupedTestBehaviors.standUp(conductor, variables);
 QuadrupedTestBehaviors.startBalancing(conductor, variables, stepTeleopManager);
 stepTeleopManager.requestSteppingState();
 conductor.addTerminalGoal(QuadrupedTestGoals.timeInFuture(variables, 1.0));
 conductor.simulate();
 List<QuadrupedTimedStepMessage> steps = getSteps();
 QuadrupedTimedStepListMessage message = QuadrupedMessageTools.createQuadrupedTimedStepListMessage(steps, false);
 stepTeleopManager.publishTimedStepListToController(message);
 // check robot is still upright and walked forward
 Point3D expectedFinalPlanarPosition = getFinalPlanarPosition();
 conductor.addTerminalGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyX(), expectedFinalPlanarPosition.getX(), 0.1));
 conductor.addTerminalGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyY(), expectedFinalPlanarPosition.getY(), 0.1));
 conductor.addTerminalGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), expectedFinalPlanarPosition.getZ(), 0.1));
 conductor.addTerminalGoal(QuadrupedTestGoals.timeInFuture(variables, message.getQuadrupedStepList().getLast().getTimeInterval().getEndTime() + 2.0));
 conductor.addSustainGoal(QuadrupedTestGoals.notFallen(variables));
 conductor.simulate();
 conductor.concludeTesting();
}

代码示例来源：origin: us.ihmc/ihmc-quadruped-robotics-test

private void lowerHeightUntilFailure(double originalHeight) throws AssertionFailedError
{
 for (double heightDelta = 0.0; (originalHeight + heightDelta) > 0.38; heightDelta -= 0.01)
 {
   double desiredCoMHeight = originalHeight + heightDelta;
   stepTeleopManager.setDesiredBodyHeight(desiredCoMHeight);
   variables.getLimitJointTorques().set(false);
   conductor.addSustainGoal(QuadrupedTestGoals.notFallen(variables));
   conductor.addTerminalGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getCurrentHeightInWorld(), originalHeight + heightDelta, 0.01));
   conductor.simulate();
   try
   {
    variables.getLimitJointTorques().set(true);
    conductor.addSustainGoal(QuadrupedTestGoals.notFallen(variables));
    conductor.addSustainGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getCurrentHeightInWorld(), originalHeight + heightDelta, 0.01));
    conductor.addTerminalGoal(YoVariableTestGoal.doubleGreaterThan(variables.getYoTime(), variables.getYoTime().getDoubleValue() + 1.0));
    conductor.simulate();
   }
   catch (AssertionFailedError assertionFailedError)
   {
    PrintTools.info("Failed to stand at " + desiredCoMHeight);
    break;
   }
 }
}

代码示例来源：origin: us.ihmc/ihmc-quadruped-robotics-test

private void raiseHeightUntilFailure(double originalHeight) throws AssertionFailedError
  {
   for (double heightDelta = 0.38 - originalHeight; (originalHeight + heightDelta) < originalHeight; heightDelta += 0.01)
   {
     double desiredCoMHeight = originalHeight + heightDelta;
     stepTeleopManager.setDesiredBodyHeight(desiredCoMHeight);

     variables.getLimitJointTorques().set(false);
     conductor.addSustainGoal(QuadrupedTestGoals.notFallen(variables));
     conductor.addTerminalGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getCurrentHeightInWorld(), originalHeight + heightDelta, 0.01));
     conductor.simulate();

     try
     {
      variables.getLimitJointTorques().set(true);
      conductor.addSustainGoal(QuadrupedTestGoals.notFallen(variables));
      conductor.addSustainGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getCurrentHeightInWorld(), originalHeight + heightDelta, 0.01));
      conductor.addTerminalGoal(YoVariableTestGoal.doubleGreaterThan(variables.getYoTime(), variables.getYoTime().getDoubleValue() + 1.0));
      conductor.simulate();
     }
     catch (AssertionFailedError assertionFailedError)
     {
      PrintTools.info("Failed to stand at " + desiredCoMHeight);
      break;
     }
   }
  }
}

代码示例来源：origin: us.ihmc/ihmc-quadruped-robotics-test

@ContinuousIntegrationTest(estimatedDuration = 120.0)
  @Test(timeout = 800000)
  public void rotate360InPlaceRight() throws SimulationExceededMaximumTimeException, ControllerFailureException, IOException
  {
   QuadrupedTestBehaviors.standUp(conductor, variables);
   
   variables.getYoPlanarVelocityInputZ().set(-0.1);
   
   int numSpins = 1;
   for (int i = 0; i < numSpins; i++)
   {
     conductor.addSustainGoal(QuadrupedTestGoals.notFallen(variables));
     conductor.addSustainGoal(YoVariableTestGoal.doubleLessThan(variables.getYoTime(), variables.getYoTime().getDoubleValue() + 50.0));
//         conductor.addWaypointGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), -Math.PI / 4.0, 1e-2));
     conductor.addTerminalGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), -Math.PI / 2.0, 1e-2));
     conductor.simulate();
//
     conductor.addSustainGoal(QuadrupedTestGoals.notFallen(variables));
     conductor.addSustainGoal(YoVariableTestGoal.doubleLessThan(variables.getYoTime(), variables.getYoTime().getDoubleValue() + 50.0));
//         conductor.addWaypointGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), Math.PI / 4.0, 1e-2));
     conductor.addTerminalGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), 0.0, 1e-2));
     conductor.simulate();
   }
  }

代码示例来源：origin: us.ihmc/ihmc-quadruped-robotics-test

@ContinuousIntegrationTest(estimatedDuration = 120.0)
  @Test(timeout = 800000)
  public void rotate360InPlaceLeft() throws SimulationExceededMaximumTimeException, ControllerFailureException, IOException
  {
   QuadrupedTestBehaviors.standUp(conductor, variables);
   
   variables.getYoPlanarVelocityInputZ().set(0.1);
   
   int numSpins = 1;
   for (int i = 0; i < numSpins; i++)
   {
     conductor.addSustainGoal(QuadrupedTestGoals.notFallen(variables));
     conductor.addSustainGoal(YoVariableTestGoal.doubleLessThan(variables.getYoTime(), variables.getYoTime().getDoubleValue() + 10.0));
//         conductor.addWaypointGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), Math.PI / 4.0, 1e-2));
     conductor.addTerminalGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), Math.PI / 2.0, 1e-2));
     conductor.simulate();

     conductor.addSustainGoal(QuadrupedTestGoals.notFallen(variables));
     conductor.addSustainGoal(YoVariableTestGoal.doubleLessThan(variables.getYoTime(), variables.getYoTime().getDoubleValue() + 10.0));
//         conductor.addWaypointGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), -Math.PI / 4.0, 1e-2));
     conductor.addTerminalGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyYaw(), 0.0, 1e-2));
     conductor.simulate();
   }
  }
}

代码示例来源：origin: us.ihmc/ihmc-quadruped-robotics-test

public static void standUp(GoalOrientedTestConductor conductor, QuadrupedPositionTestYoVariables variables) throws AssertionFailedError
{
 conductor.addTerminalGoal(YoVariableTestGoal.enumEquals(variables.getPositionControllerState(), QuadrupedPositionControllerState.DO_NOTHING));
 conductor.addTerminalGoal(QuadrupedTestGoals.timeInFuture(variables, 1.0));
 conductor.simulate();
 variables.getUserTrigger().set(QuadrupedPositionControllerRequestedEvent.REQUEST_STAND_PREP);
 conductor.addTerminalGoal(YoVariableTestGoal.enumEquals(variables.getPositionControllerState(), QuadrupedPositionControllerState.STAND_READY));
 conductor.addTerminalGoal(QuadrupedTestGoals.timeInFuture(variables, 2.0));
 conductor.simulate();
 variables.getUserTrigger().set(QuadrupedPositionControllerRequestedEvent.REQUEST_CRAWL);
 conductor.addTerminalGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyZ(), variables.getYoComPositionInputZ().getDoubleValue(), 0.1));
 conductor.addTerminalGoal(YoVariableTestGoal.enumEquals(variables.getPositionControllerState(), QuadrupedPositionControllerState.CRAWL));
 conductor.addTerminalGoal(QuadrupedTestGoals.timeInFuture(variables, 2.0));
 conductor.simulate();
}

代码示例来源：origin: us.ihmc/ihmc-quadruped-robotics-test

finalPoint.scaleAdd(0.25, lastMessage3.getQuadrupedStepMessage().getGoalPosition(), finalPoint);
conductor.addWaypointGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyX(), finalPoint.getX(), 0.15));
conductor.addWaypointGoal(YoVariableTestGoal.doubleWithinEpsilon(variables.getRobotBodyY(), finalPoint.getY(), 0.15));

内容来源于网络，如有侵权，请联系作者删除！

YoVariableTestGoal

相关文章

    查看更多

    热门标签

    更多
    JavaquerypythonNode开发语言requestUtil数据库Table后端算法LoggerMessageElementParser

    最新文章

    更多
    • 蜀ICP备13028337号-1 大数据知识库 https://www.saoniuhuo.com © All rights reserved
    • 本站内容来源互联网,如果侵犯您的权益请联系我们删除, 联系方式：448109455@qq.com