From d15cc86735cc65a2fafaef4e2cd71040efb07db2 Mon Sep 17 00:00:00 2001 From: Anthony Thibault Date: Tue, 18 Sep 2018 11:53:17 -0700 Subject: [PATCH] Added deceleration limit to AccelerationLimiterFilter --- interface/resources/controllers/vive.json | 8 +++- .../filters/AccelerationLimiterFilter.cpp | 41 ++++++++++++------- .../impl/filters/AccelerationLimiterFilter.h | 9 ++-- 3 files changed, 37 insertions(+), 21 deletions(-) diff --git a/interface/resources/controllers/vive.json b/interface/resources/controllers/vive.json index 442584a2ce..e737fec594 100644 --- a/interface/resources/controllers/vive.json +++ b/interface/resources/controllers/vive.json @@ -52,11 +52,15 @@ { "from": "Vive.RightApplicationMenu", "to": "Standard.RightSecondaryThumb" }, { "from": "Vive.LeftHand", "to": "Standard.LeftHand", - "filters" : [{"type" : "accelerationLimiter", "rotationLimit" : 4000.0, "translationLimit": 200.0}] + "filters" : [{"type" : "accelerationLimiter", + "rotationAccelerationLimit" : 4000.0, "rotationDecelerationLimit" : 8000.0, + "translationAccelerationLimit": 200.0, "translationDecelerationLimit": 400.0}] }, { "from": "Vive.RightHand", "to": "Standard.RightHand", - "filters" : [{"type" : "accelerationLimiter", "rotationLimit" : 2000.0, "translationLimit": 100.0}] + "filters" : [{"type" : "accelerationLimiter", + "rotationAccelerationLimit" : 2000.0, "rotationDecelerationLimit" : 4000.0, + "translationAccelerationLimit": 100.0, "translationDecelerationLimit": 200.0}] }, { diff --git a/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.cpp b/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.cpp index 1f63b28786..234dff1c65 100644 --- a/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.cpp +++ b/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.cpp @@ -17,8 +17,10 @@ #include #include -static const QString JSON_ROTATION_LIMIT = QStringLiteral("rotationLimit"); -static const QString JSON_TRANSLATION_LIMIT = QStringLiteral("translationLimit"); +static const QString JSON_ROTATION_ACCELERATION_LIMIT = QStringLiteral("rotationAccelerationLimit"); +static const QString JSON_ROTATION_DECELERATION_LIMIT = QStringLiteral("rotationDecelerationLimit"); +static const QString JSON_TRANSLATION_ACCELERATION_LIMIT = QStringLiteral("translationAccelerationLimit"); +static const QString JSON_TRANSLATION_DECELERATION_LIMIT = QStringLiteral("translationDecelerationLimit"); static glm::vec3 angularVelFromDeltaRot(const glm::quat& deltaQ, float dt) { // Measure the angular velocity of a delta rotation quaternion by using quaternion logarithm. @@ -36,7 +38,7 @@ static glm::quat deltaRotFromAngularVel(const glm::vec3& omega, float dt) { 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) { +static glm::vec3 filterTranslation(const glm::vec3& x0, const glm::vec3& x1, const glm::vec3& x2, const glm::vec3& x3, float dt, const float accLimit, const float decLimit) { // measure the linear velocities of this step and the previoius step glm::vec3 v1 = (x3 - x1) / (2.0f * dt); @@ -48,7 +50,10 @@ static glm::vec3 filterTranslation(const glm::vec3& x0, const glm::vec3& x1, con // clamp the acceleration if it is over the limit float aLen = glm::length(a); - if (aLen > aLimit) { + // pick limit based on if we are accelerating or decelerating. + float limit = glm::length(v1) > glm::length(v0) ? accLimit : decLimit; + + if (aLen > limit) { // 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`: @@ -56,7 +61,7 @@ static glm::vec3 filterTranslation(const glm::vec3& x0, const glm::vec3& x1, con // 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; + v1 = a * ((limit * dt) / aLen) + v0; // apply limited v1 to compute filtered x3 return v1 * dt + x2; @@ -66,7 +71,7 @@ static glm::vec3 filterTranslation(const glm::vec3& x0, const glm::vec3& x1, con } } -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) { +static glm::quat filterRotation(const glm::quat& q0In, const glm::quat& q1In, const glm::quat& q2In, const glm::quat& q3In, float dt, const float accLimit, const float decLimit) { // ensure quaternions have the same polarity glm::quat q0 = q0In; @@ -79,12 +84,15 @@ static glm::quat filterRotation(const glm::quat& q0In, const glm::quat& q1In, co glm::vec3 w0 = angularVelFromDeltaRot(q2 * glm::inverse(q0), 2.0f * dt); const glm::vec3 a = (w1 - w0) / dt; + float aLen = glm::length(a); + + // pick limit based on if we are accelerating or decelerating. + float limit = glm::length(w1) > glm::length(w0) ? accLimit : decLimit; // clamp the acceleration if it is over the limit - float aLen = glm::length(a); - if (aLen > aLimit) { + if (aLen > limit) { // solve for a new w1, such that a does not exceed the accLimit - w1 = a * ((aLimit * dt) / aLen) + w0; + w1 = a * ((limit * dt) / aLen) + w0; // apply limited w1 to compute filtered q3 return deltaRotFromAngularVel(w1, dt) * q2; @@ -113,8 +121,10 @@ namespace controller { 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); + sensorValue.translation = filterTranslation(_prevPos[0], _prevPos[1], _prevPos[2], sensorValue.translation, + DELTA_TIME, _translationAccelerationLimit, _translationDecelerationLimit); + sensorValue.rotation = filterRotation(_prevRot[0], _prevRot[1], _prevRot[2], sensorValue.rotation, + DELTA_TIME, _rotationAccelerationLimit, _rotationDecelerationLimit); // remember previous values. _prevPos[0] = _prevPos[1]; @@ -151,9 +161,12 @@ namespace controller { 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(); + if (obj.contains(JSON_ROTATION_ACCELERATION_LIMIT) && obj.contains(JSON_ROTATION_DECELERATION_LIMIT) && + obj.contains(JSON_TRANSLATION_ACCELERATION_LIMIT) && obj.contains(JSON_TRANSLATION_DECELERATION_LIMIT)) { + _rotationAccelerationLimit = (float)obj[JSON_ROTATION_ACCELERATION_LIMIT].toDouble(); + _rotationDecelerationLimit = (float)obj[JSON_ROTATION_DECELERATION_LIMIT].toDouble(); + _translationAccelerationLimit = (float)obj[JSON_TRANSLATION_ACCELERATION_LIMIT].toDouble(); + _translationDecelerationLimit = (float)obj[JSON_TRANSLATION_DECELERATION_LIMIT].toDouble(); return true; } } diff --git a/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.h b/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.h index 22a1ca530b..6a6c7f8c33 100644 --- a/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.h +++ b/libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.h @@ -18,17 +18,16 @@ namespace controller { 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 }; + float _rotationAccelerationLimit { FLT_MAX }; + float _rotationDecelerationLimit { FLT_MAX }; + float _translationAccelerationLimit { FLT_MAX }; + float _translationDecelerationLimit { FLT_MAX }; mutable glm::vec3 _prevPos[3]; // sensor space mutable glm::quat _prevRot[3]; // sensor space