private int findXIndex(double x) { return (int) Math.floor(x / cellSize.getX()); }
private double getXCoordinateInSoleFrame(int xIndex) { return ((double) xIndex + 0.5) * cellSize.getX() + gridOrigin.getX(); }
public void update(YoFrameVector2d vector) { checkReferenceFrameMatch(vector.getReferenceFrame()); xDot.update(vector.getX()); yDot.update(vector.getY()); }
public void setDesiredCapturePointState(YoFramePoint2d currentDesiredCapturePointPosition, YoFrameVector2d currentDesiredCapturePointVelocity) { // Do not set the Z to zero! desiredCapturePointPosition.checkReferenceFrameMatch(currentDesiredCapturePointPosition); desiredCapturePointPosition.setX(currentDesiredCapturePointPosition.getX()); desiredCapturePointPosition.setY(currentDesiredCapturePointPosition.getY()); desiredCapturePointVelocity.checkReferenceFrameMatch(currentDesiredCapturePointVelocity); desiredCapturePointVelocity.setX(currentDesiredCapturePointVelocity.getX()); desiredCapturePointVelocity.setY(currentDesiredCapturePointVelocity.getY()); }
double distanceToMoveAwayFromLine = Math.max(minDistanceFromLine, Math.abs(cellSize.getX() * Math.cos(theta) + cellSize.getY() * Math.sin(theta))); shiftingVector.scale(distanceToMoveAwayFromLine);
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); }
@Override public void variableChanged(YoVariable<?> v) { counterGrid.reshape(nLengthSubdivisions.getIntegerValue(), nWidthSubdivisions.getIntegerValue()); occupancyGrid.reshape(nLengthSubdivisions.getIntegerValue(), nWidthSubdivisions.getIntegerValue()); cellSize.setX(footLength / nLengthSubdivisions.getIntegerValue()); cellSize.setY(footWidth / nWidthSubdivisions.getIntegerValue()); cellArea.set(cellSize.getX() * cellSize.getY()); } };
private void constrainCMPAccordingToSupportPolygonAndUserOffsets(FramePoint2d cmpToPack, FrameConvexPolygon2d footSupportPolygon, FramePoint2d centroidOfFootstepToConsider, YoFrameVector2d cmpOffset, double minForwardCMPOffset, double maxForwardCMPOffset) { // First constrain the computed CMP to the given min/max along the x-axis. FramePoint2d footSupportCentroid = footSupportPolygon.getCentroid(); double cmpXOffsetFromCentroid = stepLengthToCMPOffsetFactor.getDoubleValue() * (centroidOfFootstepToConsider.getX() - footSupportCentroid.getX()) + cmpOffset.getX(); cmpXOffsetFromCentroid = MathTools.clipToMinMax(cmpXOffsetFromCentroid, minForwardCMPOffset, maxForwardCMPOffset); cmpToPack.setIncludingFrame(footSupportCentroid); cmpToPack.add(cmpXOffsetFromCentroid, cmpOffset.getY()); // Then constrain the computed CMP to be inside a safe support region tempSupportPolygonForShrinking.setIncludingFrameAndUpdate(footSupportPolygon); convexPolygonShrinker.shrinkConstantDistanceInto(tempSupportPolygonForShrinking, safeDistanceFromCMPToSupportEdges.getDoubleValue(), footSupportPolygon); footSupportPolygon.orthogonalProjection(cmpToPack); }
private void computeEntryCMP(FramePoint entryCMPToPack, RobotSide robotSide, ReferenceFrame soleFrame, FrameConvexPolygon2d footSupportPolygon, FramePoint2d centroidInSoleFrameOfPreviousSupportFoot, YoFramePoint previousExitCMP) { if (useTwoCMPsPerSupport) { if (centroidInSoleFrameOfPreviousSupportFoot != null) centroidOfFootstepToConsider.setIncludingFrame(centroidInSoleFrameOfPreviousSupportFoot); else centroidOfFootstepToConsider.setToZero(soleFrame); centroidOfFootstepToConsider.changeFrameAndProjectToXYPlane(soleFrame); if (previousExitCMP != null) { previousExitCMP.getFrameTuple2dIncludingFrame(previousExitCMP2d); previousExitCMP2d.changeFrameAndProjectToXYPlane(soleFrame); // Choose the laziest option if (Math.abs(previousExitCMP2d.getX()) < Math.abs(centroidOfFootstepToConsider.getX())) centroidOfFootstepToConsider.set(previousExitCMP2d); } constrainCMPAccordingToSupportPolygonAndUserOffsets(cmp2d, footSupportPolygon, centroidOfFootstepToConsider, entryCMPUserOffsets.get(robotSide), minForwardEntryCMPOffset.getDoubleValue(), maxForwardEntryCMPOffset.getDoubleValue()); } else { cmp2d.setIncludingFrame(footSupportPolygon.getCentroid()); YoFrameVector2d offset = entryCMPUserOffsets.get(robotSide); cmp2d.add(offset.getX(), offset.getY()); } entryCMPToPack.setXYIncludingFrame(cmp2d); entryCMPToPack.changeFrame(worldFrame); }
@Override public double calculateCost(FramePose stanceFoot, FramePose swingStartFoot, FramePose idealFootstep, FramePose candidateFootstep, double percentageOfFoothold) { double cost = footstepBaseCost.getDoubleValue(); setXYVectorFromPoseToPoseNormalize(forwardCostVector, swingStartFoot, idealFootstep); setXYVectorFromPoseToPoseNormalize(backwardCostVector, idealFootstep, swingStartFoot); inwardCostVector.set(forwardCostVector.getY(), -forwardCostVector.getX()); outwardCostVector.set(-forwardCostVector.getY(), forwardCostVector.getX()); upwardCostVector.set(0.0, 0.0, 1.0); downwardVector.set(0.0, 0.0, -1.0); setVectorFromPoseToPose(idealToCandidateVector, idealFootstep, candidateFootstep); setOrientationFromPoseToPose(idealToCandidateOrientation, idealFootstep, candidateFootstep); double downwardPenalizationWeightConsideringStancePitch = downwardCostScalar.getDoubleValue(); if (stanceFoot.getPitch() < 0) { downwardPenalizationWeightConsideringStancePitch += -stanceFoot.getPitch() * stancePitchDownwardCostScalar.getDoubleValue(); } cost += penalizeCandidateFootstep(forwardCostVector, forwardCostScalar.getDoubleValue()); cost += penalizeCandidateFootstep(backwardCostVector, backwardCostScalar.getDoubleValue()); cost += penalizeCandidateFootstep(inwardCostVector, inwardCostScalar.getDoubleValue()); cost += penalizeCandidateFootstep(outwardCostVector, outwardCostScalar.getDoubleValue()); cost += penalizeCandidateFootstep(upwardCostVector, upwardCostScalar.getDoubleValue()); cost += penalizeCandidateFootstep(downwardVector, downwardPenalizationWeightConsideringStancePitch); cost += angularCostScalar.getDoubleValue() * Math.abs(idealToCandidateOrientation.getYaw().getDoubleValue()); cost += angularCostScalar.getDoubleValue() * Math.abs(idealToCandidateOrientation.getPitch().getDoubleValue()); cost += angularCostScalar.getDoubleValue() * Math.abs(idealToCandidateOrientation.getRoll().getDoubleValue()); cost += (1.0 - percentageOfFoothold) * negativeFootholdLinearCostScalar.getDoubleValue(); return cost; }
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); }
tempICPOffset.setIncludingFrame(supportFrame, desiredICPOffset.getX(), desiredICPOffset.getY());