public void update() { if (currentPosition == null) { throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + "position variable to call update(), otherwise use update(FrameTuple3DReadOnly)"); } update(currentPosition); }
public SimulatedRobotCenterOfMassVisualizer(Robot robot, double dt) { this.robot = robot; YoDouble alphaSimCoMAcceleration = new YoDouble("alphaSimCoMAcceleration", registry); exactCenterOfMassAcceleration = FilteredVelocityYoFrameVector.createFilteredVelocityYoFrameVector("exactCenterOfMassAcceleration", "", alphaSimCoMAcceleration, dt, registry, exactCenterOfMassVelocity); alphaSimCoMAcceleration.set(0.99); }
/** * @deprecated Use * {@link #FilteredVelocityYoFrameVector(String, String, DoubleProvider, double, YoVariableRegistry, FrameTuple3DReadOnly)} * instead. */ @Deprecated public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoVariableRegistry registry, FrameTuple3DReadOnly frameTuple3DToDifferentiate) { return new FilteredVelocityYoFrameVector(namePrefix, nameSuffix, alpha, dt, registry, frameTuple3DToDifferentiate); }
public void update(FrameTuple3DReadOnly frameTuple) { checkReferenceFrameMatch(frameTuple); update((Tuple3DReadOnly) frameTuple); }
@Override public void doControl() { // update the linear momentum rate by numerical differentiation of the robot momentum simulatedRobot.getRootJoint().physics.recursiveComputeLinearMomentum(linearMomentum); yoLinearMomentum.set(linearMomentum); momentumChange.update(); momentumChange.get(linearMomentumRate); // get mass and COM position from the robot double totalMass = simulatedRobot.computeCenterOfMass(comPosition); comPosition2d.set(comPosition.getX(), comPosition.getY()); // now compute the COM acceleration comAcceleration.set(linearMomentumRate.getX(), linearMomentumRate.getY()); comAcceleration.scale(1.0 / totalMass); // CMP = COM - 1/omega^2 * COMAcc double omega0 = Math.sqrt(-gravity / comPosition.getZ()); cmp.set(comAcceleration); cmp.scale(- 1.0 / (omega0 * omega0)); cmp.add(comPosition2d); yoCmp.set(cmp); }
yoForcePointVelocity.update(); yoForcePointForceRateOfChange.update(); directionOfMotion.set(yoForcePointVelocity.getFrameTuple()); yoForceRateOfChangeAlongDirectionOfMotion.set(yoForcePointForceRateOfChange.dot(directionOfMotion)); double velocity = yoForcePointVelocity.length();
YoVariableRegistry registry = new YoVariableRegistry("blop"); FilteredVelocityYoFrameVector filteredPoint = new FilteredVelocityYoFrameVector("tested", "", () -> alpha, dt, registry, ReferenceFrame.getWorldFrame()); FilteredVelocityYoVariable xFiltered = new FilteredVelocityYoVariable("xRef", "", alpha, dt, registry); FilteredVelocityYoVariable yFiltered = new FilteredVelocityYoVariable("yRef", "", alpha, dt, registry); filteredPoint.update(unfilteredPoint); xFiltered.update(unfilteredPoint.getX()); yFiltered.update(unfilteredPoint.getY());
public void update(FrameTuple<?, ?> frameTuple) { checkReferenceFrameMatch(frameTuple); xDot.update(frameTuple.getX()); yDot.update(frameTuple.getY()); zDot.update(frameTuple.getZ()); }
@Override public void doControl() { // update the linear momentum rate by numerical differentiation of the robot momentum simulatedRobot.getRootJoint().physics.recursiveComputeLinearMomentum(linearMomentum); yoLinearMomentum.set(linearMomentum); momentumChange.update(); momentumChange.get(linearMomentumRate); // get mass and COM position from the robot double totalMass = simulatedRobot.computeCenterOfMass(comPosition); comPosition2d.set(comPosition.getX(), comPosition.getY()); // now compute the COM acceleration comAcceleration.set(linearMomentumRate.getX(), linearMomentumRate.getY()); comAcceleration.scale(1.0 / totalMass); // CMP = COM - 1/omega^2 * COMAcc double omega0 = Math.sqrt(-gravity / comPosition.getZ()); cmp.set(comAcceleration); cmp.scale(- 1.0 / (omega0 * omega0)); cmp.add(comPosition2d); yoCmp.set(cmp); }
public void update(YoFrameTuple<?, ?> yoFrameTuple) { checkReferenceFrameMatch(yoFrameTuple.getReferenceFrame()); xDot.update(yoFrameTuple.getX()); yDot.update(yoFrameTuple.getY()); zDot.update(yoFrameTuple.getZ()); }
@Override public void update() { localVelocityFromFD.update(); localVelocityFiltered.update(); filteredVelocityToCheck.update(); if (!localVelocityFiltered.getHasBufferWindowFilled()) return; for (Direction direction : Direction.values) delayEstimators.get(direction).update(); } }
public SimulatedRobotCenterOfMassVisualizer(Robot robot, double dt) { this.robot = robot; DoubleYoVariable alphaSimCoMAcceleration = new DoubleYoVariable("alphaSimCoMAcceleration", registry); exactCenterOfMassAcceleration = FilteredVelocityYoFrameVector.createFilteredVelocityYoFrameVector("exactCenterOfMassAcceleration", "", alphaSimCoMAcceleration, dt, registry, exactCenterOfMassVelocity); alphaSimCoMAcceleration.set(0.99); }
/** * @deprecated Use * {@link #FilteredVelocityYoFrameVector(String, String, DoubleProvider, double, YoVariableRegistry, ReferenceFrame)} * instead. */ @Deprecated public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoVariableRegistry registry, ReferenceFrame referenceFrame) { return new FilteredVelocityYoFrameVector(namePrefix, nameSuffix, alpha, dt, registry, referenceFrame); }
@Override public void doControl() { double mass = robot.computeCOMMomentum(tempCenterOfMassPoint, tempCenterOfMassVelocity, tempAngularMomentum); exactCenterOfMassPosition.set(tempCenterOfMassPoint); tempCenterOfMassVelocity.scale(1.0 / mass); exactCenterOfMassVelocity.set(tempCenterOfMassVelocity); exactCenterOfMassAcceleration.update(); }
public SimulatedRobotCenterOfMassVisualizer(Robot robot, double dt) { this.robot = robot; DoubleYoVariable alphaSimCoMAcceleration = new DoubleYoVariable("alphaSimCoMAcceleration", registry); exactCenterOfMassAcceleration = FilteredVelocityYoFrameVector.createFilteredVelocityYoFrameVector("exactCenterOfMassAcceleration", "", alphaSimCoMAcceleration, dt, registry, exactCenterOfMassVelocity); alphaSimCoMAcceleration.set(0.99); }
public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleYoVariable alpha, double dt, YoVariableRegistry registry, ReferenceFrame referenceFrame) { FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, dt, registry); FilteredVelocityYoVariable yDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, dt, registry); FilteredVelocityYoVariable zDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", alpha, dt, registry); FilteredVelocityYoFrameVector ret = new FilteredVelocityYoFrameVector(xDot, yDot, zDot, alpha, dt, registry, referenceFrame); return ret; }
@Override public void doControl() { double mass = robot.computeCOMMomentum(tempCenterOfMassPoint, tempCenterOfMassVelocity, tempAngularMomentum); exactCenterOfMassPosition.set(tempCenterOfMassPoint); tempCenterOfMassVelocity.scale(1.0 / mass); exactCenterOfMassVelocity.set(tempCenterOfMassVelocity); exactCenterOfMassAcceleration.update(); }
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; } }
public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleYoVariable alpha, double dt, YoVariableRegistry registry, YoFramePoint yoFramePointToDifferentiate) { FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoX(), dt, registry); FilteredVelocityYoVariable yDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoY(), dt, registry); FilteredVelocityYoVariable zDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoZ(), dt, registry); FilteredVelocityYoFrameVector ret = new FilteredVelocityYoFrameVector(xDot, yDot, zDot, alpha, dt, registry, yoFramePointToDifferentiate.getReferenceFrame()); return ret; }