public void startComputation() {
for (OneDoFJoint joint : oneDoFJoints) {
if (joint == null) throw new RuntimeException();
ControlFlowInputPort<double[]> positionSensorPort = positionSensorInputPorts.get(joint);
ControlFlowInputPort<double[]> velocitySensorPort = velocitySensorInputPorts.get(joint);
double positionSensorData = positionSensorPort.getData()[0];
double velocitySensorData = velocitySensorPort.getData()[0];
joint.setQ(positionSensorData);
joint.setQd(velocitySensorData);
joint.setQdd(joint.getQddDesired());
}
// TODO: Does it make sense to do this yet if the orientation of the pelvis isn't known yet?
FullInverseDynamicsStructure inverseDynamicsStructure =
inverseDynamicsStructureOutputPort.getData();
TwistCalculator twistCalculator = inverseDynamicsStructure.getTwistCalculator();
SpatialAccelerationCalculator spatialAccelerationCalculator =
inverseDynamicsStructure.getSpatialAccelerationCalculator();
twistCalculator.getRootBody().updateFramesRecursively();
twistCalculator.compute();
spatialAccelerationCalculator.compute();
inverseDynamicsStructureOutputPort.setData(inverseDynamicsStructure);
}
