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 void update(FrameTuple3DReadOnly frameTuple) { checkReferenceFrameMatch(frameTuple); update((Tuple3DReadOnly) frameTuple); }
@Override public void update() { localVelocityFromFD.update(); localVelocityFiltered.update(); filteredVelocityToCheck.update(); if (!localVelocityFiltered.getHasBufferWindowFilled()) return; for (Direction direction : Direction.values) delayEstimators.get(direction).update(); } }
@Override public void doControl() { double mass = robot.computeCOMMomentum(tempCenterOfMassPoint, tempCenterOfMassVelocity, tempAngularMomentum); exactCenterOfMassPosition.set(tempCenterOfMassPoint); tempCenterOfMassVelocity.scale(1.0 / mass); exactCenterOfMassVelocity.set(tempCenterOfMassVelocity); exactCenterOfMassAcceleration.update(); }
@Override public void doControl() { double mass = robot.computeCOMMomentum(tempCenterOfMassPoint, tempCenterOfMassVelocity, tempAngularMomentum); exactCenterOfMassPosition.set(tempCenterOfMassPoint); tempCenterOfMassVelocity.scale(1.0 / mass); exactCenterOfMassVelocity.set(tempCenterOfMassVelocity); exactCenterOfMassAcceleration.update(); }
@Override public void doControl() { double mass = robot.computeCOMMomentum(tempCenterOfMassPoint, tempCenterOfMassVelocity, tempAngularMomentum); exactCenterOfMassPosition.set(tempCenterOfMassPoint); tempCenterOfMassVelocity.scale(1.0 / mass); exactCenterOfMassVelocity.set(tempCenterOfMassVelocity); exactCenterOfMassAcceleration.update(); }
@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); }
@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); }
filteredPoint.update(unfilteredPoint); xFiltered.update(unfilteredPoint.getX()); yFiltered.update(unfilteredPoint.getY());
@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(); linearMomentumRate.set(momentumChange); // 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); comAcceleration.scale(1.0 / totalMass); // CMP = COMxy - (z/Fz)*Fxy cmp.set(comAcceleration); double z = comPosition.getZ(); double normalizedFz = -gravity + comAcceleration.getZ(); cmp.scale(-z / normalizedFz); cmp.add(comPosition2d); yoCmp.set(cmp); }