public void updateFramesRecursively() { elevator.updateFramesRecursively(); }
private void updateFrames() { localJoints[0].getPredecessor().updateFramesRecursively(); }
private void updateRobotFrames() { robotArmJoints[0].getPredecessor().updateFramesRecursively(); }
public static void createRandomChainRobot(String prefix, List<RevoluteJoint> revoluteJointsToPack, RigidBody rootBody, Vector3d[] jointAxes, Random random) { RigidBody currentIDBody = rootBody; for (int i = 0; i < jointAxes.length; i++) { RevoluteJoint currentIDJoint = addRandomRevoluteJoint(prefix + "joint" + i, jointAxes[i], random, currentIDBody); currentIDBody = addRandomRigidBody(prefix + "body" + i, random, currentIDJoint); revoluteJointsToPack.add(currentIDJoint); } rootBody.updateFramesRecursively(); }
@Override public final void updateFramesRecursively() { getFrameAfterJoint().update(); if (successor != null) { successor.updateFramesRecursively(); } }
public void compare() { for (MassMatrixCalculator massMatrixCalculator : massMatrixCalculators) { long startTime = System.nanoTime(); int nIterations = 10000; for (int i = 0; i < nIterations; i++) { ScrewTestTools.setRandomPositions(joints, random); elevator.updateFramesRecursively(); massMatrixCalculator.compute(); } long endTime = System.nanoTime(); double timeTaken = (endTime - startTime) / (1e9); double timeTakenPerIteration = timeTaken / nIterations; System.out.println("Time taken per iteration: " + timeTakenPerIteration + " s"); } }
private void updateReferenceFrames() { if (referenceFrames != null) { referenceFrames.updateFrames(); } else { rootJoint.getPredecessor().updateFramesRecursively(); } }
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 setRandomPositionsAndVelocities(Random random) { ScrewTestTools.setRandomPositionAndOrientation(getRootJoint(), random); ScrewTestTools.setRandomVelocity(getRootJoint(), random); ScrewTestTools.setRandomPositions(getRevoluteJoints(), random); setRandomVelocities(getRevoluteJoints(), random); getElevator().updateFramesRecursively(); }
private void updateFullRobotModel(RobotConfigurationData robotConfigurationData, FullRobotModel model, ForceSensorDataHolder forceSensorDataHolder) { FullRobotModelCache fullRobotModelCache = getFullRobotModelCache(model); FloatingInverseDynamicsJoint rootJoint = model.getRootJoint(); if (robotConfigurationData.jointNameHash != fullRobotModelCache.jointNameHash) { System.out.println(robotConfigurationData.jointNameHash); System.out.println(fullRobotModelCache.jointNameHash); throw new RuntimeException("Joint names do not match for RobotConfigurationData"); } float[] newJointAngles = robotConfigurationData.getJointAngles(); for (int i = 0; i < newJointAngles.length; i++) { fullRobotModelCache.allJoints[i].setQ(newJointAngles[i]); } Vector3f translation = robotConfigurationData.getPelvisTranslation(); rootJoint.setPosition(translation.getX(), translation.getY(), translation.getZ()); Quat4f orientation = robotConfigurationData.getPelvisOrientation(); rootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW()); rootJoint.getPredecessor().updateFramesRecursively(); if (forceSensorDataHolder != null) { for (int i = 0; i < forceSensorDataHolder.getForceSensorDefinitions().size(); i++) { forceSensorDataHolder.get(forceSensorDataHolder.getForceSensorDefinitions().get(i)).setWrench( robotConfigurationData.getMomentAndForceVectorForSensor(i)); } } }
Quat4f orientation = robotConfigurationData.getPelvisOrientation(); rootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW()); rootJoint.getPredecessor().updateFramesRecursively();
Quat4f orientation = robotConfigurationData.getPelvisOrientation(); rootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW()); rootJoint.getPredecessor().updateFramesRecursively();
rootBody.updateFramesRecursively(); twistCalculator.compute(); spatialAccelerationCalculator.compute();