@Override public void addTimeOffset(double timeOffsetToAdd) { time.add(timeOffsetToAdd); }
@Override public final void addTimeOffset(double timeOffsetToAdd) { time.add(timeOffsetToAdd); }
public void update(double dataSource) { dataLength.increment(); dataCumulated.add(dataSource); }
@Override public void initialize() { robot.update(); for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) { initialPositions.get(i).set(externalForcePoints.get(i).getYoPosition()); desiredHeight.add(initialPositions.get(i).getZ() / initialPositions.size()); efp_positionViz.get(i).update(); } for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) initialPositions.get(i).setZ(desiredHeight.getDoubleValue()); doControl(); }
public void computeNextTick(FramePoint3D positionToPack, double deltaT) { timeIntoStep.add(deltaT); compute(timeIntoStep.getDoubleValue()); getPosition(positionToPack); }
public void computeNextTick(FramePoint3D positionToPack, FrameVector3D velocityToPack, FrameVector3D accelerationToPack, double deltaT) { timeIntoStep.add(deltaT); compute(timeIntoStep.getDoubleValue()); getLinearData(positionToPack, velocityToPack, accelerationToPack); }
@Override public void tickAndUpdate() { scs.setTime(time.getDoubleValue()); scs.tickAndUpdate(); time.add(1.0); }
public void computeNextTick(FramePoint3D positionToPack, FrameVector3D velocityToPack, FrameVector3D accelerationToPack, double deltaT) { timeIntoStep.add(deltaT); compute(timeIntoStep.getDoubleValue()); getPosition(positionToPack); getVelocity(velocityToPack); getAcceleration(accelerationToPack); }
private void simulateOneTick(boolean checkForDiscontinuities, boolean checkIfDyanmicsAreSatisfied) { yoTime.add(dt); getAllVariablesFromPlanner(planner, icpPlannerData1); updateUpdatables(yoTime.getDoubleValue()); if (checkForDiscontinuities) testForDiscontinuities(icpPlannerData1); if (checkIfDyanmicsAreSatisfied) testIfDynamicsAreSatisfied(); if (visualize) updateVisualizePerTick(); }
@Override public void compute(double time) { integratedPhaseAngle.add(2.0 * Math.PI * frequency.getValue() * controlDT); double angle = integratedPhaseAngle.getValue(); double mult = 2.0 * Math.PI * frequency.getValue(); double alpha = 0.5 * Math.sin(angle) + 0.5; double alphaDot = 0.5 * 2.0 * mult * Math.cos(angle); double alphaDDot = -0.5 * 2.0 * 2.0 * mult * mult * Math.sin(angle); limitedPointA.update(); limitedPointB.update(); position.interpolate(limitedPointA, limitedPointB, alpha); linearVelocity.sub(limitedPointB, limitedPointA); linearVelocity.scale(alphaDot); linearAcceleration.sub(limitedPointB, limitedPointA); linearAcceleration.scale(alphaDDot); }
private void updateActionMode() { timeInCurrentMode.add(controlDt); updateWindowSize(); StictionActionMode estimatedCurrentMode = estimateCurrentActionMode(); if (timeInCurrentMode.getDoubleValue() > minTimeInMode.getValue() && estimatedCurrentMode != stictionActionMode.getEnumValue()) { stictionActionMode.set(estimatedCurrentMode); timeInCurrentMode.set(0.0); } }
private void simulateOneTickAndAssertSamePlan(SmoothCMPBasedICPPlanner planner1, SmoothCMPBasedICPPlanner planner2) { yoTime.add(dt); getAllVariablesFromPlanner(planner1, icpPlannerData1); getAllVariablesFromPlanner(planner2, icpPlannerData2); assertCoPWaypointsAreEqual(planner1, planner2, 1e-10); assertCMPWaypointsAreEqual(planner1, planner2, 1e-10); assertICPWaypointsAreEqual(planner1, planner2, 1e-10); assertCoMPlansAreEqual(planner1, planner2, 1e-10); assertPlansAreEqual(icpPlannerData1, icpPlannerData2, 1e-10); }
private void doSingleTimeUpdate() { // (1) do control and compute desired accelerations controllerToolbox.update(); walkingController.doAction(); ControllerCoreCommand coreCommand = walkingController.getControllerCoreCommand(); controllerCore.submitControllerCoreCommand(coreCommand); controllerCore.compute(); // (2) integrate accelerations in full robot model integrate(); // update viz and advance time fullRobotModel.updateFrames(); referenceFrames.updateFrames(); yoTime.add(robotModel.getControllerDT()); }
@Override public void doControl() { desiredLidarAngle.add(desiredLidarVelocity.getDoubleValue() * controlDT); double lidarJointTau = lidarJointController.compute(lidarJoint.getQYoVariable().getDoubleValue(), desiredLidarAngle.getDoubleValue(), lidarJoint.getQDYoVariable() .getDoubleValue(), desiredLidarVelocity.getDoubleValue(), controlDT) + lidarJoint.getDamping() * desiredLidarVelocity.getDoubleValue(); lidarJoint.setTau(lidarJointTau); }
public void update(ByteBuffer buffer) { for (int i = 0; i < ADC.length; i++) { ADC[i].set(buffer.getShort()); } busVoltage.set(((double) (ADC[0].getIntegerValue() & 0xFFFF)) / 491.0 - 0.1); leftLimbCurrent.set((ADC[1].getValueAsDouble() + 16.0) * 0.0061); rightLimbCurrent.set((ADC[2].getValueAsDouble() + 14.0) * 0.0061); torsoLimbCurrent.set((ADC[3].getValueAsDouble() + 15.0) * 0.0061); robotPower.set(busVoltage.getDoubleValue() * (leftLimbCurrent.getDoubleValue() + rightLimbCurrent.getDoubleValue() + torsoLimbCurrent.getDoubleValue())); robotWork.add(robotPower.getDoubleValue() * dt); }
public void update(double inputPosition, double inputVelocity, double inputAcceleration) { if (!hasBeenInitialized.getBooleanValue()) initialize(inputPosition, inputVelocity, inputAcceleration); double positionError = inputPosition - this.getDoubleValue(); double velocityError = inputVelocity - smoothedRate.getDoubleValue(); double accelerationError = inputAcceleration - smoothedAcceleration.getDoubleValue(); double jerk = accelerationGain.getDoubleValue() * accelerationError + velocityGain.getDoubleValue() * velocityError + positionGain.getDoubleValue() * positionError; jerk = MathTools.clamp(jerk, -maximumJerk.getDoubleValue(), maximumJerk.getDoubleValue()); smoothedJerk.set(jerk); smoothedAcceleration.add(smoothedJerk.getDoubleValue() * dt); smoothedAcceleration.set(MathTools.clamp(smoothedAcceleration.getDoubleValue(), maximumJerk.getDoubleValue())); smoothedRate.add(smoothedAcceleration.getDoubleValue() * dt); this.add(smoothedRate.getDoubleValue() * dt); }
@Override public void compute(double time) { currentTime.set(time); time = MathTools.clamp(time, 0.0, trajectoryTime.getDoubleValue()); if (currentTime.getDoubleValue() - currentTimeOffset.getDoubleValue() > subTrajectoryTime.getDoubleValue()) { currentTimeOffset.add(subTrajectoryTime.getDoubleValue()); currentPolynomialIndex.increment(); currentPolynomialIndex.set(Math.min(currentPolynomialIndex.getIntegerValue(), currentNumberOfWaypoints.getIntegerValue())); } time -= currentTimeOffset.getDoubleValue(); time = MathTools.clamp(time, 0.0, subTrajectoryTime.getDoubleValue()); findCurrentPolynomial().compute(time); }
public void update(double input) { if (!hasBeenInitialized.getBooleanValue()) initialize(input); double positionError = input - this.getDoubleValue(); double acceleration = -velocityGain.getDoubleValue() * smoothedRate.getDoubleValue() + positionGain.getDoubleValue() * positionError; acceleration = MathTools.clamp(acceleration, -maximumAcceleration.getDoubleValue(), maximumAcceleration.getDoubleValue()); smoothedAcceleration.set(acceleration); smoothedRate.add(smoothedAcceleration.getDoubleValue() * dt); smoothedRate.set(MathTools.clamp(smoothedRate.getDoubleValue(), maximumRate.getDoubleValue())); this.add(smoothedRate.getDoubleValue() * dt); }