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)); } } }
for (int i = 0; i < forceSensorDataHolder.getForceSensorDefinitions().size(); i++) forceSensorDataHolder.get(forceSensorDataHolder.getForceSensorDefinitions().get(i)) .setWrench(robotConfigurationData.getMomentAndForceVectorForSensor(i));
List<ForceSensorDefinition> forceSensorDefinitions = forceSensorDataHolder.getForceSensorDefinitions(); for (ForceSensorDefinition forceSensorDefinition : forceSensorDefinitions)
List<ForceSensorDefinition> forceSensorDefinitions = forceSensorDataHolder.getForceSensorDefinitions(); for (ForceSensorDefinition forceSensorDefinition : forceSensorDefinitions)