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);
}
}
private WandererSingleThreadedController(
WandererRobotModel robotModel,
PriorityParameters priorityParameters,
RobotVisualizer visualizer,
WandererController wandererController,
YoVariableRegistry registry) {
super(priorityParameters);
this.state = new WandererState(0.001, registry);
this.reader = new UDPAcsellStateReader(state);
this.visualizer = visualizer;
this.command = new WandererCommand(registry);
this.controller = wandererController;
this.outputWriter = new UDPAcsellOutputWriter(command);
FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel();
rootJoint = fullRobotModel.getRootJoint();
for (WandererJoint joint : WandererJoint.values) {
OneDoFJoint oneDoFJoint = fullRobotModel.getOneDoFJointByName(joint.getSdfName());
if (oneDoFJoint == null) {
throw new RuntimeException("Unknown joint: " + joint.getSdfName());
}
jointMap.put(joint, oneDoFJoint);
}
this.rawSensors = new RawJointSensorDataHolderMap(fullRobotModel);
this.controller.setFullRobotModel(fullRobotModel);
if (controller.getYoVariableRegistry() != null) {
registry.addChild(controller.getYoVariableRegistry());
}
if (visualizer != null) {
visualizer.setMainRegistry(registry, fullRobotModel, null);
}
outputWriter.connect(new WandererNetworkParameters());
}