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);
}
public JointStateFullRobotModelUpdater(
ControlFlowGraph controlFlowGraph,
Map<OneDoFJoint, ControlFlowOutputPort<double[]>> jointPositionSensors,
Map<OneDoFJoint, ControlFlowOutputPort<double[]>> jointVelocitySensors,
FullInverseDynamicsStructure inverseDynamicsStructure) {
InverseDynamicsJoint[] joints =
ScrewTools.computeSupportAndSubtreeJoints(
inverseDynamicsStructure.getRootJoint().getSuccessor());
this.oneDoFJoints = ScrewTools.filterJoints(joints, OneDoFJoint.class);
this.inverseDynamicsStructureOutputPort =
createOutputPort("inverseDynamicsStructureOutputPort");
inverseDynamicsStructureOutputPort.setData(inverseDynamicsStructure);
for (OneDoFJoint oneDoFJoint : oneDoFJoints) {
if (jointPositionSensors.get(oneDoFJoint) == null) {
throw new RuntimeException(
"positionSensorPorts.get(oneDoFJoint) == null. oneDoFJoint = " + oneDoFJoint);
}
}
for (OneDoFJoint oneDoFJoint : oneDoFJoints) {
ControlFlowOutputPort<double[]> positionSensorOutputPort =
jointPositionSensors.get(oneDoFJoint);
ControlFlowInputPort<double[]> positionSensorInputPort =
createInputPort("positionSensorInputPort");
positionSensorInputPorts.put(oneDoFJoint, positionSensorInputPort);
controlFlowGraph.connectElements(positionSensorOutputPort, positionSensorInputPort);
ControlFlowOutputPort<double[]> velocitySensorOutputPort =
jointVelocitySensors.get(oneDoFJoint);
ControlFlowInputPort<double[]> velocitySensorInputPort =
createInputPort("velocitySensorInputPort");
velocitySensorInputPorts.put(oneDoFJoint, velocitySensorInputPort);
controlFlowGraph.connectElements(velocitySensorOutputPort, velocitySensorInputPort);
}
}