private FramePose2d offsetMidFeetPose2d(
FramePose2d initialPose, double walkDistance, Vector2d walkDirection) {
FramePose2d ret = new FramePose2d(initialPose);
walkDirection.normalize();
ret.setX(initialPose.getX() + walkDistance * walkDirection.getX());
ret.setY(initialPose.getY() + walkDistance * walkDirection.getY());
return ret;
}
private WalkToGoalBehavior testWalkToGoalBehavior(
double walkDistance, Vector2d walkDirection, double trajectoryTime)
throws SimulationExceededMaximumTimeException {
FramePose2d initialMidFeetPose =
getCurrentMidFeetPose2dTheHardWayBecauseReferenceFramesDontUpdateProperly(robot);
FramePose2d desiredMidFeetPose =
offsetMidFeetPose2d(initialMidFeetPose, walkDistance, walkDirection);
final WalkToGoalBehavior walkToGoalBehavior =
new WalkToGoalBehavior(
communicationBridge,
fullRobotModel,
yoTime,
getRobotModel().getPhysicalProperties().getAnkleHeight());
communicationBridge.attachGlobalListener(
walkToGoalBehavior.getControllerGlobalPacketConsumer());
walkToGoalBehavior.initialize();
WalkToGoalBehaviorPacket requestedGoal =
new WalkToGoalBehaviorPacket(
desiredMidFeetPose.getX(),
desiredMidFeetPose.getY(),
desiredMidFeetPose.getYaw(),
RobotSide.RIGHT);
walkToGoalBehavior.consumeObjectFromNetworkProcessor(requestedGoal);
WalkToGoalBehaviorPacket walkToGoalFindPathPacket =
new WalkToGoalBehaviorPacket(WalkToGoalAction.FIND_PATH);
walkToGoalBehavior.consumeObjectFromNetworkProcessor(walkToGoalFindPathPacket);
WalkToGoalBehaviorPacket walkToGoalExecutePacket =
new WalkToGoalBehaviorPacket(WalkToGoalAction.EXECUTE);
walkToGoalBehavior.consumeObjectFromNetworkProcessor(walkToGoalExecutePacket);
walkToGoalBehavior.doControl();
assertTrue(walkToGoalBehavior.hasInputBeenSet());
boolean success = executeBehavior(walkToGoalBehavior, trajectoryTime);
assertTrue(success);
FramePose2d finalMidFeetPose =
getCurrentMidFeetPose2dTheHardWayBecauseReferenceFramesDontUpdateProperly(robot);
// FramePose2d finalMidFeetPose = getCurrentMidFeetPose2d(referenceFrames);
PrintTools.debug(this, " initial Midfeet Pose :\n" + initialMidFeetPose + "\n");
assertPosesAreWithinThresholds(desiredMidFeetPose, finalMidFeetPose);
return walkToGoalBehavior;
}