private void computeTwistsAndSpatialAccelerations() { spatialAccelerationCalculator.compute(); }
public void updateInternalState() { twistCalculator.compute(); spatialAccelerationCalculator.compute(); }
public void doControl() { boolean updateRootJoints = true; generator.updateInverseDynamicsRobotModelFromRobot(updateRootJoints, false); perfectTwistCalculator.compute(); perfectSpatialAccelerationCalculator.compute(); updateGroundTruth(); centerOfMassAccelerationFromFullRobotModelStealer.run(); angularAccelerationFromRobotStealer.run(); }
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 void run() { FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); ReferenceFrame estimationFrame = inverseDynamicsStructure.getEstimationFrame(); centerOfMassCalculator.compute(); updateRootJointConfiguration(rootJoint, estimationFrame); rootJoint.getFrameAfterJoint().update(); TwistCalculator twistCalculator = inverseDynamicsStructure.getTwistCalculator(); SpatialAccelerationCalculator spatialAccelerationCalculator = inverseDynamicsStructure.getSpatialAccelerationCalculator(); updateRootJointTwistAndSpatialAcceleration(twistCalculator, spatialAccelerationCalculator); twistCalculator.compute(); spatialAccelerationCalculator.compute(); }
spatialAccelerationCalculator.compute();
spatialAccelerationCalculator.compute();
spatialAccelerationCalculator.compute(); computeCoMVelocity(centroidalMomentumMatrixDense);