@Override public void wasRewound() { FloatingInverseDynamicsJoint rootJoint = fullRobotModel.getRootJoint(); yoRootJointTranslation.get(rootJointTranslation); rootJoint.setPosition(rootJointTranslation); yoRootJointRotation.get(rootJointRotation); rootJoint.setRotation(rootJointRotation); } }
public void updateForFrozenState() { // Keep setting the position so the localization updater works properly. yoRootJointPosition.get(tempRootJointTranslation); rootJoint.setPosition(tempRootJointTranslation); // Set the rootJoint twist to zero. rootJoint.getJointTwist(rootJointTwist); rootJointTwist.setToZero(); rootJoint.setJointTwist(rootJointTwist); rootJointFrame.update(); twistCalculator.compute(); updateCoMState(); }
private void updateRootJoint() { yoRootJointPosition.get(tempRootJointTranslation); rootJoint.setPosition(tempRootJointTranslation); tempVelocity.setIncludingFrame(rootJointVelocity); rootJoint.getJointTwist(rootJointTwist); tempVelocity.changeFrame(rootJointTwist.getExpressedInFrame()); rootJointTwist.setLinearPart(tempVelocity); rootJoint.setJointTwist(rootJointTwist); rootJointFrame.update(); twistCalculator.compute(); }
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)); } } }
public void updateFullRobotModel(KinematicsToolboxOutputStatus solution) { if (jointsHashCode != solution.jointNameHash) throw new RuntimeException("Hashes are different."); for (int i = 0; i < oneDoFJoints.length; i++) { float q = solution.getJointAngles()[i]; OneDoFJoint joint = oneDoFJoints[i]; joint.setQ(q); } Vector3f translation = solution.getPelvisTranslation(); rootJoint.setPosition(translation.getX(), translation.getY(), translation.getZ()); Quat4f orientation = solution.getPelvisOrientation(); rootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW()); fullRobotModelToUseForConversion.updateFrames(); }
rootJoint.setPosition(translation.getX(), translation.getY(), translation.getZ()); Quat4f orientation = robotConfigurationData.getPelvisOrientation(); rootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW());
rootJoint.setPosition(translation.getX(), translation.getY(), translation.getZ()); Quat4f orientation = robotConfigurationData.getPelvisOrientation(); rootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW());
desiredRootJoint.setPosition(translation.getX(), translation.getY(), translation.getZ()); Quat4f orientation = robotConfigurationData.getPelvisOrientation(); desiredRootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW());
desiredRootJoint.setPosition(translation.getX(), translation.getY(), translation.getZ()); Quat4f orientation = robotConfigurationData.getPelvisOrientation(); desiredRootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW());