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 FramePose2d getCurrentMidFeetPose2dTheHardWayBecauseReferenceFramesDontUpdateProperly(
HumanoidFloatingRootJointRobot robot) {
FramePose midFeetPose = getRobotMidFeetPose(robot);
FramePose2d ret = new FramePose2d();
ret.setPoseIncludingFrame(
ReferenceFrame.getWorldFrame(),
midFeetPose.getX(),
midFeetPose.getY(),
midFeetPose.getYaw());
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;
}
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);
}
private FramePose2d getCurrentMidFeetPose2d(HumanoidReferenceFrames referenceFrames) {
robotDataReceiver.updateRobotModel();
referenceFrames.updateFrames();
ReferenceFrame midFeetFrame = referenceFrames.getMidFeetZUpFrame();
FramePose midFeetPose = new FramePose();
midFeetPose.setToZero(midFeetFrame);
midFeetPose.changeFrame(ReferenceFrame.getWorldFrame());
FramePose2d ret = new FramePose2d();
ret.setPoseIncludingFrame(
midFeetPose.getReferenceFrame(),
midFeetPose.getX(),
midFeetPose.getY(),
midFeetPose.getYaw());
return ret;
}
