public HumanoidReferenceFrames createHumanoidReferenceFrames(FullHumanoidRobotModel fullHumanoidRobotModel) { ForceSensorDataHolder forceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(fullHumanoidRobotModel.getForceSensorDefinitions())); HumanoidRobotDataReceiver robotDataReceiver = new HumanoidRobotDataReceiver(fullHumanoidRobotModel, forceSensorDataHolder); packetCommunicator.attachListener(RobotConfigurationData.class, robotDataReceiver); return robotDataReceiver.getReferenceFrames(); }
public void setForceSensorValue(ForceSensorDefinition forceSensorDefinition, DenseMatrix64F value) { if (value.getNumRows() != Wrench.SIZE || value.getNumCols() != 1) throw new RuntimeException("Unexpected size"); inputForceSensors.setForceSensorValue(forceSensorDefinition, value); }
@Override public ForceSensorData getByName(String sensorName) { ForceSensorDefinition forceSensorDefinition = findForceSensorDefinition(sensorName); if (forceSensorDefinition == null) throw new RuntimeException("Force sensor not found " + sensorName); else return get(forceSensorDefinition); }
ForceSensorDataHolder forceSensorDataHolderToUpdate = new ForceSensorDataHolder(Arrays.asList(forceSensorDefinitions)); CenterOfMassDataHolder centerOfMassDataHolderToUpdate = new CenterOfMassDataHolder(); ForceSensorDataReadOnly footForceSensorForEstimator = forceSensorDataHolderToUpdate.getByName(footForceSensorName); String namePrefix = bipedFeet.get(robotSide).getName() + "StateEstimator";
List<ForceSensorDefinition> forceSensorDefinitions = forceSensorDataHolder.getForceSensorDefinitions(); for (ForceSensorDefinition forceSensorDefinition : forceSensorDefinitions) this.forceSensorData = forceSensorDataHolder.getByName(forceSensorName);
private void updateFullRobotModel(RobotConfigurationData robotConfigurationData, FullRobotModel model, ForceSensorDataHolder forceSensorDataHolder) { FullRobotModelCache fullRobotModelCache = getFullRobotModelCache(model); FloatingInverseDynamicsJoint rootJoint = model.getRootJoint(); if (robotConfigurationData.jointNameHash != fullRobotModelCache.jointNameHash) { System.out.println(robotConfigurationData.jointNameHash); System.out.println(fullRobotModelCache.jointNameHash); throw new RuntimeException("Joint names do not match for RobotConfigurationData"); } float[] newJointAngles = robotConfigurationData.getJointAngles(); for (int i = 0; i < newJointAngles.length; i++) { fullRobotModelCache.allJoints[i].setQ(newJointAngles[i]); } Vector3f translation = robotConfigurationData.getPelvisTranslation(); rootJoint.setPosition(translation.getX(), translation.getY(), translation.getZ()); Quat4f orientation = robotConfigurationData.getPelvisOrientation(); rootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW()); rootJoint.getPredecessor().updateFramesRecursively(); if (forceSensorDataHolder != null) { for (int i = 0; i < forceSensorDataHolder.getForceSensorDefinitions().size(); i++) { forceSensorDataHolder.get(forceSensorDataHolder.getForceSensorDefinitions().get(i)).setWrench( robotConfigurationData.getMomentAndForceVectorForSensor(i)); } } }
ForceSensorDataReadOnly footForceSensorForEstimator = estimatorForceSensorDataHolderToUpdate.getByName(footForceSensorName); String namePrefix = bipedFeet.get(robotSide).getName() + "StateEstimator";
inputForceSensors.getForceSensorValue(forceSensorDefinition, tempWrench); tempWrench.getLinearPartIncludingFrame(tempForce); tempWrench.getAngularPartIncludingFrame(tempTorque); intermediateTorques.get(forceSensorDefinition).getFrameTupleIncludingFrame(tempTorque); tempWrench.set(tempForce, tempTorque); outputForceSensors.setForceSensorValue(forceSensorDefinition, tempWrench);
public void updateForceSensorState() { outputForceSensorDataHolder.set(inputForceSensorDataHolder); outputForceSensorDataHolderWithGravityCancelled.set(inputForceSensorDataHolder); if (hasFootForceSensors) updateFootForceSensorState(); if (hasWristForceSensors) updateWristForceSensorState(); if (forceSensorDataHolderToUpdate != null) forceSensorDataHolderToUpdate.set(outputForceSensorDataHolder); if (wrenchVisualizer != null) wrenchVisualizer.visualize(wrenches); }
DesiredJointDataHolder estimatorDesiredJointDataHolder = null; ForceSensorDataHolder forceSensorDataHolderToUpdate = new ForceSensorDataHolder(Arrays.asList(forceSensorDefinitions)); CenterOfMassDataHolder centerOfMassDataHolderToUpdate = new CenterOfMassDataHolder(); ForceSensorDataReadOnly footForceSensorForEstimator = forceSensorDataHolderToUpdate.getByName(footForceSensorName); String namePrefix = bipedFeet.get(robotSide).getName() + "StateEstimator";
List<ForceSensorDefinition> forceSensorDefinitions = forceSensorDataHolder.getForceSensorDefinitions(); for (ForceSensorDefinition forceSensorDefinition : forceSensorDefinitions) this.forceSensorData = forceSensorDataHolder.getByName(forceSensorName);
for (int i = 0; i < forceSensorDataHolder.getForceSensorDefinitions().size(); i++) forceSensorDataHolder.get(forceSensorDataHolder.getForceSensorDefinitions().get(i)) .setWrench(robotConfigurationData.getMomentAndForceVectorForSensor(i));
ForceSensorDataReadOnly footForceSensorForEstimator = estimatorForceSensorDataHolderToUpdate.getByName(footForceSensorName); String namePrefix = bipedFeet.get(robotSide).getName() + "StateEstimator";
public void getIntoControllerModel() { intermediateToControllerCopier.copy(); rawDataIntermediateToControllerCopier.copy(); controllerForceSensorDataHolder.set(intermediateForceSensorDataHolder); controllerCenterOfMassDataHolder.set(intermediateCenterOfMassDataHolder); controllerContactSensorHolder.set(intermediateContactSensorHolder); }
estimatorForceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(estimatorFullRobotModel.getForceSensorDefinitions())); estimatorCenterOfMassDataHolder = new CenterOfMassDataHolder(); estimatorRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(estimatorFullRobotModel);
DesiredJointDataHolder estimatorDesiredJointDataHolder = null; ForceSensorDataHolder forceSensorDataHolderToUpdate = new ForceSensorDataHolder(Arrays.asList(forceSensorDefinitions)); CenterOfMassDataHolder centerOfMassDataHolderToUpdate = new CenterOfMassDataHolder(); ForceSensorDataReadOnly footForceSensorForEstimator = forceSensorDataHolderToUpdate.getByName(footForceSensorName); String namePrefix = bipedFeet.get(robotSide).getName() + "StateEstimator";
@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 ForceSensorData getByName(String sensorName) { ForceSensorDefinition forceSensorDefinition = findForceSensorDefinition(sensorName); if (forceSensorDefinition == null) throw new RuntimeException("Force sensor not found " + sensorName); else return get(forceSensorDefinition); }
public void getIntoControllerModel() { intermediateToControllerCopier.copy(); rawDataIntermediateToControllerCopier.copy(); controllerForceSensorDataHolder.set(intermediateForceSensorDataHolder); controllerCenterOfMassDataHolder.set(intermediateCenterOfMassDataHolder); controllerContactSensorHolder.set(intermediateContactSensorHolder); }
estimatorForceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(estimatorFullRobotModel.getForceSensorDefinitions())); estimatorCenterOfMassDataHolder = new CenterOfMassDataHolder(); estimatorRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(estimatorFullRobotModel);