public void process(long timestamp) { for (WandererJoint joint : WandererJoint.values) { AcsellJointState jointState = state.getJointState(joint); OneDoFJoint oneDoFJoint = jointMap.get(joint); RawJointSensorDataHolder rawSensor = rawSensors.get(oneDoFJoint); oneDoFJoint.setQ(jointState.getQ()); oneDoFJoint.setQd(jointState.getQd()); state.updateRawSensorData(joint, rawSensor); } AcsellXSensState xSensState = state.getXSensState(); xSensState.getQuaternion(rotation); // rootJoint.setRotation(rotation); rootJoint.updateFramesRecursively(); if (!initialized) { controller.initialize(timestamp); initialized = true; } controller.doControl(timestamp); if (controller.turnOutputOn()) { command.enableActuators(); } for (WandererJoint joint : WandererJoint.values) { OneDoFJoint oneDoFJoint = jointMap.get(joint); RawJointSensorDataHolder rawSensor = rawSensors.get(oneDoFJoint); AcsellJointCommand jointCommand = command.getAcsellJointCommand(joint); jointCommand.setTauDesired(oneDoFJoint.getTau(), 0.0, rawSensor); jointCommand.setDamping(oneDoFJoint.getKd()); } outputWriter.write(); if (visualizer != null) { visualizer.update(timestamp); } }