/** * Creates a copy of {@code this} by finding the duplicated {@code YoVariable}s in the given * {@link YoVariableRegistry}. * <p> * This method does not duplicate {@code YoVariable}s. Assuming the given registry is a duplicate * of the registry that was used to create {@code this}, this method searches for the duplicated * {@code YoVariable}s and use them to duplicate {@code this}. * </p> * * @param newRegistry YoVariableRegistry to duplicate {@code this} to. * @return the duplicate of {@code this}. */ public YoFrameVector2D duplicate(YoVariableRegistry newRegistry) { YoDouble x = (YoDouble) newRegistry.getVariable(getYoX().getFullNameWithNameSpace()); YoDouble y = (YoDouble) newRegistry.getVariable(getYoY().getFullNameWithNameSpace()); return new YoFrameVector2D(x, y, getReferenceFrame()); } }
public void computeForcePerturbance() { if (perturbanceUnitVector.getX() > 0.0) { perturbanceForce.setX(perturbanceUnitVector.getX() * xPosForce); } else { perturbanceForce.setX(perturbanceUnitVector.getX() * xNegForce); } if (perturbanceUnitVector.getY() > 0.0) { perturbanceForce.setY(perturbanceUnitVector.getY() * yPosForce); } else { perturbanceForce.setY(perturbanceUnitVector.getY() * yNegForce); } }
public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, YoFrameVector2D unfilteredVector) { RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); RateLimitedYoFrameVector2d ret = new RateLimitedYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); return ret; }
/** * Creates a new {@code YoFrameLine2D}. * * @param namePrefix a unique name string to use as the prefix for child variable names. * @param nameSuffix a string to use as the suffix for child variable names. * @param referenceFrame the reference frame for this line. * @param registry the registry to register child variables to. */ public YoFrameLine2D(String namePrefix, String nameSuffix, ReferenceFrame referenceFrame, YoVariableRegistry registry) { point = new YoFramePoint2D(namePrefix, nameSuffix, referenceFrame, registry); direction = new YoFrameVector2D(namePrefix, nameSuffix, referenceFrame, registry); }
YoFrameVector2D radii = new YoFrameVector2D("radii", ReferenceFrame.getWorldFrame(), registry); center.set(-1.0, -1.0); radii.set(1.0, 0.7);
@Override public double calculateCost(FramePose3D stanceFoot, FramePose3D swingStartFoot, FramePose3D idealFootstep, FramePose3D 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; }
public void update(Tuple2DReadOnly currentPosition) { if (!hasBeenCalled.getBooleanValue()) { hasBeenCalled.set(true); lastPosition.set(currentPosition); setToZero(); } currentRawDerivative.sub(currentPosition, lastPosition); currentRawDerivative.scale(1.0 / dt); interpolate(currentRawDerivative, this, alphaProvider.getValue()); lastPosition.set(currentPosition); } }
/** * Gets the internal reference to the x-component used for this line direction. * * @return the direction x-component as {@code YoVariable}. */ public YoDouble getYoDirectionX() { return direction.getYoX(); }
/** * Gets the internal reference to the y-component used for this line direction. * * @return the direction y-component as {@code YoVariable}. */ public YoDouble getYoDirectionY() { return direction.getYoY(); }
/** * Creates a copy of {@code this} by finding the duplicated {@code YoVariable}s in the given * {@link YoVariableRegistry}. * <p> * This method does not duplicate {@code YoVariable}s. Assuming the given registry is a duplicate * of the registry that was used to create {@code this}, this method searches for the duplicated * {@code YoVariable}s and use them to duplicate {@code this}. * </p> * * @param newRegistry YoVariableRegistry to duplicate {@code this} to. * @return the duplicate of {@code this}. */ public YoFrameLine2D duplicate(YoVariableRegistry newRegistry) { return new YoFrameLine2D(point.duplicate(newRegistry), direction.duplicate(newRegistry)); }
public static BetaFilteredYoFrameVector2d createBetaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, int beta, YoFrameVector2D unfilteredVector) { // beta is a int BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, beta, unfilteredVector.getYoX()); BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, beta, unfilteredVector.getYoY()); BetaFilteredYoFrameVector2d ret = new BetaFilteredYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); return ret; }
/** * Creates a new {@code YoFrameLine2D} using the given {@code YoVariable}s and sets its reference * frame to {@code referenceFrame}. * * @param pointX the variable to use for the x-coordinate of this line point. * @param pointY the variable to use for the y-coordinate of this line point. * @param directionX the variable to use for the x-component of this line direction. * @param directionY the variable to use for the x-component of this line direction. * @param referenceFrame the reference frame for this line. */ public YoFrameLine2D(YoDouble pointX, YoDouble pointY, YoDouble directionX, YoDouble directionY, ReferenceFrame referenceFrame) { point = new YoFramePoint2D(pointX, pointY, referenceFrame); direction = new YoFrameVector2D(directionX, directionY, referenceFrame); }
doPelvisAndChestYaw.set(true); pelvisShiftScaleFactor = new YoFrameVector2D("DiagnosticPelvisShiftScaleFactor", null, registry); pelvisShiftScaleFactor.set(0.4, 0.7);
/** * Creates a new {@code YoFrameLine2D} using the given {@code YoVariable}s and sets its reference * frame to {@code referenceFrame}. * * @param point the {@code YoFramePoint2D} to use internally for this line point. * @param direction the {@code YoFrameVector2D} to use internally for this line direction. * @throws ReferenceFrameMismatchException if {@code point} and {@code direction} are not * expressed in the same reference frame. */ public YoFrameLine2D(YoFramePoint2D point, YoFrameVector2D direction) { this(point.getYoX(), point.getYoY(), direction.getYoX(), direction.getYoY(), point.getReferenceFrame()); point.checkReferenceFrameMatch(direction); }
private void setXYVectorFromPoseToPoseNormalize(YoFrameVector2D vectorToPack, FramePose3D fromPose, FramePose3D toPose) { if (fromPose.getPosition().epsilonEquals(toPose.getPosition(), 1e-7)) { vectorToPack.set(fromPose.getReferenceFrame(), 0.0, 0.0); } else { FrameVector2D frameTuple2d = new FrameVector2D(vectorToPack); frameTuple2d.set(toPose.getPosition()); fromPose.checkReferenceFrameMatch(vectorToPack); frameTuple2d.sub(fromPose.getX(), fromPose.getY()); frameTuple2d.normalize(); vectorToPack.set((Tuple2DReadOnly) frameTuple2d); } }
@Override public YoArtifact duplicate(YoVariableRegistry newRegistry) { return new YoArtifactOval(getName(), center.duplicate(newRegistry), radii.duplicate(newRegistry), color); } }
public static AccelerationLimitedYoFrameVector2d createAccelerationLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, YoDouble 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 FilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoVariableRegistry registry, ReferenceFrame referenceFrame) { super(namePrefix, nameSuffix, referenceFrame, registry); this.alphaProvider = alpha; this.dt = dt; hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); currentPosition = null; lastPosition = new YoFrameVector2D(namePrefix + "_lastPosition", nameSuffix, getReferenceFrame(), registry); reset(); }
public ActualCMPComputer(boolean createViz, SimulationConstructionSet scs, FloatingRootJointRobot simulatedRobot) { this.simulatedRobot = simulatedRobot; simulateDT = scs.getDT(); gravity = simulatedRobot.getGravityZ(); momentumChange = FilteredVelocityYoFrameVector .createFilteredVelocityYoFrameVector("rateOfChangeLinearMomentum", "", alpha, simulateDT, registry, yoLinearMomentum); if (createViz) { yoGraphicsListRegistry = new YoGraphicsListRegistry(); YoArtifactPosition cmpViz = new YoArtifactPosition("SimulationCMP", yoCmp.getYoX(), yoCmp.getYoY(), GraphicType.BALL_WITH_CROSS, Color.RED, 0.005); cmpViz.setVisible(visibleByDefault); yoGraphicsListRegistry.registerArtifact(getClass().getSimpleName(), cmpViz); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); } else { yoGraphicsListRegistry = null; } }