Position avatar hands with Leap Motion with no roll, pitch or yaw

examples/leapHands.js

@@ -21,6 +21,7 @@ var leapHands = (function () {
         THUMB = 0,
         NUM_FINGER_JOINTS = 3,  // 0 = metacarpal(hand)-proximal(finger) joint; ...; 2 = intermediate-distal(tip) joint
         MAX_HAND_INACTIVE_COUNT = 20,
+        LEAP_HEIGHT_OFFSET = 0.15,
         PI = 3.141593;
 
     function printSkeletonJointNames() {
@@ -114,14 +115,22 @@ var leapHands = (function () {
         // TODO: Leap Motion controller joint naming doesn't match up with skeleton joint naming; numbers are out by 1.
 
         hands = [
-            { controller: Controller.createInputController("Spatial", "joint_L_hand"), inactiveCount: 0 },
+            {
-            { controller: Controller.createInputController("Spatial", "joint_R_hand"), inactiveCount: 0 }
+                jointName: "LeftHand",
+                controller: Controller.createInputController("Spatial", "joint_L_hand"),
+                inactiveCount: 0
+            },
+            {
+                jointName: "RightHand",
+                controller: Controller.createInputController("Spatial", "joint_R_hand"),
+                inactiveCount: 0
+            }
         ];
 
         wrists = [
             { controller: Controller.createInputController("Spatial", "joint_L_wrist") },
             { controller: Controller.createInputController("Spatial", "joint_R_wrist") }
-        ]
+        ];
 
         fingers = [{}, {}];
         fingers[0] = [
@@ -178,6 +187,19 @@ var leapHands = (function () {
                 { jointName: "RightHandPinky3", controller: Controller.createInputController("Spatial", "joint_R_pinky4") }
             ]
         ];
+
+        // TODO: Add automatic calibration of avatar's hands.
+        // Calibration values for Ron.
+        hands[0].zeroPosition = {
+            x: -0.157,
+            y: 0.204,
+            z: 0.336
+        };
+        hands[1].zeroPosition = {
+            x: 0.234,
+            y: 0.204,
+            z: 0.339
+        };
     }
 
     function moveHands() {
@@ -185,16 +207,25 @@ var leapHands = (function () {
             i,
             j,
             side,
-            locRotation,
+            handOffset,
             handRoll,
             handPitch,
-            handYaw;
+            handYaw,
+            locRotation;
 
         for (h = 0; h < NUM_HANDS; h += 1) {
             side = h === 0 ? -1.0 : 1.0;
 
             if (hands[h].controller.isActive()) {
 
+                // Hand position ...
+                handOffset = hands[h].controller.getAbsTranslation();
+                handOffset = {
+                    x: -handOffset.x,
+                    y: hands[h].zeroPosition.y + handOffset.y - LEAP_HEIGHT_OFFSET,
+                    z: hands[h].zeroPosition.z - handOffset.z
+                };
+
                 // TODO: 2.0* scale factor should not be necessary; Leap Motion controller code needs investigating.
                 handRoll = 2.0 * -hands[h].controller.getAbsRotation().z;
                 handPitch = 2.0 * -wrists[h].controller.getAbsRotation().x;
@@ -205,16 +236,8 @@ var leapHands = (function () {
                     handRoll = handRoll + PI / 2.0;
                 }
 
-                // Fixed hand location for starters ...
+                // Hand position ...
-                if (h === 0) {
+                MyAvatar.setJointModelPositionAndOrientation(hands[h].jointName, handOffset);
-                    MyAvatar.setJointData("LeftArm", Quat.fromPitchYawRollDegrees(90.0, 0.0, -90.0));
-                    MyAvatar.setJointData("LeftForeArm", Quat.fromPitchYawRollDegrees(90.0, 0.0, 180.0));
-                    MyAvatar.setJointData("LeftHand", Quat.fromPitchYawRollRadians(handPitch, handRoll, handYaw));
-                } else {
-                    MyAvatar.setJointData("RightArm", Quat.fromPitchYawRollDegrees(90.0, 0.0, 90.0));
-                    MyAvatar.setJointData("RightForeArm", Quat.fromPitchYawRollDegrees(90.0, 0.0, 180.0));
-                    MyAvatar.setJointData("RightHand", Quat.fromPitchYawRollRadians(handPitch, handRoll, handYaw));
-                }
 
                 // Finger joints ...
                 // TODO: 2.0 * scale factors should not be necessary; Leap Motion controller code needs investigating.

interface/src/avatar/Avatar.cpp

@@ -840,6 +840,38 @@ glm::quat Avatar::getJointCombinedRotation(const QString& name) const {
     return rotation;
 }
 
+bool Avatar::setJointModelPositionAndOrientation(int index, glm::vec3 position, const glm::quat& rotation, 
+                                                 bool useRotation) {
+    bool success;
+    if (QThread::currentThread() != thread()) {
+        QMetaObject::invokeMethod(const_cast<Avatar*>(this), "setJointModelPositionAndOrientation", 
+            Qt::BlockingQueuedConnection, Q_RETURN_ARG(bool, success), Q_ARG(const int, index), 
+            Q_ARG(const glm::vec3, position),
+            Q_ARG(const glm::quat&, rotation), Q_ARG(bool, useRotation));
+    } else {
+        qDebug() << "setJointModelPositionAndOrientation()";
+        success = _skeletonModel.setJointPosition(index, position, rotation, useRotation, -1, true, 
+                                                  glm::vec3(0.0, -1.0, 0.0), DEFAULT_PRIORITY + 2.0f);
+    }
+    return success;
+}
+
+bool Avatar::setJointModelPositionAndOrientation(const QString& name, glm::vec3 position, const glm::quat& rotation, 
+                                                 bool useRotation) {
+    bool success;
+    if (QThread::currentThread() != thread()) {
+        QMetaObject::invokeMethod(const_cast<Avatar*>(this), "setJointModelPositionAndOrientation", 
+            Qt::BlockingQueuedConnection, Q_RETURN_ARG(bool, success), Q_ARG(const QString&, name), 
+            Q_ARG(const glm::vec3, position),
+            Q_ARG(const glm::quat&, rotation), Q_ARG(bool, useRotation));
+    } else {
+        qDebug() << "setJointModelPositionAndOrientation()";
+        success = _skeletonModel.setJointPosition(getJointIndex(name), position, rotation, useRotation, -1, true, 
+                                                  glm::vec3(0.0, -1.0, 0.0), DEFAULT_PRIORITY + 2.0f);
+    }
+    return success;
+}
+
 void Avatar::scaleVectorRelativeToPosition(glm::vec3 &positionToScale) const {
     //Scale a world space vector as if it was relative to the position
     positionToScale = _position + _scale * (positionToScale - _position);

interface/src/avatar/Avatar.h

@@ -151,6 +151,11 @@ public:
     Q_INVOKABLE glm::quat getJointCombinedRotation(int index) const;
     Q_INVOKABLE glm::quat getJointCombinedRotation(const QString& name) const;
     
+    Q_INVOKABLE bool setJointModelPositionAndOrientation(int index, const glm::vec3 position,
+        const glm::quat& rotation = glm::quat(), bool useRotation = false);
+    Q_INVOKABLE bool setJointModelPositionAndOrientation(const QString& name, const glm::vec3 position,
+        const glm::quat& rotation = glm::quat(), bool useRotation = false);
+
     Q_INVOKABLE glm::vec3 getVelocity() const { return _velocity; }
     Q_INVOKABLE glm::vec3 getAcceleration() const { return _acceleration; }
     Q_INVOKABLE glm::vec3 getAngularVelocity() const { return _angularVelocity; }

interface/src/renderer/Model.h

@@ -183,6 +183,18 @@ public:
     QVector<JointState>& getJointStates() { return _jointStates; }
     const QVector<JointState>& getJointStates() const { return _jointStates; }
  
+    /// \param jointIndex index of joint in model structure
+    /// \param position position of joint in model-frame
+    /// \param rotation rotation of joint in model-frame
+    /// \param useRotation false if rotation should be ignored
+    /// \param lastFreeIndex
+    /// \param allIntermediatesFree
+    /// \param alignment
+    /// \return true if joint exists
+    bool setJointPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation = glm::quat(),
+        bool useRotation = false, int lastFreeIndex = -1, bool allIntermediatesFree = false,
+        const glm::vec3& alignment = glm::vec3(0.0f, -1.0f, 0.0f), float priority = 1.0f);
+
 protected:
     QSharedPointer<NetworkGeometry> _geometry;
     
@@ -226,18 +238,6 @@ protected:
 
     virtual void updateVisibleJointStates();
     
-    /// \param jointIndex index of joint in model structure
-    /// \param position position of joint in model-frame
-    /// \param rotation rotation of joint in model-frame
-    /// \param useRotation false if rotation should be ignored
-    /// \param lastFreeIndex
-    /// \param allIntermediatesFree
-    /// \param alignment
-    /// \return true if joint exists
-    bool setJointPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation = glm::quat(),
-        bool useRotation = false, int lastFreeIndex = -1, bool allIntermediatesFree = false,
-        const glm::vec3& alignment = glm::vec3(0.0f, -1.0f, 0.0f), float priority = 1.0f);
-
     void inverseKinematics(int jointIndex, glm::vec3 position, const glm::quat& rotation, float priority);
     
     /// Restores the indexed joint to its default position.