forceTorqueSensor.calculate(); sensorProcessing.setForceSensorValue(forceTorqueSensors.get(i).getLeft(), forceTorqueSensor.getWrench());
private void updateMeasurement() { wrenchCalculatorInterface.calculate(); MatrixTools.extractFrameTupleFromEJMLVector(measuredForce, wrenchCalculatorInterface.getWrench(), measurementFrame, 3); measuredForceWorld.setIncludingFrame(measuredForce); measuredForceWorld.changeFrame(worldFrame); yoMeasuredForceWorld.setAndMatchFrame(measuredForce); }
forceTorqueSensor.calculate(); sensorProcessing.setForceSensorValue(forceTorqueSensors.get(i).getLeft(), 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()); } } }
@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(); updateReferenceFrames(); readAndUpdateRootJointAngularAndLinearVelocity(); long timestamp = TimeTools.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()); } } }
point1.getYoPosition().set(new Point3D(-1.0, 0.0, 0.0)); calculator.calculate(); point1.getYoPosition().set(new Point3D(-2.0, -2.0, 1.0)); calculator.calculate(); calculator.calculate(); wholeWrench = calculator.getWrench(); assertTrue(wholeWrench.getNumRows() == 6); robot.update(); calculator.calculate(); wholeWrench = calculator.getWrench(); assertTrue(wholeWrench.getNumRows() == 6);
ReferenceFrame bodyFixedFrame = rigidBodyToApplyWrenchTo.getBodyFixedFrame(); groundContactPointBasedWrenchCalculator.calculate(); DenseMatrix64F wrenchFromSimulation = groundContactPointBasedWrenchCalculator.getWrench(); ReferenceFrame frameAtJoint = rigidBodyToApplyWrenchTo.getParentJoint().getFrameAfterJoint();