public void reset() { icpErrorIntegrated.set(0.0, 0.0); }
public ManualDesiredVelocityControlModule(ReferenceFrame referenceFrame, YoVariableRegistry parentRegistry) { desiredVelocity = new YoFrameVector2d("desiredVelocity", "", referenceFrame, registry); parentRegistry.addChild(registry); }
public void update(YoFrameVector2d vector) { checkReferenceFrameMatch(vector.getReferenceFrame()); xDot.update(vector.getX()); yDot.update(vector.getY()); }
public double cross(YoFrameVector2d yoFrameVector) { checkReferenceFrameMatch(yoFrameVector); return cross(yoFrameVector.getFrameTuple2d()); }
public double dot(YoFrameVector2d yoFrameVector) { return dot(yoFrameVector.getFrameTuple2d()); }
public double cross(FrameVector2d frameVector) { checkReferenceFrameMatch(frameVector); return getFrameTuple2d().cross(frameVector); }
public static AccelerationLimitedYoFrameVector2d createAccelerationLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleYoVariable maxRate, DoubleYoVariable maxAcceleration, double dt, YoFrameVector2d unfilteredVector) { AccelerationLimitedYoVariable x = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, unfilteredVector.getYoX(), dt); AccelerationLimitedYoVariable y = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, unfilteredVector.getYoY(), dt); AccelerationLimitedYoFrameVector2d ret = new AccelerationLimitedYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); return ret; }
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()); }
@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; }
icpError.set(capturePoint); icpError.sub(desiredCapturePoint); icpError.getFrameTuple2d(tempControl); double epsilonZeroICPVelocity = 1e-5; if (desiredCapturePointVelocity.lengthSquared() > MathTools.square(epsilonZeroICPVelocity)) icpError.getFrameTuple2d(tempICPErrorIntegrated); tempICPErrorIntegrated.scale(controlDT); tempICPErrorIntegrated.scale(captureKi.getDoubleValue()); icpErrorIntegrated.scale(captureKiBleedoff.getDoubleValue()); icpErrorIntegrated.add(tempICPErrorIntegrated); double length = icpErrorIntegrated.length(); double maxLength = 0.02; if (length > maxLength) icpErrorIntegrated.scale(maxLength / length); icpErrorIntegrated.set(0.0, 0.0); icpErrorIntegrated.getFrameTuple2d(icpIntegral); tempControl.add(icpIntegral); feedbackPart.set(tempControl); desiredCMP.add(tempControl);
copCommandWeightVector = new YoFrameVector2d(footName + "CopCommandWeight", null, registry); copCommandWeightVector.set(copCommandWeight.getDoubleValue(), copCommandWeight.getDoubleValue());
private void computeDesiredICPOffset() { pelvisPositionError.set(desiredPelvisPosition); tempPosition2d.setToZero(pelvisZUpFrame); tempPosition2d.changeFrame(worldFrame); pelvisPositionError.sub(tempPosition2d); pelvisPositionError.getFrameTuple2dIncludingFrame(tempError2d); tempError2d.scale(controlDT); pelvisPositionCumulatedError.add(tempError2d); double cumulativeErrorMagnitude = pelvisPositionCumulatedError.length(); if (cumulativeErrorMagnitude > maximumIntegralError.getDoubleValue()) { pelvisPositionCumulatedError.scale(maximumIntegralError.getDoubleValue() / cumulativeErrorMagnitude); } proportionalTerm.set(pelvisPositionError); proportionalTerm.scale(proportionalGain.getDoubleValue()); integralTerm.set(pelvisPositionCumulatedError); integralTerm.scale(integralGain.getDoubleValue()); desiredICPOffset.set(proportionalTerm); desiredICPOffset.add(integralTerm); }
desiredICPOffset.setToZero(); icpOffsetForFreezing.setToZero(); desiredICPToModify.changeFrame(worldFrame); tempICPOffset.setIncludingFrame(supportFrame, desiredICPOffset.getX(), desiredICPOffset.getY()); desiredICPOffset.getFrameTuple2dIncludingFrame(tempICPOffset); tempICPOffset.changeFrame(supportFrame); desiredICPOffset.setAndMatchFrame(icpOffsetForFreezing); desiredICPToModify.changeFrame(icpOffsetForFreezing.getReferenceFrame()); desiredICPToModify.add(icpOffsetForFreezing);
@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 setXYVectorFromPoseToPoseNormalize(YoFrameVector2d vectorToPack, FramePose fromPose, FramePose toPose) { if (fromPose.epsilonEquals(toPose, 1e-7, Double.MAX_VALUE)) { vectorToPack.set(fromPose.getReferenceFrame(), 0.0, 0.0); } else { FrameVector2d frameTuple2d = vectorToPack.getFrameTuple2d(); frameTuple2d.setByProjectionOntoXYPlane(toPose.getFramePointCopy()); fromPose.checkReferenceFrameMatch(vectorToPack); frameTuple2d.sub(fromPose.getX(), fromPose.getY()); frameTuple2d.normalize(); vectorToPack.setWithoutChecks(frameTuple2d); } }
public void setDesiredVelocity(FrameVector2d newDesiredVelocity) { newDesiredVelocity.changeFrame(desiredVelocity.getReferenceFrame()); desiredVelocity.set(newDesiredVelocity); }