public void setDesiredVelocity(FrameVector2d newDesiredVelocity) { newDesiredVelocity.changeFrame(desiredVelocity.getReferenceFrame()); desiredVelocity.set(newDesiredVelocity); }
public void reportControllerFailure(FrameVector2d fallingDirection) { fallingDirection.changeFrame(worldFrame); failureStatusMessage.setFallingDirection(fallingDirection); statusOutputManager.reportStatusMessage(failureStatusMessage); }
public void setXAxis(FrameVector2d xAxis) { this.xAxis.setIncludingFrame(xAxis); this.xAxis.changeFrame(parentFrame); this.xAxis.normalize(); update(); }
public void setXAxis(FrameVector2d xAxis) { this.xAxis.setIncludingFrame(xAxis); this.xAxis.changeFrame(parentFrame); this.xAxis.normalize(); update(); }
private FramePoint computeDesiredFootPosition(ReferenceFrame upcomingSupportFrame, FrameVector2d desiredOffsetFromSupport) { desiredOffsetFromSupport.changeFrame(upcomingSupportFrame); FramePoint footstepPosition = new FramePoint(upcomingSupportFrame, desiredOffsetFromSupport.getX(), desiredOffsetFromSupport.getY(), 0.0); footstepPosition.changeFrame(worldFrame); return footstepPosition; }
private double computeFrameOrientationRelativeToWalkingPath(ReferenceFrame referenceFrame) { this.walkPathVector.sub(this.targetLocation, robotYoPose.getPosition()); fullRobotModel.updateFrames(); FrameVector2d frameHeadingVector = new FrameVector2d(referenceFrame, 1.0, 0.0); frameHeadingVector.changeFrame(worldFrame); double ret = -frameHeadingVector.angle(walkPathVector.getFrameVector2dCopy()); if (DEBUG) { PrintTools.debug(this, "FrameHeadingVector : " + frameHeadingVector); PrintTools.debug(this, "WalkPathVector : " + walkPathVector); PrintTools.debug(this, "OrientationToWalkPath : " + ret); } return ret; }
public void changeFrame(MetersReferenceFrame metersReferenceFrame) { if (getReferenceFrame() instanceof PixelsReferenceFrame) { changeFrame(ReferenceFrame.getWorldFrame()); scale(metersReferenceFrame.getConversionToMeters().getX(), metersReferenceFrame.getConversionToMeters().getY()); } super.changeFrame(metersReferenceFrame); }
public void changeFrame(MetersReferenceFrame metersReferenceFrame) { if (getReferenceFrame() instanceof PixelsReferenceFrame) { changeFrame(ReferenceFrame.getWorldFrame()); scale(metersReferenceFrame.getConversionToMeters().getX(), metersReferenceFrame.getConversionToMeters().getY()); } super.changeFrame(metersReferenceFrame); }
public void computeAchievedCMP(FrameVector achievedLinearMomentumRate, FramePoint2d achievedCMPToPack) { if (achievedLinearMomentumRate.containsNaN()) return; centerOfMass2d.setToZero(centerOfMassFrame); centerOfMass2d.changeFrame(worldFrame); achievedCoMAcceleration2d.setByProjectionOntoXYPlaneIncludingFrame(achievedLinearMomentumRate); achievedCoMAcceleration2d.scale(1.0 / totalMass); achievedCoMAcceleration2d.changeFrame(worldFrame); achievedCMPToPack.set(achievedCoMAcceleration2d); achievedCMPToPack.scale(-1.0 / (omega0 * omega0)); achievedCMPToPack.add(centerOfMass2d); }
public boolean isTransitionToSingleSupportSafe(RobotSide transferToSide) { getICPError(icpError2d); ReferenceFrame leadingAnkleZUpFrame = bipedSupportPolygons.getAnkleZUpFrames().get(transferToSide); icpError2d.changeFrame(leadingAnkleZUpFrame); double ellipticErrorSquared = MathTools.square(icpError2d.getX() / maxICPErrorBeforeSingleSupportX.getDoubleValue()) + MathTools.square(icpError2d.getY() / maxICPErrorBeforeSingleSupportY.getDoubleValue()); boolean closeEnough = ellipticErrorSquared < 1.0; return closeEnough; }
public void addICPOffset(FramePoint2d desiredICPToModify, FrameVector2d desiredICPVelocityToModify) desiredICPVelocityToModify.changeFrame(supportPolygon.getReferenceFrame()); icpOffsetForFreezing.setToZero(); desiredICPToModify.changeFrame(worldFrame); desiredICPVelocityToModify.changeFrame(worldFrame); return; tempICPOffset.changeFrame(supportFrame); desiredICPVelocityToModify.changeFrame(worldFrame);
desiredVelocity.changeFrame(desiredHeading.getReferenceFrame()); velocityMagnitudeInHeading.set(desiredVelocity.dot(desiredHeading)); velocityMagnitudeToLeftOfHeading.set(desiredVelocity.dot(toLeftOfDesiredHeading)); desiredVelocity.changeFrame(desiredHeadingFrame); desiredOffsetInPredictedHeadingFrame.changeFrame(predictedHeadingFrame); desiredOffsetInPredictedHeadingFrame.set(desiredVelocity.getX(), desiredVelocity.getY()); desiredOffsetInPredictedHeadingFrame.scale(stepDuration); desiredOffsetInPredictedHeadingFrame.changeFrame(predictedHeadingFrame); desiredOffsetInPredictedHeadingFrame.scale(stepDuration);
capturePoint.changeFrame(supportAnkleZUp); footCentroid.changeFrame(supportAnkleZUp); firstKinematicExtremeDirection.changeFrame(supportAnkleZUp); lastKinematicExtremeDirection.changeFrame(supportAnkleZUp); predictedICP.changeFrame(supportAnkleZUp); projectedLine.changeFrame(supportAnkleZUp);
desiredCapturePoint.changeFrame(worldFrame); finalDesiredCapturePoint.changeFrame(worldFrame); desiredCapturePointVelocity.changeFrame(worldFrame); tempControl.changeFrame(icpVelocityDirectionFrame); tempControl.setX(tempControl.getX() * captureKpParallelToMotion.getDoubleValue()); tempControl.setY(tempControl.getY() * captureKpOrthogonalToMotion.getDoubleValue()); tempControl.changeFrame(desiredCMP.getReferenceFrame());
public void checkIfRobotIsFalling(FramePoint2d capturePoint2d, FramePoint2d desiredCapturePoint2d) { updateCombinedPolygon(); if (isUsingNextFootstep.getBooleanValue()) icpDistanceFromFootPolygon.set(combinedFootPolygonWithNextFootstep.distance(capturePoint2d)); else icpDistanceFromFootPolygon.set(combinedFootPolygon.distance(capturePoint2d)); // TODO need to investigate this method, seems to be buggy // boolean isCapturePointCloseToFootPolygon = combinedFootPolygon.isPointInside(capturePoint, icpDistanceFromFootPolygonThreshold.getDoubleValue()); boolean isCapturePointCloseToFootPolygon = icpDistanceFromFootPolygon.getDoubleValue() < icpDistanceFromFootPolygonThreshold.getDoubleValue(); boolean isCapturePointCloseToDesiredCapturePoint = desiredCapturePoint2d.distance(capturePoint2d) < icpDistanceFromFootPolygonThreshold.getDoubleValue(); isRobotFalling.set(!isCapturePointCloseToFootPolygon && !isCapturePointCloseToDesiredCapturePoint); if (isRobotFalling.getBooleanValue()) { tempFallingDirection.set(capturePoint2d); FramePoint2d footCenter = combinedFootPolygon.getCentroid(); tempFallingDirection.changeFrame(ReferenceFrame.getWorldFrame()); footCenter.changeFrame(ReferenceFrame.getWorldFrame()); tempFallingDirection.sub(footCenter); fallingDirection.set(tempFallingDirection); } }
desiredOffsetFromSupport.changeFrame(upcomingSupportZUpFrame); FramePoint footstepPosition = new FramePoint(upcomingSupportZUpFrame, desiredOffsetFromSupport.getX(), desiredOffsetFromSupport.getY(), 0.0); footstepPosition.changeFrame(worldFrame);
public void compute(double currentTime, FramePoint2d desiredICP, FrameVector2d desiredICPVelocity, FramePoint2d currentICP, double omega0) desiredICPVelocity.changeFrame(worldFrame); currentICP.changeFrame(worldFrame);
comXYVelocity.changeFrame(pelvisZUpFrame); equivalentDesiredHipVelocity.setX(comXYVelocity.getX()); equivalentDesiredHipVelocity.changeFrame(virtualLegTangentialFrameAnkleCentered);