public SimulatedDRCRobotTimeProvider(double controlDT) { nanoSecondsPerTick = Conversions.secondsToNanoseconds(controlDT); }
@Override public void tickAndUpdate(double timeToSetInSeconds) { this.update(Conversions.secondsToNanoseconds(timeToSetInSeconds)); }
@Override public void run() { if (Thread.interrupted()) return; serverTime += Conversions.millisecondsToSeconds(updatePeriodMilliseconds); yoVariableServer.update(Conversions.secondsToNanoseconds(serverTime)); } };
@Override public void run() { if (Thread.interrupted()) return; serverTime += Conversions.millisecondsToSeconds(updatePeriodMilliseconds); yoVariableServer.update(Conversions.secondsToNanoseconds(serverTime)); } };
private void setTimeStampsFromDeltaTimes() { long currentTimeStampNano = initialTimeOffsetNano; for (int i = 0; i < tableData.size(); i++) { tableData.get(i).setTimeStamp(currentTimeStampNano); double deltaT = 0.0; Object dtToCast = table.getValueAt(i, 2); if (dtToCast instanceof String) deltaT = Double.parseDouble((String) dtToCast); else if (dtToCast instanceof Double) deltaT = (Double) dtToCast; currentTimeStampNano += Conversions.secondsToNanoseconds(deltaT); } }
public void deleteSelectedRows(int rowIndex) { long timeDelta = 0; if (rowIndex == table.getRowCount() - 1) { return; } timeDelta = Conversions.secondsToNanoseconds(Double.parseDouble(model.getValueAt(rowIndex, 2).toString())); deletedScripts.add(tableData.get(rowIndex)); deletedScriptsPos.add(rowIndex); deletedScriptsTime.add(timeDelta); tableData.remove(rowIndex); loadScriptListener.removeRow(rowIndex); setTimeStamp(rowIndex, timeDelta, false); loadScriptListener.setData(tableData); if (tableData.size() == 0) frameLabel.setText("No reference frame"); }
@Override public void scan(float[] scan, RigidBodyTransform lidarTransform, double time) { final SimulatedLidarScanPacket lidarScan = MessageTools.createSimulatedLidarScanPacket(lidarSensorId, new LidarScanParameters(lidarScanParameters, Conversions.secondsToNanoseconds(time)), Arrays.copyOf(scan, scan.length)); objectCommunicator.consumeObject(lidarScan); } }
@Override public void actionPerformed(ActionEvent event) { int selected = scriptTypeSelector.getSelectedIndex(); for (int i = 0; i < tableData.size(); i++) { if (tableData.get(i).getScriptObject().toString().contains(scriptTypeSelectorString[selected])) { long time = tableData.get(i).getTimeStamp(); double deltaTimeDes = Double.parseDouble(setDeltaTime.getText()) * 1000000000; long deltaTimeDesired = (long) deltaTimeDes; tableData.get(i + 1).setTimeStamp(time + deltaTimeDesired); for (int j = i + 2; j < tableData.size(); j++) { long deltaTime = Conversions.secondsToNanoseconds(Double.parseDouble(model.getValueAt(j - 1, 2).toString())); tableData.get(j).setTimeStamp(tableData.get(j - 1).getTimeStamp() + deltaTime); } loadScriptListener.setData(tableData); } } } }
@Override public void run() { while (running()) { try { Thread.sleep(900); pelvisTransform.set(pelvis.getJointTransform3D()); long timeStamp = Conversions.secondsToNanoseconds(simulationConstructionSet.getTime()); Thread.sleep((int) (random.nextDouble() * 200)); TimeStampedTransform3D timeStampedTransform = new TimeStampedTransform3D(pelvisTransform, timeStamp); StampedPosePacket posePacket = HumanoidMessageTools.createStampedPosePacket("/pelvis", timeStampedTransform, 1.0); externalPelvisPoseCreator.setNewestPose(posePacket); } catch (InterruptedException e) { e.printStackTrace(); } } } };
return; long timeDeltaToCopy = Conversions.secondsToNanoseconds(Double.parseDouble(model2.getValueAt(selectedRow[0], 2).toString())); if (selectedRow[0] == 0) long timeDelta = Conversions.secondsToNanoseconds(Double.parseDouble(model.getValueAt(selectedRow[0] - 1, 2).toString())); setTimeForCopy = tableData.get(selectedRow[0] - 1).getTimeStamp() + timeDelta;
public void copyRow() { int[] selectedRow = table.getSelectedRows(); if (!(selectedRow.length > 0)) { // Do nothing } else { long timeDelta = Conversions.secondsToNanoseconds(Double.parseDouble(model.getValueAt(selectedRow[0], 2).toString())); ScriptObject selectedRowinTable = tableData.get(selectedRow[0]); ScriptObject cloneObject = new ScriptObject(selectedRowinTable.getTimeStamp() + timeDelta, selectedRowinTable.getScriptObject()); tableData.add(selectedRow[0] + 1, cloneObject); for (int i = selectedRow[0] + 2; i < tableData.size(); i++) { long time = tableData.get(i).getTimeStamp(); tableData.get(i).setTimeStamp(time + timeDelta); } loadScriptListener.setData(tableData); refreshTableData(); table.getSelectionModel().setSelectionInterval(selectedRow[0] + 1, selectedRow[0] + 1); table.scrollRectToVisible(new Rectangle(table.getCellRect(selectedRow[0] + 1, 0, true))); } }
@Override public void run() { try { if (!hasBeenInitialized.getBooleanValue()) { initialize(); hasBeenInitialized.set(true); } doControl(); if (yoVariableServer != null) { yoVariableServer.update(Conversions.secondsToNanoseconds(yoTime.getDoubleValue())); } } catch (Exception e) { e.printStackTrace(); } }
public ExecutionTimer(String name, double measurementDelayInSeconds, YoVariableRegistry registry) { this.measurementDelay = Conversions.secondsToNanoseconds(measurementDelayInSeconds); current = new YoDouble(name + "Current", registry); average = new YoDouble(name + "Average", registry); movingAverage = new SimpleMovingAverageFilteredYoVariable(name + "MovingAverage", 100, current, registry); standardDeviation = new YoDouble(name + "StandardDeviation", registry); maximum = new YoDouble(name + "Maximum", registry); count = new YoLong(name + "Count", registry); }
@Override public void read() { readAndUpdateOneDoFJointPositionsVelocitiesAndAccelerations(); readAndUpdateRootJointPositionAndOrientation(); readAndUpdateRootJointAngularAndLinearVelocity(); readAndUpdateIMUSensors(); long timestamp = Conversions.secondsToNanoseconds(robot.getTime()); sensorOutputMap.setTimestamp(timestamp); sensorOutputMap.setVisionSensorTimestamp(timestamp); sensorOutputMap.setSensorHeadPPSTimetamp(timestamp); if (forceSensorDataHolderToUpdate != null) { for (Entry<ForceSensorDefinition, WrenchCalculatorInterface> forceTorqueSensorEntry : forceTorqueSensors.entrySet()) { final WrenchCalculatorInterface forceTorqueSensor = forceTorqueSensorEntry.getValue(); forceTorqueSensor.calculate(); forceSensorDataHolderToUpdate.setForceSensorValue(forceTorqueSensorEntry.getKey(), forceTorqueSensor.getWrench()); } } }
@Override public void read() { // Think about adding root body acceleration to the fullrobotmodel readAndUpdateOneDoFJointPositionsVelocitiesAndAccelerations(); readAndUpdateRootJointPositionAndOrientation(); readAndUpdateRootJointAngularAndLinearVelocity(); // Update frames after setting angular and linear velocities to correctly update zup frames updateReferenceFrames(); long timestamp = Conversions.secondsToNanoseconds(robot.getTime()); this.timestamp.set(timestamp); this.visionSensorTimestamp.set(timestamp); this.sensorHeadPPSTimetamp.set(timestamp); if (forceSensorDataHolderToUpdate != null) { for (Entry<ForceSensorDefinition, WrenchCalculatorInterface> forceTorqueSensorEntry : forceTorqueSensors.entrySet()) { final WrenchCalculatorInterface forceTorqueSensor = forceTorqueSensorEntry.getValue(); forceTorqueSensor.calculate(); forceSensorDataHolderToUpdate.setForceSensorValue(forceTorqueSensorEntry.getKey(), forceTorqueSensor.getWrench()); } } }
private boolean yawBigInDoubleSupport(ExternalPelvisPoseCreator externalPelvisPoseCreator) throws SimulationExceededMaximumTimeException { long timeStamp = Conversions.secondsToNanoseconds(simulationConstructionSet.getTime()); RigidBodyTransform yawTransform = TransformTools.createTransformFromTranslationAndEulerAngles(1, 1, 0.8, 0, 0, Math.PI); TimeStampedTransform3D timeStampedTransform = new TimeStampedTransform3D(yawTransform, timeStamp); StampedPosePacket posePacket = HumanoidMessageTools.createStampedPosePacket("/pelvis", timeStampedTransform, 1.0); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1); externalPelvisPoseCreator.setNewestPose(posePacket); return drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(10); }
private boolean yawBigInSingleSupport(ExternalPelvisPoseCreator externalPelvisPoseCreator) throws SimulationExceededMaximumTimeException { // FootPosePacket packet = new FootPosePacket(RobotSide.RIGHT, new Point3D(1, 1, 0.3), new Quat4d(), 0.6); // drcSimulationTestHelper.send(packet); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(2); long timeStamp = Conversions.secondsToNanoseconds(simulationConstructionSet.getTime()); RigidBodyTransform yawTransform = TransformTools.createTransformFromTranslationAndEulerAngles(1, 1, 0.8, 0, 0, Math.PI); TimeStampedTransform3D timeStampedTransform = new TimeStampedTransform3D(yawTransform, timeStamp); StampedPosePacket posePacket = HumanoidMessageTools.createStampedPosePacket("/pelvis", timeStampedTransform, 1.0); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1); externalPelvisPoseCreator.setNewestPose(posePacket); return drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(10); }
private boolean localizeOutsideOfFootInSingleSupport(ExternalPelvisPoseCreator externalPelvisPoseCreator) throws SimulationExceededMaximumTimeException { // FootPosePacket packet = new FootPosePacket(RobotSide.RIGHT, new Point3D(1.0, 1.0, 0.3), new Quat4d(), 0.6); // drcSimulationTestHelper.send(packet); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(2.0); long timeStamp = Conversions.secondsToNanoseconds(simulationConstructionSet.getTime()); RigidBodyTransform outsideOfFootTransform = TransformTools.createTransformFromTranslationAndEulerAngles(1.5, 1.0, 0.8, 0.0, 0.0, 0.0); TimeStampedTransform3D timeStampedTransform = new TimeStampedTransform3D(outsideOfFootTransform, timeStamp); StampedPosePacket posePacket = HumanoidMessageTools.createStampedPosePacket("/pelvis", timeStampedTransform, 1.0); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1.0); externalPelvisPoseCreator.setNewestPose(posePacket); return drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(10.0); }
@Override public void doControl() { refFrame.update(); sixDofPelvisJoint.setJointConfiguration(robotPose); pelvisPoseHistoryCorrection.doControl(Conversions.secondsToNanoseconds(scs.getTime())); floatingJoint.setQuaternion(sixDofPelvisJoint.getJointPose().getOrientation()); floatingJoint.setPosition(sixDofPelvisJoint.getJointPose().getPosition()); } }
stateEstimatorPelvisPoseBuffer.put(lastPelvisPoseInWorldFrame, Conversions.secondsToNanoseconds(yoTime.getDoubleValue())); long timestamp = Conversions.secondsToNanoseconds(yoTime.getDoubleValue() - icpTimeDelay.getDoubleValue());