From 8642edd144242f061b493e843ed68d2805c5ddcf Mon Sep 17 00:00:00 2001 From: Anthony Thibault Date: Thu, 13 Sep 2018 18:18:23 -0700 Subject: [PATCH] Added acceleration limit filter to controller system --- interface/resources/controllers/vive.json | 9 +- .../src/controllers/impl/Filter.cpp | 2 + .../filters/AccelerationLimiterFilter.cpp | 163 ++++++++++++++++++ .../impl/filters/AccelerationLimiterFilter.h | 40 +++++ 4 files changed, 212 insertions(+), 2 deletions(-) create mode 100644 libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.cpp create mode 100644 libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.h diff --git a/interface/resources/controllers/vive.json b/interface/resources/controllers/vive.json index 8a7744efb3..442584a2ce 100644 --- a/interface/resources/controllers/vive.json +++ b/interface/resources/controllers/vive.json @@ -51,8 +51,13 @@ { "from": "Vive.RSCenter", "to": "Standard.RightPrimaryThumb" }, { "from": "Vive.RightApplicationMenu", "to": "Standard.RightSecondaryThumb" }, - { "from": "Vive.LeftHand", "to": "Standard.LeftHand"}, - { "from": "Vive.RightHand", "to": "Standard.RightHand"}, + { "from": "Vive.LeftHand", "to": "Standard.LeftHand", + "filters" : [{"type" : "accelerationLimiter", "rotationLimit" : 4000.0, "translationLimit": 200.0}] + }, + + { "from": "Vive.RightHand", "to": "Standard.RightHand", + "filters" : [{"type" : "accelerationLimiter", "rotationLimit" : 2000.0, "translationLimit": 100.0}] + }, { "from": "Vive.LeftFoot", "to" : "Standard.LeftFoot", diff --git a/libraries/controllers/src/controllers/impl/Filter.cpp b/libraries/controllers/src/controllers/impl/Filter.cpp index 6e6dc816d0..f230fb83dc 100644 --- a/libraries/controllers/src/controllers/impl/Filter.cpp +++ b/libraries/controllers/src/controllers/impl/Filter.cpp @@ -31,6 +31,7 @@ #include "filters/RotateFilter.h" #include "filters/LowVelocityFilter.h" #include "filters/ExponentialSmoothingFilter.h" +#include "filters/AccelerationLimiterFilter.h" using namespace controller; @@ -51,6 +52,7 @@ REGISTER_FILTER_CLASS_INSTANCE(PostTransformFilter, "postTransform") REGISTER_FILTER_CLASS_INSTANCE(RotateFilter, "rotate") REGISTER_FILTER_CLASS_INSTANCE(LowVelocityFilter, "lowVelocity") REGISTER_FILTER_CLASS_INSTANCE(ExponentialSmoothingFilter, "exponentialSmoothing") +REGISTER_FILTER_CLASS_INSTANCE(AccelerationLimiterFilter, "accelerationLimiter") const QString JSON_FILTER_TYPE = QStringLiteral("type"); const QString JSON_FILTER_PARAMS = QStringLiteral("params"); diff --git a/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.cpp b/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.cpp new file mode 100644 index 0000000000..1f63b28786 --- /dev/null +++ b/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.cpp @@ -0,0 +1,163 @@ +// +// Created by Anthony Thibault 2018/11/09 +// Copyright 2018 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + + +#include "AccelerationLimiterFilter.h" + +#include +#include +#include "../../UserInputMapper.h" +#include "../../Input.h" +#include +#include +#include + +static const QString JSON_ROTATION_LIMIT = QStringLiteral("rotationLimit"); +static const QString JSON_TRANSLATION_LIMIT = QStringLiteral("translationLimit"); + +static glm::vec3 angularVelFromDeltaRot(const glm::quat& deltaQ, float dt) { + // Measure the angular velocity of a delta rotation quaternion by using quaternion logarithm. + // The logarithm of a unit quternion returns the axis of rotation with a length of one half the angle of rotation in the imaginary part. + // The real part will be 0. Then we multiply it by 2 / dt. turning it into the angular velocity, (except for the extra w = 0 part). + glm::quat omegaQ((2.0f / dt) * glm::log(deltaQ)); + return glm::vec3(omegaQ.x, omegaQ.y, omegaQ.z); +} + +static glm::quat deltaRotFromAngularVel(const glm::vec3& omega, float dt) { + // Convert angular velocity into a delta quaternion by using quaternion exponent. + // The exponent of quaternion will return a delta rotation around the axis of the imaginary part, by twice the angle as determined by the length of that imaginary part. + // It is the inverse of the logarithm step in angularVelFromDeltaRot + glm::quat omegaQ(0.0f, omega.x, omega.y, omega.z); + return glm::exp((dt / 2.0f) * omegaQ); +} + +static glm::vec3 filterTranslation(const glm::vec3& x0, const glm::vec3& x1, const glm::vec3& x2, const glm::vec3& x3, float dt, const float aLimit) { + + // measure the linear velocities of this step and the previoius step + glm::vec3 v1 = (x3 - x1) / (2.0f * dt); + glm::vec3 v0 = (x2 - x0) / (2.0f * dt); + + // compute the acceleration + const glm::vec3 a = (v1 - v0) / dt; + + // clamp the acceleration if it is over the limit + float aLen = glm::length(a); + + if (aLen > aLimit) { + // Solve for a new `v1`, such that `a` does not exceed `aLimit` + // This combines two steps: + // 1) Computing a limited accelration in the direction of `a`, but with a magnitute of `aLimit`: + // `newA = a * (aLimit / aLen)` + // 2) Computing new `v1` + // `v1 = newA * dt + v0` + // We combine the scalars from step 1 and step 2 into a single term to avoid having to do multiple scalar-vec3 multiplies. + v1 = a * ((aLimit * dt) / aLen) + v0; + + // apply limited v1 to compute filtered x3 + return v1 * dt + x2; + } else { + // did not exceed limit, no filtering necesary + return x3; + } +} + +static glm::quat filterRotation(const glm::quat& q0In, const glm::quat& q1In, const glm::quat& q2In, const glm::quat& q3In, float dt, const float aLimit) { + + // ensure quaternions have the same polarity + glm::quat q0 = q0In; + glm::quat q1 = glm::dot(q0In, q1In) < 0.0f ? -q1In : q1In; + glm::quat q2 = glm::dot(q1In, q2In) < 0.0f ? -q2In : q2In; + glm::quat q3 = glm::dot(q2In, q3In) < 0.0f ? -q3In : q3In; + + // measure the angular velocities of this step and the previous step + glm::vec3 w1 = angularVelFromDeltaRot(q3 * glm::inverse(q1), 2.0f * dt); + glm::vec3 w0 = angularVelFromDeltaRot(q2 * glm::inverse(q0), 2.0f * dt); + + const glm::vec3 a = (w1 - w0) / dt; + + // clamp the acceleration if it is over the limit + float aLen = glm::length(a); + if (aLen > aLimit) { + // solve for a new w1, such that a does not exceed the accLimit + w1 = a * ((aLimit * dt) / aLen) + w0; + + // apply limited w1 to compute filtered q3 + return deltaRotFromAngularVel(w1, dt) * q2; + } else { + // did not exceed limit, no filtering necesary + return q3; + } +} + +namespace controller { + + Pose AccelerationLimiterFilter::apply(Pose value) const { + + if (value.isValid()) { + + // to perform filtering in sensor space, we need to compute the transformations. + auto userInputMapper = DependencyManager::get(); + const InputCalibrationData calibrationData = userInputMapper->getInputCalibrationData(); + glm::mat4 sensorToAvatarMat = glm::inverse(calibrationData.avatarMat) * calibrationData.sensorToWorldMat; + glm::mat4 avatarToSensorMat = glm::inverse(calibrationData.sensorToWorldMat) * calibrationData.avatarMat; + + // transform pose into sensor space. + Pose sensorValue = value.transform(avatarToSensorMat); + + if (_prevValid) { + + const float DELTA_TIME = 0.01111111f; + + sensorValue.translation = filterTranslation(_prevPos[0], _prevPos[1], _prevPos[2], sensorValue.translation, DELTA_TIME, _translationLimit); + sensorValue.rotation = filterRotation(_prevRot[0], _prevRot[1], _prevRot[2], sensorValue.rotation, DELTA_TIME, _rotationLimit); + + // remember previous values. + _prevPos[0] = _prevPos[1]; + _prevPos[1] = _prevPos[2]; + _prevPos[2] = sensorValue.translation; + _prevRot[0] = _prevRot[1]; + _prevRot[1] = _prevRot[2]; + _prevRot[2] = sensorValue.rotation; + + // transform back into avatar space + return sensorValue.transform(sensorToAvatarMat); + } else { + // initialize previous values with the current sample. + _prevPos[0] = sensorValue.translation; + _prevPos[1] = sensorValue.translation; + _prevPos[2] = sensorValue.translation; + _prevRot[0] = sensorValue.rotation; + _prevRot[1] = sensorValue.rotation; + _prevRot[2] = sensorValue.rotation; + _prevValid = true; + + // no previous value to smooth with, so return value unchanged + return value; + } + } else { + // mark previous poses as invalid. + _prevValid = false; + + // return invalid value unchanged + return value; + } + } + + bool AccelerationLimiterFilter::parseParameters(const QJsonValue& parameters) { + if (parameters.isObject()) { + auto obj = parameters.toObject(); + if (obj.contains(JSON_ROTATION_LIMIT) && obj.contains(JSON_TRANSLATION_LIMIT)) { + _rotationLimit = (float)obj[JSON_ROTATION_LIMIT].toDouble(); + _translationLimit = (float)obj[JSON_TRANSLATION_LIMIT].toDouble(); + return true; + } + } + return false; + } + +} diff --git a/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.h b/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.h new file mode 100644 index 0000000000..22a1ca530b --- /dev/null +++ b/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.h @@ -0,0 +1,40 @@ +// +// Created by Anthony Thibault 2018/11/09 +// Copyright 2018 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_Controllers_Filters_Acceleration_Limiter_h +#define hifi_Controllers_Filters_Acceleration_Limiter_h + +#include "../Filter.h" + +namespace controller { + + class AccelerationLimiterFilter : public Filter { + REGISTER_FILTER_CLASS(AccelerationLimiterFilter); + + public: + AccelerationLimiterFilter() {} + AccelerationLimiterFilter(float rotationLimit, float translationLimit) : + _rotationLimit(rotationLimit), + _translationLimit(translationLimit) {} + + float apply(float value) const override { return value; } + Pose apply(Pose value) const override; + bool parseParameters(const QJsonValue& parameters) override; + + private: + float _rotationLimit { FLT_MAX }; + float _translationLimit { FLT_MAX }; + + mutable glm::vec3 _prevPos[3]; // sensor space + mutable glm::quat _prevRot[3]; // sensor space + mutable bool _prevValid { false }; + }; + +} + +#endif