@Override public void wasRewound() { FloatingInverseDynamicsJoint rootJoint = fullRobotModel.getRootJoint(); yoRootJointTranslation.get(rootJointTranslation); rootJoint.setPosition(rootJointTranslation); yoRootJointRotation.get(rootJointRotation); rootJoint.setRotation(rootJointRotation); } }
@Override public void updateRootJointOrientationAndAngularVelocity() { yoRootJointFrameOrientation.getQuaternion(rootJointOrientation); rootJoint.setRotation(rootJointOrientation); rootJoint.getJointTwist(twistRootBodyRelativeToWorld); twistRootBodyRelativeToWorld.setToZero(); rootJoint.setJointTwist(twistRootBodyRelativeToWorld); }
private void updateRootJointRotation(FloatingInverseDynamicsJoint rootJoint, FrameOrientation estimationLinkOrientation, ReferenceFrame estimationFrame) { tempOrientationEstimatinLink.setIncludingFrame(estimationLinkOrientation); computeEstimationLinkToWorldTransform(tempEstimationLinkToWorld, tempOrientationEstimatinLink); computeRootJointToWorldTransform(rootJoint, estimationFrame, tempRootJointToWorld, tempEstimationLinkToWorld); Matrix3d rootJointRotation = new Matrix3d(); tempRootJointToWorld.getRotation(rootJointRotation); rootJoint.setRotation(rootJointRotation); }
private void updateRootJointRotation() { // R_{measurementFrame}^{world} imuProcessedOutput.getOrientationMeasurement(orientationMeasurement); transformFromMeasurementFrameToWorld.setRotationAndZeroTranslation(orientationMeasurement); // R_{root}^{measurementFrame} rootJointFrame.getTransformToDesiredFrame(transformFromRootJointFrameToMeasurementFrame, measurementFrame); // R_{root}^{world} = R_{estimationLink}^{world} * R_{root}^{estimationLink} transformFromRootJointFrameToWorld.multiply(transformFromMeasurementFrameToWorld, transformFromRootJointFrameToMeasurementFrame); transformFromRootJointFrameToWorld.getRotation(rotationFromRootJointFrameToWorld); rotationFromRootJointFrameToWorld.mul(rotationFrozenOffset, rotationFromRootJointFrameToWorld); rootJoint.setRotation(rotationFromRootJointFrameToWorld); rootJointFrame.update(); if (imuYawDriftEstimator != null) { imuYawDriftEstimator.update(); yawBiasMatrix.rotZ(imuYawDriftEstimator.getYawBiasInWorldFrame()); yawBiasMatrix.transpose(); rotationFromRootJointFrameToWorld.mul(yawBiasMatrix, rotationFromRootJointFrameToWorld); } rootJoint.setRotation(rotationFromRootJointFrameToWorld); rootJointFrame.update(); }
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.getPredecessor().updateFramesRecursively();
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();
@Override public void initializeForFrozenState() { rotationFrozenOffset.setIdentity(); // R_{measurementFrame}^{world} imuProcessedOutput.getOrientationMeasurement(orientationMeasurement); transformFromMeasurementFrameToWorld.setRotationAndZeroTranslation(orientationMeasurement); // R_{root}^{measurementFrame} rootJointFrame.getTransformToDesiredFrame(transformFromRootJointFrameToMeasurementFrame, measurementFrame); // R_{root}^{world} = R_{estimationLink}^{world} * R_{root}^{estimationLink} transformFromRootJointFrameToWorld.multiply(transformFromMeasurementFrameToWorld, transformFromRootJointFrameToMeasurementFrame); transformFromRootJointFrameToWorld.getRotation(rotationFromRootJointFrameToWorld); double initialYaw = RotationTools.computeYaw(rotationFromRootJointFrameToWorld); rootJointYawOffsetFromFrozenState.set(initialYaw); rotationFrozenOffset.rotZ(initialYaw); yoRootJointFrameQuaternion.setToZero(); yoRootJointFrameOrientation.setToZero(); yoRootJointFrameQuaternion.get(rotationFromRootJointFrameToWorld); rootJoint.setRotation(rotationFromRootJointFrameToWorld); // Set the rootJoint twist to zero. rootJoint.getJointTwist(twistRootBodyRelativeToWorld); twistRootBodyRelativeToWorld.setToZero(); rootJoint.setJointTwist(twistRootBodyRelativeToWorld); }
@Override public void updateForFrozenState() { // R_{measurementFrame}^{world} imuProcessedOutput.getOrientationMeasurement(orientationMeasurement); transformFromMeasurementFrameToWorld.setRotationAndZeroTranslation(orientationMeasurement); // R_{root}^{measurementFrame} rootJointFrame.getTransformToDesiredFrame(transformFromRootJointFrameToMeasurementFrame, measurementFrame); // R_{root}^{world} = R_{estimationLink}^{world} * R_{root}^{estimationLink} transformFromRootJointFrameToWorld.multiply(transformFromMeasurementFrameToWorld, transformFromRootJointFrameToMeasurementFrame); transformFromRootJointFrameToWorld.getRotation(rotationFromRootJointFrameToWorld); yoRootJointFrameQuaternion.getYawPitchRoll(lastComputedYawPitchRoll); double currentYaw = RotationTools.computeYaw(rotationFromRootJointFrameToWorld); double yawDifference = AngleTools.computeAngleDifferenceMinusPiToPi(lastComputedYawPitchRoll[0], currentYaw); rootJointYawOffsetFromFrozenState.set(yawDifference); rotationFrozenOffset.rotZ(yawDifference); // Keep setting the orientation so that the localization updater works properly. yoRootJointFrameQuaternion.get(rotationFromRootJointFrameToWorld); rootJoint.setRotation(rotationFromRootJointFrameToWorld); // Set the rootJoint twist to zero. rootJoint.getJointTwist(twistRootBodyRelativeToWorld); twistRootBodyRelativeToWorld.setToZero(); rootJoint.setJointTwist(twistRootBodyRelativeToWorld); updateViz(); }
desiredRootJoint.setPosition(translation.getX(), translation.getY(), translation.getZ()); Quat4f orientation = robotConfigurationData.getPelvisOrientation(); desiredRootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW()); desiredRootJoint.setVelocity(zeroVelocityMatrix, 0);
desiredRootJoint.setPosition(translation.getX(), translation.getY(), translation.getZ()); Quat4f orientation = robotConfigurationData.getPelvisOrientation(); desiredRootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW()); desiredRootJoint.setVelocity(zeroVelocityMatrix, 0);