Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-08-07 00:44:38 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Added deceleration limit to AccelerationLimiterFilter

Browse source
This commit is contained in:
Anthony Thibault 2018-09-18 11:53:17 -07:00
parent 8642edd144
commit d15cc86735
3 changed files with 37 additions and 21 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
interface/resources/controllers/vive.json
View file

@ -52,11 +52,15 @@
{ "from": "Vive.RightApplicationMenu", "to": "Standard.RightSecondaryThumb" }, { "from": "Vive.RightApplicationMenu", "to": "Standard.RightSecondaryThumb" },
{ "from": "Vive.LeftHand", "to": "Standard.LeftHand", { "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", { "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}]
}, },
{ {

41
libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.cpp
View file

@ -17,8 +17,10 @@
#include <QDebug> #include <QDebug>
#include <StreamUtils.h> #include <StreamUtils.h>
static const QString JSON_ROTATION_LIMIT = QStringLiteral("rotationLimit"); static const QString JSON_ROTATION_ACCELERATION_LIMIT = QStringLiteral("rotationAccelerationLimit");
static const QString JSON_TRANSLATION_LIMIT = QStringLiteral("translationLimit"); 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) { static glm::vec3 angularVelFromDeltaRot(const glm::quat& deltaQ, float dt) {
// Measure the angular velocity of a delta rotation quaternion by using quaternion logarithm. // 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); 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 // measure the linear velocities of this step and the previoius step
glm::vec3 v1 = (x3 - x1) / (2.0f * dt); 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 // clamp the acceleration if it is over the limit
float aLen = glm::length(a); 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` // Solve for a new `v1`, such that `a` does not exceed `aLimit`
// This combines two steps: // This combines two steps:
// 1) Computing a limited accelration in the direction of `a`, but with a magnitute of `aLimit`: // 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` // 2) Computing new `v1`
// `v1 = newA * dt + v0` // `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. // 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 // apply limited v1 to compute filtered x3
return v1 * dt + x2; 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 // ensure quaternions have the same polarity
glm::quat q0 = q0In; 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); glm::vec3 w0 = angularVelFromDeltaRot(q2 * glm::inverse(q0), 2.0f * dt);
const glm::vec3 a = (w1 - w0) / 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 // clamp the acceleration if it is over the limit
float aLen = glm::length(a); if (aLen > limit) {
if (aLen > aLimit) {
// solve for a new w1, such that a does not exceed the accLimit // 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 // apply limited w1 to compute filtered q3
return deltaRotFromAngularVel(w1, dt) * q2; return deltaRotFromAngularVel(w1, dt) * q2;
@ -113,8 +121,10 @@ namespace controller {
const float DELTA_TIME = 0.01111111f; const float DELTA_TIME = 0.01111111f;
sensorValue.translation = filterTranslation(_prevPos[0], _prevPos[1], _prevPos[2], sensorValue.translation, DELTA_TIME, _translationLimit); sensorValue.translation = filterTranslation(_prevPos[0], _prevPos[1], _prevPos[2], sensorValue.translation,
sensorValue.rotation = filterRotation(_prevRot[0], _prevRot[1], _prevRot[2], sensorValue.rotation, DELTA_TIME, _rotationLimit); DELTA_TIME, _translationAccelerationLimit, _translationDecelerationLimit);
sensorValue.rotation = filterRotation(_prevRot[0], _prevRot[1], _prevRot[2], sensorValue.rotation,
DELTA_TIME, _rotationAccelerationLimit, _rotationDecelerationLimit);
// remember previous values. // remember previous values.
_prevPos[0] = _prevPos[1]; _prevPos[0] = _prevPos[1];
@ -151,9 +161,12 @@ namespace controller {
bool AccelerationLimiterFilter::parseParameters(const QJsonValue& parameters) { bool AccelerationLimiterFilter::parseParameters(const QJsonValue& parameters) {
if (parameters.isObject()) { if (parameters.isObject()) {
auto obj = parameters.toObject(); auto obj = parameters.toObject();
if (obj.contains(JSON_ROTATION_LIMIT) && obj.contains(JSON_TRANSLATION_LIMIT)) { if (obj.contains(JSON_ROTATION_ACCELERATION_LIMIT) && obj.contains(JSON_ROTATION_DECELERATION_LIMIT) &&
_rotationLimit = (float)obj[JSON_ROTATION_LIMIT].toDouble(); obj.contains(JSON_TRANSLATION_ACCELERATION_LIMIT) && obj.contains(JSON_TRANSLATION_DECELERATION_LIMIT)) {
_translationLimit = (float)obj[JSON_TRANSLATION_LIMIT].toDouble(); _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; return true;
} }
} }

9
libraries/controllers/src/controllers/impl/filters/AccelerationLimiterFilter.h
View file

@ -18,17 +18,16 @@ namespace controller {
public: public:
AccelerationLimiterFilter() {} AccelerationLimiterFilter() {}
AccelerationLimiterFilter(float rotationLimit, float translationLimit) :
_rotationLimit(rotationLimit),
_translationLimit(translationLimit) {}
float apply(float value) const override { return value; } float apply(float value) const override { return value; }
Pose apply(Pose value) const override; Pose apply(Pose value) const override;
bool parseParameters(const QJsonValue& parameters) override; bool parseParameters(const QJsonValue& parameters) override;
private: private:
float _rotationLimit { FLT_MAX }; float _rotationAccelerationLimit { FLT_MAX };
float _translationLimit { FLT_MAX }; float _rotationDecelerationLimit { FLT_MAX };
float _translationAccelerationLimit { FLT_MAX };
float _translationDecelerationLimit { FLT_MAX };
mutable glm::vec3 _prevPos[3]; // sensor space mutable glm::vec3 _prevPos[3]; // sensor space
mutable glm::quat _prevRot[3]; // sensor space mutable glm::quat _prevRot[3]; // sensor space