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); }
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 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 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 ActualCMPComputer(boolean createViz, SimulationConstructionSet scs, HumanoidFloatingRootJointRobot 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 ActualCMPComputer(boolean createViz, SimulationConstructionSet scs, HumanoidFloatingRootJointRobot 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 PositionVelocity3DConsistencyChecker(String namePrefix, YoFramePoint position, YoFrameVector angularVelocityToCheck, double updateDT, YoVariableRegistry parentRegistry) { registry = new YoVariableRegistry(namePrefix + "PositionVelocity3DCheck"); dummyAlpha = new DoubleYoVariable("dummyAlpha", registry); localVelocityFromFD = FilteredVelocityYoFrameVector.createFilteredVelocityYoFrameVector(namePrefix, "referenceFD", dummyAlpha, updateDT, registry, position); int windowSize = 10; localVelocityFiltered = createSimpleMovingAverageFilteredYoFrameVector(namePrefix, "_referenceFiltered", windowSize, localVelocityFromFD, registry); filteredVelocityToCheck = createSimpleMovingAverageFilteredYoFrameVector(namePrefix, "_filtered", windowSize, angularVelocityToCheck, registry); DelayEstimatorBetweenTwoSignals xVelocityDelayEstimator = new DelayEstimatorBetweenTwoSignals(namePrefix + "X", localVelocityFiltered.getYoX(), filteredVelocityToCheck.getYoX(), updateDT, registry); DelayEstimatorBetweenTwoSignals yVelocityDelayEstimator = new DelayEstimatorBetweenTwoSignals(namePrefix + "Y", localVelocityFiltered.getYoY(), filteredVelocityToCheck.getYoY(), updateDT, registry); DelayEstimatorBetweenTwoSignals zVelocityDelayEstimator = new DelayEstimatorBetweenTwoSignals(namePrefix + "Z", localVelocityFiltered.getYoZ(), filteredVelocityToCheck.getYoZ(), updateDT, registry); delayEstimators.put(Direction.X, xVelocityDelayEstimator); delayEstimators.put(Direction.Y, yVelocityDelayEstimator); delayEstimators.put(Direction.Z, zVelocityDelayEstimator); parentRegistry.addChild(registry); }
public TaskSpaceStiffnessCalculator(String namePrefix, double controlDT, YoVariableRegistry registry) { alphaLowPass = new DoubleYoVariable(namePrefix + "Alpha", registry); alphaLowPass.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(lowPassCutoffFreq_Hz, controlDT)); yoForcePointPosition = new YoFramePoint(namePrefix + "Position", world, registry); yoForcePointForce = new YoFrameVector(namePrefix + "Force", world, registry); yoForcePointVelocity = FilteredVelocityYoFrameVector.createFilteredVelocityYoFrameVector(namePrefix + "Velocity", "", alphaLowPass, controlDT, registry, yoForcePointPosition); yoForcePointForceRateOfChange = FilteredVelocityYoFrameVector.createFilteredVelocityYoFrameVector(namePrefix + "ForceRateOfChange", "", alphaLowPass, controlDT, registry, yoForcePointForce); yoForceAlongDirectionOfMotion = new DoubleYoVariable(namePrefix + "ForceAlongDirOfMotion", registry); yoForceRateOfChangeAlongDirectionOfMotion = new DoubleYoVariable(namePrefix + "DeltaForceAlongDirOfMotion", registry); yoStiffnessAlongDirectionOfMotion = new DoubleYoVariable(namePrefix + "StiffnessAlongDirOfMotion", registry); yoMaxStiffness = new DoubleYoVariable(namePrefix + "MaxStiffness", registry); yoCrossProductOfCurrentVelWithForce = new YoFrameVector(namePrefix + "VelocityCrossForce", world, registry); yoDirectionOfFreeMotion = new YoFrameVector(namePrefix + "DirOfFreeMotion", world, registry); addSimulatedSensorNoise = new BooleanYoVariable(namePrefix + "AddSimulatedNoise", registry); addSimulatedSensorNoise.set(false); }
public TaskSpaceStiffnessCalculator(String namePrefix, double controlDT, YoVariableRegistry registry) { alphaLowPass = new YoDouble(namePrefix + "Alpha", registry); alphaLowPass.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(lowPassCutoffFreq_Hz, controlDT)); yoForcePointPosition = new YoFramePoint3D(namePrefix + "Position", world, registry); yoForcePointForce = new YoFrameVector3D(namePrefix + "Force", world, registry); yoForcePointVelocity = FilteredVelocityYoFrameVector.createFilteredVelocityYoFrameVector(namePrefix + "Velocity", "", alphaLowPass, controlDT, registry, yoForcePointPosition); yoForcePointForceRateOfChange = FilteredVelocityYoFrameVector.createFilteredVelocityYoFrameVector(namePrefix + "ForceRateOfChange", "", alphaLowPass, controlDT, registry, yoForcePointForce); yoForceAlongDirectionOfMotion = new YoDouble(namePrefix + "ForceAlongDirOfMotion", registry); yoForceRateOfChangeAlongDirectionOfMotion = new YoDouble(namePrefix + "DeltaForceAlongDirOfMotion", registry); yoStiffnessAlongDirectionOfMotion = new YoDouble(namePrefix + "StiffnessAlongDirOfMotion", registry); yoMaxStiffness = new YoDouble(namePrefix + "MaxStiffness", registry); yoCrossProductOfCurrentVelWithForce = new YoFrameVector3D(namePrefix + "VelocityCrossForce", world, registry); yoDirectionOfFreeMotion = new YoFrameVector3D(namePrefix + "DirOfFreeMotion", world, registry); addSimulatedSensorNoise = new YoBoolean(namePrefix + "AddSimulatedNoise", registry); addSimulatedSensorNoise.set(false); }