Remove arm stretching, add sixense calibration

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;

interface/src/ControllerScriptingInterface.cpp

@@ -8,6 +8,7 @@
 
 #include <HandData.h>
 #include "Application.h"
+#include "devices/SixenseManager.h"
 #include "ControllerScriptingInterface.h"
 
 ControllerScriptingInterface::ControllerScriptingInterface() :

interface/src/avatar/SkeletonModel.cpp

@@ -123,6 +123,7 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& 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<int>& 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<int>& 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);
-}

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;
 };
 

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 <vector>
 
 #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<PalmData>::iterator it = hand->getPalms().begin(); it != hand->getPalms().end(); it++) {
+        for (std::vector<PalmData>::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
+

interface/src/devices/SixenseManager.h

@@ -9,6 +9,21 @@
 #ifndef __interface__SixenseManager__
 #define __interface__SixenseManager__
 
+#include <QObject>
+
+#ifdef HAVE_SIXENSE
+    #include <glm/glm.hpp>
+    #include <glm/gtc/quaternion.hpp>
+    #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;
 };
 

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) {

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<PalmData>& 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;
     

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;