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);
}
}
