private void updatedFinalHeadingLine(double desiredHeadingFinal) { FramePoint2d endpoint1 = processedSensors.getCenterOfMassGroundProjectionInFrame(ReferenceFrame.getWorldFrame()).toFramePoint2d(); FramePoint2d endpoint2 = new FramePoint2d(endpoint1); double length = 1.0; endpoint2.setX(endpoint2.getX() + length * Math.cos(desiredHeadingFinal)); endpoint2.setY(endpoint2.getY() + length * Math.sin(desiredHeadingFinal)); FrameLineSegment2d frameLineSegment2d = new FrameLineSegment2d(endpoint1, endpoint2); finalHeadingLine.setFrameLineSegment2d(frameLineSegment2d); } }
private void setReferenceFootstepLocation(int footstepIndex, FramePoint2d referenceFootstepLocation) { referenceFootstepLocation.changeFrame(worldFrame); referenceFootstepLocations.get(footstepIndex).set(0, 0, referenceFootstepLocation.getX()); referenceFootstepLocations.get(footstepIndex).set(1, 0, referenceFootstepLocation.getY()); }
/** * Changes frame of this FramePoint2d to the given ReferenceFrame, projects into xy plane, and returns a copy. * * @param desiredFrame ReferenceFrame to change the FramePoint2d into. * @return Copied FramePoint2d in the new reference frame. */ public FramePoint2d changeFrameAndProjectToXYPlaneCopy(ReferenceFrame desiredFrame) { FramePoint2d ret = new FramePoint2d(this); ret.changeFrameAndProjectToXYPlane(desiredFrame); return ret; }
private FramePoint2d closestPoint(FramePoint2d point, FramePoint2d candidate1, FramePoint2d candidate2) { if (candidate1.containsNaN() && candidate2.containsNaN()) return null; if (candidate1.containsNaN()) return candidate2; if (candidate2.containsNaN()) return candidate1; if (point.distance(candidate1) <= point.distance(candidate2)) return candidate1; return candidate2; }
/** * This function computes the position of the capture point after the time dt has passed given * the current capture point and the Cop which is assumed to be at constant position over dt. */ public static void predictCapturePoint(FramePoint2d ICP, FramePoint2d CoP, double dt, double omega0, FramePoint2d predictedICPtoPack) { // make sure everything is in the same frame: ICP.checkReferenceFrameMatch(CoP); ICP.checkReferenceFrameMatch(predictedICPtoPack); // CP = (ICP - CoP) * exp(omega0*dt) + CoP predictedICPtoPack.set(ICP); predictedICPtoPack.sub(CoP); predictedICPtoPack.scale(Math.exp(omega0 * dt)); predictedICPtoPack.add(CoP); }
public ContactPoint(FramePoint2d point2d, PlaneContactState parentContactState) { position = new FramePoint(point2d.getReferenceFrame(), point2d.getX(), point2d.getY(), 0.0); this.parentContactState = parentContactState; }
public FrameVector2d computeDesiredMomentumXY(FramePoint2d desiredCoMXY, MutableDouble errorMagnitude) { FrameVector2d ret = new FrameVector2d(); FramePoint2d errorCoMXY = new FramePoint2d(desiredCoMXY); errorCoMXY.changeFrame(referenceFrames.getCenterOfMassFrame()); errorMagnitude.setValue(MathTools.square(errorCoMXY.getX()) + MathTools.square(errorCoMXY.getY())); errorMagnitude.setValue(Math.sqrt(errorMagnitude.doubleValue())); errorCoMXY.scale(1.0 / updateDT); ret.setIncludingFrame(errorCoMXY); ret.scale(toolbox.getTotalRobotMass()); return ret; }
shrunkPolygonCentroid.changeFrame(soleFrame); desiredCenterOfPressure.setIncludingFrame(soleFrame, shrunkPolygonCentroid.getX() + 0.10 * percentRampOut * Math.cos(spiralAngle.getDoubleValue()), shrunkPolygonCentroid.getY() + 0.05 * percentRampOut * Math.sin(spiralAngle.getDoubleValue())); desiredCenterOfPressure.scale(0.9); currentCorner.changeFrame(soleFrame); centroid.changeFrame(soleFrame); desiredCenterOfPressure.changeFrame(soleFrame); desiredCenterOfPressure.interpolate(centroid, currentCorner, percent); desiredCenterOfPressure.set(currentCorner); desiredCenterOfPressure.setIncludingFrame(soleFrame, distanceToMoveFrontBack, 0.0); partialFootholdControlModule.projectOntoShrunkenPolygon(desiredCenterOfPressure); desiredCenterOfPressure.setIncludingFrame(soleFrame, 0.0, distanceToMoveSideSide); partialFootholdControlModule.projectOntoShrunkenPolygon(desiredCenterOfPressure); desiredCenterOfPressure.setIncludingFrame(soleFrame, -distanceToMoveFrontBack, 0.0); partialFootholdControlModule.projectOntoShrunkenPolygon(desiredCenterOfPressure); desiredCenterOfPressure.setIncludingFrame(soleFrame, 0.0, -distanceToMoveSideSide); partialFootholdControlModule.projectOntoShrunkenPolygon(desiredCenterOfPressure); desiredCenterOfPressure.setIncludingFrame(soleFrame, 0.0, 0.0);
private void computeCop() { double force = 0.0; centerOfPressure.setToZero(worldFrame); for (RobotSide robotSide : RobotSide.values) { footSwitches.get(robotSide).computeAndPackCoP(tempFootCop2d); if (tempFootCop2d.containsNaN()) continue; footSwitches.get(robotSide).computeAndPackFootWrench(tempFootWrench); double footForce = tempFootWrench.getLinearPartZ(); force += footForce; tempFootCop.setIncludingFrame(tempFootCop2d.getReferenceFrame(), tempFootCop2d.getX(), tempFootCop2d.getY(), 0.0); tempFootCop.changeFrame(worldFrame); tempFootCop.scale(footForce); centerOfPressure.add(tempFootCop.getX(), tempFootCop.getY()); } centerOfPressure.scale(1.0 / force); yoCenterOfPressure.set(centerOfPressure); }
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); }
public static void projectOntoPolygonAndCheckDistance(FramePoint2d point, FrameConvexPolygon2d polygon, double epsilon) { ReferenceFrame originalReferenceFrame = point.getReferenceFrame(); point.changeFrame(polygon.getReferenceFrame()); FramePoint2d originalPoint = new FramePoint2d(point); polygon.orthogonalProjection(point); double distance = originalPoint.distance(point); if (distance > epsilon) throw new RuntimeException("point outside polygon by " + distance); point.changeFrame(originalReferenceFrame); }
public static void computeCapturePoint(FramePoint2d capturePointToPack, FramePoint2d centerOfMassInWorld, FrameVector2d centerOfMassVelocityInWorld, double omega0) { centerOfMassInWorld.checkReferenceFrameMatch(worldFrame); centerOfMassVelocityInWorld.checkReferenceFrameMatch(worldFrame); capturePointToPack.setToZero(worldFrame); capturePointToPack.set(centerOfMassVelocityInWorld); capturePointToPack.scale(1.0 / omega0); capturePointToPack.add(centerOfMassInWorld); }
/** * This function is called at beginning of every DoubleSupport, not frequent enough to preallocate eveything. */ public void updatePointAndPolygon(FrameConvexPolygon2d polygon, Point2d pointInPolygonFrame) { //copy external point back to polygon frame framePoint2d = new FramePoint2d(polygon.getReferenceFrame(), pointInPolygonFrame); //update polygon class point2dInConvexPolygon2d = new Point2dInConvexPolygon2d(polygon.getConvexPolygon2d(), framePoint2d.getX(), framePoint2d.getY()); angle.set(point2dInConvexPolygon2d.getAngle()); eccentricity.set(point2dInConvexPolygon2d.getEccentricity()); }
@Override public StraightLineOverheadPath changeFrameCopy(ReferenceFrame desiredFrame) { FramePoint2d newStartPoint = new FramePoint2d(startPoint); newStartPoint.changeFrame(desiredFrame); FrameOrientation2d newStartOrientation = new FrameOrientation2d(orientation); newStartOrientation.changeFrame(desiredFrame); FramePose2d newStartPose = new FramePose2d(newStartPoint, newStartOrientation); FramePoint2d newEndPoint = new FramePoint2d(endPoint); newEndPoint.changeFrame(desiredFrame); return new StraightLineOverheadPath(newStartPose, newEndPoint); }
this.desiredCMP.setIncludingFrame(desiredCMP); this.desiredCMP.changeFrameAndProjectToXYPlane(projectionArea.getReferenceFrame()); this.capturePoint.setIncludingFrame(capturePoint); this.capturePoint.changeFrameAndProjectToXYPlane(projectionArea.getReferenceFrame()); if (finalCapturePoint != null) this.finalCapturePoint.setIncludingFrame(finalCapturePoint); else this.finalCapturePoint.setToNaN(); this.finalCapturePoint.changeFrameAndProjectToXYPlane(projectionArea.getReferenceFrame()); ReferenceFrame returnFrame = desiredCMP.getReferenceFrame(); desiredCMP.setIncludingFrame(projectedCMP); desiredCMP.changeFrame(returnFrame); this.desiredCMP.changeFrameAndProjectToXYPlane(worldFrame); projectedCMP.changeFrameAndProjectToXYPlane(worldFrame); if (cmpWasProjected.getBooleanValue())
private void findProjectionOntoPlaneFrame(ReferenceFrame planeFrame, FramePoint2d pointInWorld, FramePoint pointProjectedOntoPlaneFrameToPack) { pointInWorld.checkReferenceFrameMatch(worldFrame); double z = getPlaneZGivenXY(planeFrame, pointInWorld.getX(), pointInWorld.getY()); pointProjectedOntoPlaneFrameToPack.setXYIncludingFrame(pointInWorld); pointProjectedOntoPlaneFrameToPack.setZ(z); }
public void getAdjustedDesiredCapturePoint(FramePoint2d desiredCapturePoint, FramePoint2d adjustedDesiredCapturePoint) { filteredYoAngularMomentum.getFrameTuple(angularMomentum); ReferenceFrame comFrame = angularMomentum.getReferenceFrame(); localDesiredCapturePoint.setIncludingFrame(desiredCapturePoint); localDesiredCapturePoint.changeFrameAndProjectToXYPlane(comFrame); double scaleFactor = momentumGain.getDoubleValue() * omega0.getDoubleValue() / (totalMass.getDoubleValue() * gravity); adjustedDesiredCapturePoint.setIncludingFrame(comFrame, -angularMomentum.getY(), angularMomentum.getX()); adjustedDesiredCapturePoint.scale(scaleFactor); adjustedDesiredCapturePoint.add(localDesiredCapturePoint); adjustedDesiredCapturePoint.changeFrameAndProjectToXYPlane(desiredCapturePoint.getReferenceFrame()); }
public void updatePointAndPolygon(FrameConvexPolygon2d polygon, FramePoint2d pointInAnyFrame) { FramePoint2d temp = new FramePoint2d(pointInAnyFrame); temp.changeFrame(polygon.getReferenceFrame()); updatePointAndPolygon(polygon, temp.getPoint()); }
/** * Compute the capture point position at a given time. * x<sup>ICP<sub>des</sub></sup> = (e<sup>ω0 t</sup>) x<sup>ICP<sub>0</sub></sup> + (1-e<sup>ω0 t</sup>)x<sup>CMP<sub>0</sub></sup> * * @param omega0 the natural frequency ω = √<span style="text-decoration:overline;"> g / z0 </span> of the biped. * @param time * @param initialCapturePoint * @param initialCMP * @param desiredCapturePointToPack */ public static void computeCapturePointPosition(double omega0, double time, FramePoint2d initialCapturePoint, FramePoint2d initialCMP, FramePoint2d desiredCapturePointToPack) { desiredCapturePointToPack.setToZero(initialCapturePoint.getReferenceFrame()); if (initialCapturePoint.distance(initialCMP) > EPSILON) desiredCapturePointToPack.interpolate(initialCMP, initialCapturePoint, Math.exp(omega0 * time)); else desiredCapturePointToPack.set(initialCapturePoint); } }