public FrameConvexPolygon2d getContactPolygonCopy() { return new FrameConvexPolygon2d(soleFrame, contactPoints); }
@Override public FrameConvexPolygon2d changeFrameAndProjectToXYPlaneCopy(ReferenceFrame desiredFrame) { FrameConvexPolygon2d ret = new FrameConvexPolygon2d(this); ret.changeFrameAndProjectToXYPlane(desiredFrame); return ret; }
@Override public FrameConvexPolygon2d applyTransformAndProjectToXYPlaneCopy(RigidBodyTransform transform) { FrameConvexPolygon2d copy = new FrameConvexPolygon2d(this); copy.applyTransformAndProjectToXYPlane(transform); return copy; }
public FrameConvexPolygon2d getFootSupportPolygon(RobotSide robotSide) { if (robotSide == RobotSide.LEFT && leftFootSupportPolygonLength != 0) return new FrameConvexPolygon2d(ReferenceFrame.getWorldFrame(), Arrays.copyOf(leftFootSupportPolygonStore, leftFootSupportPolygonLength)); else if (rightFootSupportPolygonStore != null) return new FrameConvexPolygon2d(ReferenceFrame.getWorldFrame(), Arrays.copyOf(rightFootSupportPolygonStore, rightFootSupportPolygonLength)); else return new FrameConvexPolygon2d(ReferenceFrame.getWorldFrame()); }
@Override public FrameConvexPolygon2d applyTransformCopy(RigidBodyTransform transform) { FrameConvexPolygon2d copy = new FrameConvexPolygon2d(this); copy.applyTransform(transform); return copy; }
public YoFrameConvexPolygon2d(ArrayList<YoFramePoint2d> yoFramePoints, IntegerYoVariable yoNumVertices, ReferenceFrame referenceFrame) { this.numVertices = yoNumVertices; numVertices.addVariableChangedListener(this); this.referenceFrame = referenceFrame; for (YoFramePoint2d point : yoFramePoints) { point.attachVariableChangedListener(this); this.yoFramePoints.add(point); } convexPolygon2dForReading = new FrameConvexPolygon2d(referenceFrame); convexPolygon2dForWriting = new FrameConvexPolygon2d(referenceFrame); }
public synchronized void addConvexPolygons(ArrayList<ConvexPolygon2d> convexPolygons, Color color) { FrameConvexPolygonGroup frameConvexPolygonGroup = frameConvexPolygonGroups.get(color); if (frameConvexPolygonGroup == null) { frameConvexPolygonGroup = new FrameConvexPolygonGroup(color); frameConvexPolygonGroups.put(color, frameConvexPolygonGroup); } ArrayList<FrameConvexPolygon2d> frameConvexPolygons = new ArrayList<FrameConvexPolygon2d>(); for (ConvexPolygon2d polygon : convexPolygons) { frameConvexPolygons.add(new FrameConvexPolygon2d(ReferenceFrame.getWorldFrame(), polygon)); } frameConvexPolygonGroup.addFrameConvexPolygon2ds(frameConvexPolygons); }
public synchronized void addConvexPolygon(ConvexPolygon2d convexPolygon, Color color) { FrameConvexPolygonGroup frameConvexPolygonGroup = frameConvexPolygonGroups.get(color); if (frameConvexPolygonGroup == null) { frameConvexPolygonGroup = new FrameConvexPolygonGroup(color); frameConvexPolygonGroups.put(color, frameConvexPolygonGroup); } ArrayList<FrameConvexPolygon2d> frameConvexPolygons = new ArrayList<FrameConvexPolygon2d>(); if (convexPolygon == null) return; frameConvexPolygons.add(new FrameConvexPolygon2d(ReferenceFrame.getWorldFrame(), convexPolygon)); frameConvexPolygonGroup.addFrameConvexPolygon2ds(frameConvexPolygons); }
public synchronized void addConvexPolygons(ConvexPolygon2d[] convexPolygons, Color color) { FrameConvexPolygonGroup frameConvexPolygonGroup = frameConvexPolygonGroups.get(color); if (frameConvexPolygonGroup == null) { frameConvexPolygonGroup = new FrameConvexPolygonGroup(color); frameConvexPolygonGroups.put(color, frameConvexPolygonGroup); } ArrayList<FrameConvexPolygon2d> frameConvexPolygons = new ArrayList<FrameConvexPolygon2d>(); for (ConvexPolygon2d polygon : convexPolygons) { if (polygon == null) continue; frameConvexPolygons.add(new FrameConvexPolygon2d(ReferenceFrame.getWorldFrame(), polygon)); } frameConvexPolygonGroup.addFrameConvexPolygon2ds(frameConvexPolygons); }
public YoFrameConvexPolygon2d(String namePrefix, String nameSuffix, ReferenceFrame referenceFrame, int maxNumberOfVertices, YoVariableRegistry registry) { this.numVertices = new IntegerYoVariable(namePrefix + "NumVertices" + nameSuffix, registry); numVertices.addVariableChangedListener(this); this.referenceFrame = referenceFrame; for (int i = 0; i < maxNumberOfVertices; i++) { YoFramePoint2d point = new YoFramePoint2d(namePrefix + "_" + i + "_", nameSuffix, referenceFrame, registry); point.attachVariableChangedListener(this); yoFramePoints.add(point); } convexPolygon2dForReading = new FrameConvexPolygon2d(referenceFrame); convexPolygon2dForWriting = new FrameConvexPolygon2d(referenceFrame); }
public FootstepAdjusterVisualizer(FootstepAdjustor footstepAdjustor, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry) { this.footstepAdjustor = footstepAdjustor; String nextFootstepCaption = "DesiredTouchdown"; yoNextFootstepPolygon = new YoFrameConvexPolygon2d(nextFootstepCaption, "", worldFrame, 8, registry); nextFootstepPolygon = new FrameConvexPolygon2d(worldFrame); nextFootstepPolygonArtifact = new YoArtifactPolygon(nextFootstepCaption, yoNextFootstepPolygon, colorDefault, false); nextFootstepPolygonArtifact.setVisible(false); yoGraphicsListRegistry.registerArtifact(getClass().getSimpleName(), nextFootstepPolygonArtifact); parentRegistry.addChild(registry); }
@Override public FrameConvexPolygon2d intersectionWith(FrameConvexPolygon2d secondConvexPolygon) { checkReferenceFrameMatch(secondConvexPolygon); ConvexPolygon2d intersection = this.convexPolygon.intersectionWith(secondConvexPolygon.convexPolygon); if (intersection == null) return null; return new FrameConvexPolygon2d(secondConvexPolygon.getReferenceFrame(), intersection); }
public SimpleFootstepMask(ReferenceFrame yawFrame2D, ContactablePlaneBody foot, double footKernelMaskSafetyBuffer) { this.yawFrame2d = yawFrame2D; this.safetyBuffer = footKernelMaskSafetyBuffer; ArrayList<FramePoint2d> contactPoints = new ArrayList<FramePoint2d>(); for (FramePoint2d point : foot.getContactPoints2d()) { contactPoints.add(new FramePoint2d(yawFrame2D, inflate(point.getX()), inflate(point.getY()))); } footPolygon = new FrameConvexPolygon2d(contactPoints); if (DEBUG) System.out.println("SimpleFootstepMask: footPolygon is \n" + footPolygon + " \nand yawFrame2d = \n" + yawFrame2d.getTransformToDesiredFrame(ReferenceFrame.getWorldFrame())); }
private void calculateReachableRegions() { for (RobotSide side : RobotSide.values) { FrameConvexPolygon2d reachableRegion = new FrameConvexPolygon2d(); reachableRegion.clear(ankleZUpFrames.get(side)); double sign = side == RobotSide.RIGHT ? 1.0 : -1.0; for (int i = 0; i < MAX_CAPTURE_REGION_POLYGON_POINTS - 1; i++) { double angle = sign * reachableRegionCutoffAngle * Math.PI * ((double) i) / ((double) (MAX_CAPTURE_REGION_POLYGON_POINTS - 2)); double x = kinematicStepRange * Math.cos(angle) + midFootAnkleXOffset; double y = kinematicStepRange * Math.sin(angle); if (Math.abs(y) < footWidth / 2.0) y = sign * footWidth / 2.0; reachableRegion.addVertex(ankleZUpFrames.get(side), x, y); } reachableRegion.addVertex(ankleZUpFrames.get(side), midFootAnkleXOffset, sign * footWidth / 2.0); reachableRegion.update(); reachableRegions.set(side, reachableRegion); } }
public FootstepAdjustor(SideDependentList<? extends ContactablePlaneBody> contactableFeet, YoVariableRegistry parentRegistry, YoGraphicsListRegistry yoGraphicsListRegistry) { parentRegistry.addChild(registry); if (yoGraphicsListRegistry != null && VISUALIZE) { footstepAdjusterVisualizer = new FootstepAdjusterVisualizer(this, yoGraphicsListRegistry, registry); } defaultSupportPolygons = new SideDependentList<>(); for (RobotSide robotSide : RobotSide.values) { defaultSupportPolygons.put(robotSide, new FrameConvexPolygon2d(contactableFeet.get(robotSide).getContactPoints2d()).getConvexPolygon2d()); } }
public WalkingFailureDetectionControlModule(SideDependentList<? extends ContactablePlaneBody> contactableFeet, YoVariableRegistry parentRegistry) { for (RobotSide robotSide : RobotSide.values) { FrameConvexPolygon2d footPolygonInFootFrame = new FrameConvexPolygon2d(contactableFeet.get(robotSide).getContactPoints2d()); footPolygons.put(robotSide, footPolygonInFootFrame); FrameConvexPolygon2d footPolygonInWorldFrame = new FrameConvexPolygon2d(); footPolygonsInWorldFrame.put(robotSide, footPolygonInWorldFrame); } // Just for allocating memory for the nextFootstepPolygon nextFootstepPolygon.setIncludingFrameAndUpdate(footPolygons.get(RobotSide.LEFT)); isUsingNextFootstep = new BooleanYoVariable("isFallDetectionUsingNextFootstep", registry); isUsingNextFootstep.set(false); updateCombinedPolygon(); isFallDetectionActivated = new BooleanYoVariable("isFallDetectionActivated", registry); isFallDetectionActivated.set(true); icpDistanceFromFootPolygonThreshold = new DoubleYoVariable("icpDistanceFromFootPolygonThreshold", registry); icpDistanceFromFootPolygonThreshold.set(0.05); icpDistanceFromFootPolygon = new DoubleYoVariable("icpDistanceFromFootPolygon", registry); isRobotFalling = new BooleanYoVariable("isRobotFalling", registry); parentRegistry.addChild(registry); }
/** * generates garbage * @param polygon1 * @param polygon2 * @return */ //this should throw away points that are inside of the other polygon public static FrameConvexPolygon2dAndConnectingEdges combineDisjointPolygons(FrameConvexPolygon2d polygon1, FrameConvexPolygon2d polygon2) { ReferenceFrame referenceFrame = polygon1.getReferenceFrame(); polygon2.checkReferenceFrameMatch(referenceFrame); ConvexPolygon2dAndConnectingEdges polygonAndEdges = combineDisjointPolygons(polygon1.convexPolygon, polygon2.convexPolygon); if (polygonAndEdges == null) return null; // Return null if not disjoint FrameConvexPolygon2d frameConvexPolygon2d = new FrameConvexPolygon2d(referenceFrame, polygonAndEdges.getConvexPolygon2d()); FrameLineSegment2d connectingEdge1 = new FrameLineSegment2d(referenceFrame, polygonAndEdges.getConnectingEdge1()); FrameLineSegment2d connectingEdge2 = new FrameLineSegment2d(referenceFrame, polygonAndEdges.getConnectingEdge2()); FrameConvexPolygon2dAndConnectingEdges ret = new FrameConvexPolygon2dAndConnectingEdges(polygon1, polygon2, frameConvexPolygon2d, connectingEdge1, connectingEdge2); return ret; }
private void sequenceShiftWeight() { FramePoint2d center = new FramePoint2d(midFeetZUpFrame); FrameConvexPolygon2d supportPolygon = new FrameConvexPolygon2d(yoSupportPolygon.getFrameConvexPolygon2d()); supportPolygon.changeFrameAndProjectToXYPlane(midFeetZUpFrame); FrameVector2d desiredPelvisOffset = new FrameVector2d(midFeetZUpFrame); for (int i = 0; i < supportPolygon.getNumberOfVertices(); i++) { supportPolygon.getFrameVertex(i, desiredPelvisOffset); desiredPelvisOffset.sub(center); submitDesiredPelvisPositionOffset(false, pelvisShiftScaleFactor.getX() * desiredPelvisOffset.getX(), pelvisShiftScaleFactor.getY() * desiredPelvisOffset.getY(), 0.0); } // Get back to the first vertex again supportPolygon.getFrameVertex(0, desiredPelvisOffset); desiredPelvisOffset.sub(center); submitDesiredPelvisPositionOffset(false, pelvisShiftScaleFactor.getX() * desiredPelvisOffset.getX(), pelvisShiftScaleFactor.getY() * desiredPelvisOffset.getY(), 0.0); submitPelvisHomeCommand(false); }
private void sequenceMediumWarmup() { FramePoint2d center = new FramePoint2d(midFeetZUpFrame); FrameVector2d shiftScaleVector = new FrameVector2d(midFeetZUpFrame, 0.1, 0.7); FrameConvexPolygon2d supportPolygon = new FrameConvexPolygon2d(yoSupportPolygon.getFrameConvexPolygon2d()); supportPolygon.changeFrameAndProjectToXYPlane(midFeetZUpFrame); FrameVector2d desiredPelvisOffset = new FrameVector2d(midFeetZUpFrame); for (int i = 0; i < supportPolygon.getNumberOfVertices(); i++) { supportPolygon.getFrameVertex(i, desiredPelvisOffset); desiredPelvisOffset.sub(center); submitDesiredPelvisPositionOffset(false, shiftScaleVector.getX() * desiredPelvisOffset.getX(), shiftScaleVector.getY() * desiredPelvisOffset.getY(), 0.0); sequenceSquats(); sequenceChestRotations(0.55); //TODO increase/decrease limit? sequencePelvisRotations(0.3); //TODO increase/decrease limit? } // Get back to the first vertex again supportPolygon.getFrameVertex(0, desiredPelvisOffset); desiredPelvisOffset.sub(center); submitDesiredPelvisPositionOffset(false, pelvisShiftScaleFactor.getX() * desiredPelvisOffset.getX(), pelvisShiftScaleFactor.getY() * desiredPelvisOffset.getY(), 0.0); submitChestHomeCommand(false); submitPelvisHomeCommand(false); }
public WalkOnTheEdgesManager(FullHumanoidRobotModel fullRobotModel, WalkingControllerParameters walkingControllerParameters, SideDependentList<? extends ContactablePlaneBody> feet, SideDependentList<YoPlaneContactState> footContactStates, YoVariableRegistry parentRegistry) { this.doToeOffIfPossible.set(walkingControllerParameters.doToeOffIfPossible()); this.doToeOffIfPossibleInSingleSupport.set(walkingControllerParameters.doToeOffIfPossibleInSingleSupport()); this.doToeOffWhenHittingAnkleLimit.set(walkingControllerParameters.doToeOffWhenHittingAnkleLimit()); this.ankleLowerLimitToTriggerToeOff.set(walkingControllerParameters.getAnkleLowerLimitToTriggerToeOff()); this.icpProximityToLeadingFootForDSToeOff.set(walkingControllerParameters.getICPProximityToLeadingFootForToeOff()); this.icpPercentOfStanceForSSToeOff.set(walkingControllerParameters.getICPPercentOfStanceForSSToeOff()); this.walkingControllerParameters = walkingControllerParameters; this.fullRobotModel = fullRobotModel; this.feet = feet; this.inPlaceWidth = walkingControllerParameters.getInPlaceWidth(); this.footLength = walkingControllerParameters.getFootBackwardOffset() + walkingControllerParameters.getFootForwardOffset(); extraCoMMaxHeightWithToes.set(0.08); minStepLengthForToeOff.set(walkingControllerParameters.getMinStepLengthForToeOff()); minStepHeightForToeOff.set(walkingControllerParameters.getMinStepHeightForToeOff()); isRearAnklePitchHittingLimit = new BooleanYoVariable("isRearAnklePitchHittingLimit", registry); isRearAnklePitchHittingLimitFilt = new GlitchFilteredBooleanYoVariable("isRearAnklePitchHittingLimitFilt", registry, isRearAnklePitchHittingLimit, 10); footDefaultPolygons = new SideDependentList<>(); for (RobotSide robotSide : RobotSide.values) { footDefaultPolygons.put(robotSide, new FrameConvexPolygon2d(feet.get(robotSide).getContactPoints2d())); } this.footContactStates = footContactStates; parentRegistry.addChild(registry); }