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; }