From a045a87cca07c6e36d5ddc44fd4e26bd40827a3d Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 14 Mar 2014 11:11:00 -0700 Subject: [PATCH] Remove arm stretching, add sixense calibration --- interface/src/BuckyBalls.cpp | 1 + .../src/ControllerScriptingInterface.cpp | 1 + interface/src/avatar/SkeletonModel.cpp | 42 +--- interface/src/avatar/SkeletonModel.h | 4 - interface/src/devices/SixenseManager.cpp | 222 +++++++++++++++--- interface/src/devices/SixenseManager.h | 37 ++- libraries/avatars/src/HandData.cpp | 8 +- libraries/avatars/src/HandData.h | 20 +- libraries/shared/src/SharedUtil.h | 7 +- 9 files changed, 244 insertions(+), 98 deletions(-) diff --git a/interface/src/BuckyBalls.cpp b/interface/src/BuckyBalls.cpp index 14d4d0f1a8..8e489db74b 100644 --- a/interface/src/BuckyBalls.cpp +++ b/interface/src/BuckyBalls.cpp @@ -10,6 +10,7 @@ #include "Util.h" #include "world.h" +#include "devices/SixenseManager.h" const int NUM_ELEMENTS = 3; const float RANGE_BBALLS = 0.5f; diff --git a/interface/src/ControllerScriptingInterface.cpp b/interface/src/ControllerScriptingInterface.cpp index b60615f124..f5ab1653f5 100644 --- a/interface/src/ControllerScriptingInterface.cpp +++ b/interface/src/ControllerScriptingInterface.cpp @@ -8,6 +8,7 @@ #include #include "Application.h" +#include "devices/SixenseManager.h" #include "ControllerScriptingInterface.h" ControllerScriptingInterface::ControllerScriptingInterface() : diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index f405358710..7edf48602e 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -123,6 +123,7 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector& fingerJoin return; } const FBXGeometry& geometry = _geometry->getFBXGeometry(); + setJointPosition(jointIndex, palm.getPosition()); float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f; glm::quat palmRotation; getJointRotation(jointIndex, palmRotation, true); @@ -154,7 +155,6 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector& fingerJoin // no point in continuing if there are no fingers if (palm.getNumFingers() == 0 || fingerJointIndices.isEmpty()) { - stretchArm(jointIndex, palm.getPosition()); return; } @@ -172,8 +172,6 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector& fingerJoin setJointRotation(fingerJointIndex, rotationBetween(palmRotation * jointVector, fingerVector) * palmRotation, true); } - - stretchArm(jointIndex, palm.getPosition()); } void SkeletonModel::updateJointState(int index) { @@ -200,41 +198,3 @@ void SkeletonModel::maybeUpdateLeanRotation(const JointState& parentState, const glm::normalize(inverse * axes[0])) * joint.rotation; } -void SkeletonModel::stretchArm(int jointIndex, const glm::vec3& position) { - // find out where the hand is pointing - glm::quat handRotation; - getJointRotation(jointIndex, handRotation, true); - const FBXGeometry& geometry = _geometry->getFBXGeometry(); - glm::vec3 forwardVector(jointIndex == geometry.rightHandJointIndex ? -1.0f : 1.0f, 0.0f, 0.0f); - glm::vec3 handVector = handRotation * forwardVector; - - // align elbow with hand - const FBXJoint& joint = geometry.joints.at(jointIndex); - if (joint.parentIndex == -1) { - return; - } - glm::quat elbowRotation; - getJointRotation(joint.parentIndex, elbowRotation, true); - applyRotationDelta(joint.parentIndex, rotationBetween(elbowRotation * forwardVector, handVector), false); - - // set position according to normal length - float scale = extractUniformScale(_scale); - glm::vec3 handPosition = position - _translation; - glm::vec3 elbowPosition = handPosition - handVector * joint.distanceToParent * scale; - - // set shoulder orientation to point to elbow - const FBXJoint& parentJoint = geometry.joints.at(joint.parentIndex); - if (parentJoint.parentIndex == -1) { - return; - } - glm::quat shoulderRotation; - getJointRotation(parentJoint.parentIndex, shoulderRotation, true); - applyRotationDelta(parentJoint.parentIndex, rotationBetween(shoulderRotation * forwardVector, - elbowPosition - extractTranslation(_jointStates.at(parentJoint.parentIndex).transform)), false); - - // update the shoulder state - updateJointState(parentJoint.parentIndex); - - // adjust the elbow's local translation - setJointTranslation(joint.parentIndex, elbowPosition); -} diff --git a/interface/src/avatar/SkeletonModel.h b/interface/src/avatar/SkeletonModel.h index 3d95d805ea..533f22975f 100644 --- a/interface/src/avatar/SkeletonModel.h +++ b/interface/src/avatar/SkeletonModel.h @@ -43,10 +43,6 @@ protected: private: - /// Using the current position and rotation of the identified (hand) joint, computes a - /// reasonable stretched configuration for the connected arm. - void stretchArm(int jointIndex, const glm::vec3& position); - Avatar* _owningAvatar; }; diff --git a/interface/src/devices/SixenseManager.cpp b/interface/src/devices/SixenseManager.cpp index 0482dbf84c..8e54e034ac 100644 --- a/interface/src/devices/SixenseManager.cpp +++ b/interface/src/devices/SixenseManager.cpp @@ -6,17 +6,33 @@ // Copyright (c) 2013 High Fidelity, Inc. All rights reserved. // -#ifdef HAVE_SIXENSE - #include "sixense.h" -#endif +#include #include "Application.h" #include "SixenseManager.h" -using namespace std; - -SixenseManager::SixenseManager() : _lastMovement(0) { #ifdef HAVE_SIXENSE +const int CALIBRATION_STATE_IDLE = 0; +const int CALIBRATION_STATE_X = 1; +const int CALIBRATION_STATE_Y = 2; +const int CALIBRATION_STATE_Z = 3; +const int CALIBRATION_STATE_COMPLETE = 4; + +// default (expected) location of neck in sixense space +const float NECK_X = 250.f; // millimeters +const float NECK_Y = 300.f; // millimeters +const float NECK_Z = 300.f; // millimeters +#endif + +SixenseManager::SixenseManager() { +#ifdef HAVE_SIXENSE + _lastMovement = 0; + + _calibrationState = CALIBRATION_STATE_IDLE; + // By default we assume the _neckBase (in orb frame) is as high above the orb + // as the "torso" is below it. + _neckBase = glm::vec3(NECK_X, -NECK_Y, NECK_Z); + sixenseInit(); #endif } @@ -48,12 +64,18 @@ void SixenseManager::update(float deltaTime) { Hand* hand = avatar->getHand(); int maxControllers = sixenseGetMaxControllers(); - for (int i = 0; i < maxControllers; i++) { + + // we only support two controllers + sixenseControllerData controllers[2]; + + int numActiveControllers = 0; + for (int i = 0; i < maxControllers && numActiveControllers < 2; i++) { if (!sixenseIsControllerEnabled(i)) { continue; } - sixenseControllerData data; - sixenseGetNewestData(i, &data); + sixenseControllerData* data = controllers + numActiveControllers; + ++numActiveControllers; + sixenseGetNewestData(i, data); // Set palm position and normal based on Hydra position/orientation @@ -61,7 +83,7 @@ void SixenseManager::update(float deltaTime) { PalmData* palm; bool foundHand = false; for (size_t j = 0; j < hand->getNumPalms(); j++) { - if (hand->getPalms()[j].getSixenseID() == data.controller_index) { + if (hand->getPalms()[j].getSixenseID() == data->controller_index) { palm = &(hand->getPalms()[j]); foundHand = true; } @@ -70,26 +92,27 @@ void SixenseManager::update(float deltaTime) { PalmData newPalm(hand); hand->getPalms().push_back(newPalm); palm = &(hand->getPalms()[hand->getNumPalms() - 1]); - palm->setSixenseID(data.controller_index); - printf("Found new Sixense controller, ID %i\n", data.controller_index); + palm->setSixenseID(data->controller_index); + printf("Found new Sixense controller, ID %i\n", data->controller_index); } palm->setActive(true); // Read controller buttons and joystick into the hand - palm->setControllerButtons(data.buttons); - palm->setTrigger(data.trigger); - palm->setJoystick(data.joystick_x, data.joystick_y); - - glm::vec3 position(data.pos[0], data.pos[1], data.pos[2]); - // Adjust for distance between acquisition 'orb' and the user's torso - // (distance to the right of body center, distance below torso, distance behind torso) - const glm::vec3 SPHERE_TO_TORSO(-250.f, -300.f, -300.f); - position = SPHERE_TO_TORSO + position; - + palm->setControllerButtons(data->buttons); + palm->setTrigger(data->trigger); + palm->setJoystick(data->joystick_x, data->joystick_y); + + glm::vec3 position(data->pos[0], data->pos[1], data->pos[2]); + // Transform the measured position into body frame. + glm::vec3 neck = _neckBase; + // Zeroing y component of the "neck" effectively raises the measured position a little bit. + neck.y = 0.f; + position = _orbRotation * (position - neck); + // Rotation of Palm - glm::quat rotation(data.rot_quat[3], -data.rot_quat[0], data.rot_quat[1], -data.rot_quat[2]); - rotation = glm::angleAxis(PI, glm::vec3(0.f, 1.f, 0.f)) * rotation; + glm::quat rotation(data->rot_quat[3], -data->rot_quat[0], data->rot_quat[1], -data->rot_quat[2]); + rotation = glm::angleAxis(PI, glm::vec3(0.f, 1.f, 0.f)) * _orbRotation * rotation; const glm::vec3 PALM_VECTOR(0.0f, -1.0f, 0.0f); glm::vec3 newNormal = rotation * PALM_VECTOR; palm->setRawNormal(newNormal); @@ -126,14 +149,159 @@ void SixenseManager::update(float deltaTime) { palm->getFingers().push_back(finger); palm->getFingers().push_back(finger); } - + + if (numActiveControllers == 2) { + updateCalibration(controllers); + } + // if the controllers haven't been moved in a while, disable const unsigned int MOVEMENT_DISABLE_DURATION = 30 * 1000 * 1000; if (usecTimestampNow() - _lastMovement > MOVEMENT_DISABLE_DURATION) { - for (vector::iterator it = hand->getPalms().begin(); it != hand->getPalms().end(); it++) { + for (std::vector::iterator it = hand->getPalms().begin(); it != hand->getPalms().end(); it++) { it->setActive(false); } } -#endif +#endif // HAVE_SIXENSE } +#ifdef HAVE_SIXENSE + +// the calibration sequence is: +// (1) press BUTTON_FWD on both hands +// (2) reach arm straight out to the side (X) +// (3) lift arms staight up above head (Y) +// (4) move arms a bit forward (Z) +// (5) release BUTTON_FWD on both hands + +const float MINIMUM_ARM_REACH = 300.f; // millimeters +const float MAXIMUM_NOISE_LEVEL = 50.f; // millimeters +const quint64 LOCK_DURATION = USECS_PER_SECOND / 4; // time for lock to be acquired + +void SixenseManager::updateCalibration(const sixenseControllerData* controllers) { + const sixenseControllerData* dataLeft = controllers; + const sixenseControllerData* dataRight = controllers + 1; + + // calibration only happpens while both hands are holding BUTTON_FORWARD + if (dataLeft->buttons != BUTTON_FWD || dataRight->buttons != BUTTON_FWD) { + if (_calibrationState == CALIBRATION_STATE_IDLE) { + return; + } + switch (_calibrationState) { + case CALIBRATION_STATE_Y: + case CALIBRATION_STATE_Z: + case CALIBRATION_STATE_COMPLETE: + { + // compute calibration results + // ATM we only handle the case where the XAxis has been measured, and we assume the rest + // (i.e. that the orb is on a level surface) + // TODO: handle COMPLETE state where all three axes have been defined. This would allow us + // to also handle the case where left and right controllers have been reversed. + _neckBase = 0.5f * (_reachLeft + _reachRight); // neck is midway between right and left reaches + glm::vec3 xAxis = glm::normalize(_reachRight - _reachLeft); + glm::vec3 yAxis(0.f, 1.f, 0.f); + glm::vec3 zAxis = glm::normalize(glm::cross(xAxis, yAxis)); + xAxis = glm::normalize(glm::cross(yAxis, zAxis)); + _orbRotation = glm::inverse(glm::quat_cast(glm::mat3(xAxis, yAxis, zAxis))); + qDebug("succeess: sixense calibration"); + } + break; + default: + qDebug("failed: sixense calibration"); + break; + } + + _calibrationState = CALIBRATION_STATE_IDLE; + return; + } + + const float* pos = dataLeft->pos; + glm::vec3 positionLeft(pos[0], pos[1], pos[2]); + pos = dataRight->pos; + glm::vec3 positionRight(pos[0], pos[1], pos[2]); + + if (_calibrationState == CALIBRATION_STATE_IDLE) { + float reach = glm::distance(positionLeft, positionRight); + if (reach > 2.f * MINIMUM_ARM_REACH) { + qDebug("started: sixense calibration"); + _averageLeft = positionLeft; + _averageRight = positionRight; + _reachLeft = _averageLeft; + _reachRight = _averageRight; + _lastDistance = reach; + _lockExpiry = usecTimestampNow() + LOCK_DURATION; + // move to next state + _calibrationState = CALIBRATION_STATE_X; + } + return; + } + + quint64 now = usecTimestampNow() + LOCK_DURATION; + // these are weighted running averages + _averageLeft = 0.9f * _averageLeft + 0.1f * positionLeft; + _averageRight = 0.9f * _averageRight + 0.1f * positionRight; + + if (_calibrationState == CALIBRATION_STATE_X) { + // compute new sliding average + float distance = glm::distance(_averageLeft, _averageRight); + if (fabs(distance - _lastDistance) > MAXIMUM_NOISE_LEVEL) { + // distance is increasing so acquire the data and push the expiry out + _reachLeft = _averageLeft; + _reachRight = _averageRight; + _lastDistance = distance; + _lockExpiry = now + LOCK_DURATION; + } else if (now > _lockExpiry) { + // lock has expired so clamp the data and move on + _lockExpiry = now + LOCK_DURATION; + _lastDistance = 0.f; + _reachUp = 0.5f * (_reachLeft + _reachRight); + _calibrationState = CALIBRATION_STATE_Y; + qDebug("success: sixense calibration: left"); + } + } + else if (_calibrationState == CALIBRATION_STATE_Y) { + glm::vec3 torso = 0.5f * (_reachLeft + _reachRight); + glm::vec3 averagePosition = 0.5f * (_averageLeft + _averageRight); + float distance = (averagePosition - torso).y; + if (fabs(distance) > fabs(_lastDistance) + MAXIMUM_NOISE_LEVEL) { + // distance is increasing so acquire the data and push the expiry out + _reachUp = averagePosition; + _lastDistance = distance; + _lockExpiry = now + LOCK_DURATION; + } else if (now > _lockExpiry) { + if (_lastDistance > MINIMUM_ARM_REACH) { + // lock has expired so clamp the data and move on + _reachForward = _reachUp; + _lastDistance = 0.f; + _lockExpiry = now + LOCK_DURATION; + _calibrationState = CALIBRATION_STATE_Z; + qDebug("success: sixense calibration: up"); + } + } + } + else if (_calibrationState == CALIBRATION_STATE_Z) { + glm::vec3 xAxis = glm::normalize(_reachRight - _reachLeft); + glm::vec3 torso = 0.5f * (_reachLeft + _reachRight); + //glm::vec3 yAxis = glm::normalize(_reachUp - torso); + glm::vec3 yAxis(0.f, 1.f, 0.f); + glm::vec3 zAxis = glm::normalize(glm::cross(xAxis, yAxis)); + + glm::vec3 averagePosition = 0.5f * (_averageLeft + _averageRight); + float distance = glm::dot((averagePosition - torso), zAxis); + if (fabs(distance) > fabs(_lastDistance)) { + // distance is increasing so acquire the data and push the expiry out + _reachForward = averagePosition; + _lastDistance = distance; + _lockExpiry = now + LOCK_DURATION; + } else if (now > _lockExpiry) { + if (fabs(_lastDistance) > 0.05f * MINIMUM_ARM_REACH) { + // lock has expired so clamp the data and move on + _calibrationState = CALIBRATION_STATE_COMPLETE; + qDebug("success: sixense calibration: forward"); + // TODO: it is theoretically possible to detect that the controllers have been + // accidentally switched (left hand is holding right controller) and to swap the order. + } + } + } +} +#endif // HAVE_SIXENSE + diff --git a/interface/src/devices/SixenseManager.h b/interface/src/devices/SixenseManager.h index 1d8263ee30..de4cccb399 100644 --- a/interface/src/devices/SixenseManager.h +++ b/interface/src/devices/SixenseManager.h @@ -9,6 +9,21 @@ #ifndef __interface__SixenseManager__ #define __interface__SixenseManager__ +#include + +#ifdef HAVE_SIXENSE + #include + #include + #include "sixense.h" +#endif + +const unsigned int BUTTON_0 = 1U << 0; // the skinny button between 1 and 2 +const unsigned int BUTTON_1 = 1U << 5; +const unsigned int BUTTON_2 = 1U << 6; +const unsigned int BUTTON_3 = 1U << 3; +const unsigned int BUTTON_4 = 1U << 4; +const unsigned int BUTTON_FWD = 1U << 7; + /// Handles interaction with the Sixense SDK (e.g., Razer Hydra). class SixenseManager : public QObject { Q_OBJECT @@ -24,7 +39,27 @@ public slots: void setFilter(bool filter); private: - +#ifdef HAVE_SIXENSE + void updateCalibration(const sixenseControllerData* controllers); + + int _calibrationState; + + // these are calibration results + glm::vec3 _neckBase; // midpoint between controllers during X-axis calibration + glm::quat _orbRotation; // rotates from orb frame into body frame + float _armLength; + + // these are measured values used to compute the calibration results + quint64 _lockExpiry; + glm::vec3 _averageLeft; + glm::vec3 _averageRight; + glm::vec3 _reachLeft; + glm::vec3 _reachRight; + glm::vec3 _reachUp; + glm::vec3 _reachForward; + float _lastDistance; + +#endif quint64 _lastMovement; }; diff --git a/libraries/avatars/src/HandData.cpp b/libraries/avatars/src/HandData.cpp index 0355a4c86b..46dade6d64 100644 --- a/libraries/avatars/src/HandData.cpp +++ b/libraries/avatars/src/HandData.cpp @@ -25,10 +25,6 @@ HandData::HandData(AvatarData* owningAvatar) : addNewPalm(); } -glm::vec3 HandData::worldPositionToLeapPosition(const glm::vec3& worldPosition) const { - return glm::inverse(getBaseOrientation()) * (worldPosition - getBasePosition()) / LEAP_UNIT_SCALE; -} - glm::vec3 HandData::worldVectorToLeapVector(const glm::vec3& worldVector) const { return glm::inverse(getBaseOrientation()) * worldVector / LEAP_UNIT_SCALE; } @@ -272,9 +268,7 @@ glm::quat HandData::getBaseOrientation() const { } glm::vec3 HandData::getBasePosition() const { - const glm::vec3 LEAP_HANDS_OFFSET_FROM_TORSO(0.0, 0.3, -0.3); - return _owningAvatarData->getPosition() + getBaseOrientation() * LEAP_HANDS_OFFSET_FROM_TORSO * - _owningAvatarData->getTargetScale(); + return _owningAvatarData->getPosition(); } void FingerData::setTrailLength(unsigned int length) { diff --git a/libraries/avatars/src/HandData.h b/libraries/avatars/src/HandData.h index cdab9f71e9..f649faf080 100755 --- a/libraries/avatars/src/HandData.h +++ b/libraries/avatars/src/HandData.h @@ -28,13 +28,6 @@ const int NUM_FINGERS = NUM_HANDS * NUM_FINGERS_PER_HAND; const int LEAPID_INVALID = -1; const int SIXENSEID_INVALID = -1; -const int BUTTON_0 = 1; // the skinny button between 1 and 2 -const int BUTTON_1 = 32; -const int BUTTON_2 = 64; -const int BUTTON_3 = 8; -const int BUTTON_4 = 16; -const int BUTTON_FWD = 128; - const float LEAP_UNIT_SCALE = 0.001f; ///< convert mm to meters const int SIXENSE_CONTROLLER_ID_LEFT_HAND = 0; @@ -55,7 +48,6 @@ public: glm::vec3 leapDirectionToWorldDirection(const glm::vec3& leapDirection) { return getBaseOrientation() * leapDirection; } - glm::vec3 worldPositionToLeapPosition(const glm::vec3& worldPosition) const; glm::vec3 worldVectorToLeapVector(const glm::vec3& worldVector) const; std::vector& getPalms() { return _palms; } @@ -182,11 +174,11 @@ public: int getFramesWithoutData() const { return _numFramesWithoutData; } // Controller buttons - void setControllerButtons(int controllerButtons) { _controllerButtons = controllerButtons; } - void setLastControllerButtons(int controllerButtons) { _lastControllerButtons = controllerButtons; } + void setControllerButtons(unsigned int controllerButtons) { _controllerButtons = controllerButtons; } + void setLastControllerButtons(unsigned int controllerButtons) { _lastControllerButtons = controllerButtons; } - int getControllerButtons() const { return _controllerButtons; } - int getLastControllerButtons() const { return _lastControllerButtons; } + unsigned int getControllerButtons() const { return _controllerButtons; } + unsigned int getLastControllerButtons() const { return _lastControllerButtons; } void setTrigger(float trigger) { _trigger = trigger; } float getTrigger() const { return _trigger; } @@ -217,8 +209,8 @@ private: glm::vec3 _tipPosition; glm::vec3 _tipVelocity; - int _controllerButtons; - int _lastControllerButtons; + unsigned int _controllerButtons; + unsigned int _lastControllerButtons; float _trigger; float _joystickX, _joystickY; diff --git a/libraries/shared/src/SharedUtil.h b/libraries/shared/src/SharedUtil.h index 1695b3b253..439b85aa54 100644 --- a/libraries/shared/src/SharedUtil.h +++ b/libraries/shared/src/SharedUtil.h @@ -57,10 +57,9 @@ static const float DEGREES_PER_RADIAN = 180.0f / PI; static const float EPSILON = 0.000001f; //smallish positive number - used as margin of error for some computations static const float SQUARE_ROOT_OF_2 = (float)sqrt(2.f); static const float SQUARE_ROOT_OF_3 = (float)sqrt(3.f); -static const float METER = 1.0f; -static const float DECIMETER = 0.1f; -static const float CENTIMETER = 0.01f; -static const float MILLIIMETER = 0.001f; +static const float METERS_PER_DECIMETER = 0.1f; +static const float METERS_PER_CENTIMETER = 0.01f; +static const float METERS_PER_MILLIMETER = 0.001f; static const quint64 USECS_PER_MSEC = 1000; static const quint64 MSECS_PER_SECOND = 1000; static const quint64 USECS_PER_SECOND = USECS_PER_MSEC * MSECS_PER_SECOND;