diff --git a/examples/leapHands.js b/examples/leapHands.js new file mode 100644 index 0000000000..95d3969a08 --- /dev/null +++ b/examples/leapHands.js @@ -0,0 +1,395 @@ +// +// leapHands.js +// examples +// +// Created by David Rowe on 8 Sep 2014. +// Copyright 2014 High Fidelity, Inc. +// +// This is an example script that uses the Leap Motion to make the avatar's hands replicate the user's hand actions. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +var leapHands = (function () { + + var hands, + wrists, + NUM_HANDS = 2, // 0 = left; 1 = right + fingers, + NUM_FINGERS = 5, // 0 = thumb; ...; 4 = pinky + THUMB = 0, + NUM_FINGER_JOINTS = 3, // 0 = metacarpal(hand)-proximal(finger) joint; ...; 2 = intermediate-distal(tip) joint + MAX_HAND_INACTIVE_COUNT = 20, + calibrationStatus, + UNCALIBRATED = 0, + CALIBRATING = 1, + CALIBRATED = 2, + CALIBRATION_TIME = 1000, // milliseconds + PI = 3.141593, + isWindows; + + function printSkeletonJointNames() { + var jointNames, + i; + + print(MyAvatar.skeletonModelURL); + + print("Skeleton joint names ..."); + jointNames = MyAvatar.getJointNames(); + for (i = 0; i < jointNames.length; i += 1) { + print(i + ": " + jointNames[i]); + } + print("... skeleton joint names"); + + /* + http://public.highfidelity.io/models/skeletons/ron_standing.fst + Skeleton joint names ... + 0: Hips + 1: RightUpLeg + 2: RightLeg + 3: RightFoot + 4: RightToeBase + 5: RightToe_End + 6: LeftUpLeg + 7: LeftLeg + 8: LeftFoot + 9: LeftToeBase + 10: LeftToe_End + 11: Spine + 12: Spine1 + 13: Spine2 + 14: RightShoulder + 15: RightArm + 16: RightForeArm + 17: RightHand + 18: RightHandPinky1 + 19: RightHandPinky2 + 20: RightHandPinky3 + 21: RightHandPinky4 + 22: RightHandRing1 + 23: RightHandRing2 + 24: RightHandRing3 + 25: RightHandRing4 + 26: RightHandMiddle1 + 27: RightHandMiddle2 + 28: RightHandMiddle3 + 29: RightHandMiddle4 + 30: RightHandIndex1 + 31: RightHandIndex2 + 32: RightHandIndex3 + 33: RightHandIndex4 + 34: RightHandThumb1 + 35: RightHandThumb2 + 36: RightHandThumb3 + 37: RightHandThumb4 + 38: LeftShoulder + 39: LeftArm + 40: LeftForeArm + 41: LeftHand + 42: LeftHandPinky1 + 43: LeftHandPinky2 + 44: LeftHandPinky3 + 45: LeftHandPinky4 + 46: LeftHandRing1 + 47: LeftHandRing2 + 48: LeftHandRing3 + 49: LeftHandRing4 + 50: LeftHandMiddle1 + 51: LeftHandMiddle2 + 52: LeftHandMiddle3 + 53: LeftHandMiddle4 + 54: LeftHandIndex1 + 55: LeftHandIndex2 + 56: LeftHandIndex3 + 57: LeftHandIndex4 + 58: LeftHandThumb1 + 59: LeftHandThumb2 + 60: LeftHandThumb3 + 61: LeftHandThumb4 + 62: Neck + 63: Head + 64: HeadTop_End + 65: body + ... skeleton joint names + */ + } + + function finishCalibration() { + var avatarPosition, + avatarOrientation, + avatarHandPosition, + leapHandHeight, + h; + + if (hands[0].controller.isActive() && hands[1].controller.isActive()) { + leapHandHeight = (hands[0].controller.getAbsTranslation().y + hands[1].controller.getAbsTranslation().y) / 2.0; + + // TODO: Temporary detection of Windows to work around Leap Controller problem. + isWindows = (hands[1].controller.getAbsRotation().z > (0.25 * PI)); + } else { + calibrationStatus = UNCALIBRATED; + return; + } + + avatarPosition = MyAvatar.position; + avatarOrientation = MyAvatar.orientation; + + for (h = 0; h < NUM_HANDS; h += 1) { + avatarHandPosition = MyAvatar.getJointPosition(hands[h].jointName); + hands[h].zeroPosition = { + x: avatarHandPosition.x - avatarPosition.x, + y: avatarHandPosition.y - avatarPosition.y, + z: avatarPosition.z - avatarHandPosition.z + }; + hands[h].zeroPosition = Vec3.multiplyQbyV(MyAvatar.orientation, hands[h].zeroPosition); + hands[h].zeroPosition.y = hands[h].zeroPosition.y - leapHandHeight; + } + + MyAvatar.clearJointData("LeftHand"); + MyAvatar.clearJointData("LeftForeArm"); + MyAvatar.clearJointData("LeftArm"); + MyAvatar.clearJointData("RightHand"); + MyAvatar.clearJointData("RightForeArm"); + MyAvatar.clearJointData("RightArm"); + + calibrationStatus = CALIBRATED; + print("Leap Motion: Calibrated"); + } + + function calibrate() { + + calibrationStatus = CALIBRATING; + + // Set avatar arms vertical, forearms horizontal, as "zero" position for calibration + 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(0.0, 0.0, 0.0)); + 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(0.0, 0.0, 0.0)); + + // Wait for arms to assume their positions before calculating + Script.setTimeout(finishCalibration, CALIBRATION_TIME); + } + + function checkCalibration() { + + if (calibrationStatus === CALIBRATED) { + return true; + } + + if (calibrationStatus !== CALIBRATING) { + calibrate(); + } + + return false; + } + + function setUp() { + + calibrationStatus = UNCALIBRATED; + + // TODO: Leap Motion controller joint naming doesn't match up with skeleton joint naming; numbers are out by 1. + + hands = [ + { + 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] = [ + [ + { jointName: "LeftHandThumb1", controller: Controller.createInputController("Spatial", "joint_L_thumb2") }, + { jointName: "LeftHandThumb2", controller: Controller.createInputController("Spatial", "joint_L_thumb3") }, + { jointName: "LeftHandThumb3", controller: Controller.createInputController("Spatial", "joint_L_thumb4") } + ], + [ + { jointName: "LeftHandIndex1", controller: Controller.createInputController("Spatial", "joint_L_index2") }, + { jointName: "LeftHandIndex2", controller: Controller.createInputController("Spatial", "joint_L_index3") }, + { jointName: "LeftHandIndex3", controller: Controller.createInputController("Spatial", "joint_L_index4") } + ], + [ + { jointName: "LeftHandMiddle1", controller: Controller.createInputController("Spatial", "joint_L_middle2") }, + { jointName: "LeftHandMiddle2", controller: Controller.createInputController("Spatial", "joint_L_middle3") }, + { jointName: "LeftHandMiddle3", controller: Controller.createInputController("Spatial", "joint_L_middle4") } + ], + [ + { jointName: "LeftHandRing1", controller: Controller.createInputController("Spatial", "joint_L_ring2") }, + { jointName: "LeftHandRing2", controller: Controller.createInputController("Spatial", "joint_L_ring3") }, + { jointName: "LeftHandRing3", controller: Controller.createInputController("Spatial", "joint_L_ring4") } + ], + [ + { jointName: "LeftHandPinky1", controller: Controller.createInputController("Spatial", "joint_L_pinky2") }, + { jointName: "LeftHandPinky2", controller: Controller.createInputController("Spatial", "joint_L_pinky3") }, + { jointName: "LeftHandPinky3", controller: Controller.createInputController("Spatial", "joint_L_pinky4") } + ] + ]; + fingers[1] = [ + [ + { jointName: "RightHandThumb1", controller: Controller.createInputController("Spatial", "joint_R_thumb2") }, + { jointName: "RightHandThumb2", controller: Controller.createInputController("Spatial", "joint_R_thumb3") }, + { jointName: "RightHandThumb3", controller: Controller.createInputController("Spatial", "joint_R_thumb4") } + ], + [ + { jointName: "RightHandIndex1", controller: Controller.createInputController("Spatial", "joint_R_index2") }, + { jointName: "RightHandIndex2", controller: Controller.createInputController("Spatial", "joint_R_index3") }, + { jointName: "RightHandIndex3", controller: Controller.createInputController("Spatial", "joint_R_index4") } + ], + [ + { jointName: "RightHandMiddle1", controller: Controller.createInputController("Spatial", "joint_R_middle2") }, + { jointName: "RightHandMiddle2", controller: Controller.createInputController("Spatial", "joint_R_middle3") }, + { jointName: "RightHandMiddle3", controller: Controller.createInputController("Spatial", "joint_R_middle4") } + ], + [ + { jointName: "RightHandRing1", controller: Controller.createInputController("Spatial", "joint_R_ring2") }, + { jointName: "RightHandRing2", controller: Controller.createInputController("Spatial", "joint_R_ring3") }, + { jointName: "RightHandRing3", controller: Controller.createInputController("Spatial", "joint_R_ring4") } + ], + [ + { jointName: "RightHandPinky1", controller: Controller.createInputController("Spatial", "joint_R_pinky2") }, + { jointName: "RightHandPinky2", controller: Controller.createInputController("Spatial", "joint_R_pinky3") }, + { jointName: "RightHandPinky3", controller: Controller.createInputController("Spatial", "joint_R_pinky4") } + ] + ]; + } + + function moveHands() { + var h, + i, + j, + side, + handOffset, + handRoll, + handPitch, + handYaw, + handRotation, + wristAbsRotation, + locRotation; + + for (h = 0; h < NUM_HANDS; h += 1) { + side = h === 0 ? -1.0 : 1.0; + + if (hands[h].controller.isActive()) { + + // Calibrate when and if a controller is first active. + if (!checkCalibration()) { + return; + } + + // Hand position ... + handOffset = hands[h].controller.getAbsTranslation(); + handOffset = { + x: -handOffset.x, + y: hands[h].zeroPosition.y + handOffset.y, + 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; + wristAbsRotation = wrists[h].controller.getAbsRotation(); + handPitch = 2.0 * -wristAbsRotation.x; + handYaw = 2.0 * wristAbsRotation.y; + + // TODO: Leap Motion controller's right-hand roll calculation only works if physical hand is upside down. + // Approximate fix is to add a fudge factor. + if (h === 1 && isWindows) { + handRoll = handRoll + 0.6 * PI; + } + + // Hand position and orientation ... + if (h === 0) { + handRotation = Quat.multiply(Quat.angleAxis(-90.0, { x: 0, y: 1, z: 0 }), + Quat.fromVec3Radians({ x: handRoll, y: handYaw, z: -handPitch })); + + + } else { + handRotation = Quat.multiply(Quat.angleAxis(90.0, { x: 0, y: 1, z: 0 }), + Quat.fromVec3Radians({ x: -handRoll, y: handYaw, z: handPitch })); + } + MyAvatar.setJointModelPositionAndOrientation(hands[h].jointName, handOffset, handRotation, true); + + // Finger joints ... + // TODO: 2.0 * scale factors should not be necessary; Leap Motion controller code needs investigating. + for (i = 0; i < NUM_FINGERS; i += 1) { + for (j = 0; j < NUM_FINGER_JOINTS; j += 1) { + if (fingers[h][i][j].controller !== null) { + locRotation = fingers[h][i][j].controller.getLocRotation(); + if (i === THUMB) { + MyAvatar.setJointData(fingers[h][i][j].jointName, + Quat.fromPitchYawRollRadians(2.0 * side * locRotation.y, 2.0 * -locRotation.z, + 2.0 * side * -locRotation.x)); + } else { + MyAvatar.setJointData(fingers[h][i][j].jointName, + Quat.fromPitchYawRollRadians(2.0 * -locRotation.x, 0.0, 2.0 * -locRotation.y)); + } + } + } + } + + hands[h].inactiveCount = 0; + + } else { + + hands[h].inactiveCount += 1; + + if (hands[h].inactiveCount === MAX_HAND_INACTIVE_COUNT) { + if (h === 0) { + MyAvatar.clearJointData("LeftHand"); + MyAvatar.clearJointData("LeftForeArm"); + MyAvatar.clearJointData("LeftArm"); + } else { + MyAvatar.clearJointData("RightHand"); + MyAvatar.clearJointData("RightForeArm"); + MyAvatar.clearJointData("RightArm"); + } + } + } + } + } + + function tearDown() { + var h, + i, + j; + + for (h = 0; h < NUM_HANDS; h += 1) { + Controller.releaseInputController(hands[h].controller); + Controller.releaseInputController(wrists[h].controller); + for (i = 0; i < NUM_FINGERS; i += 1) { + for (j = 0; j < NUM_FINGER_JOINTS; j += 1) { + if (fingers[h][i][j].controller !== null) { + Controller.releaseInputController(fingers[h][i][j].controller); + } + } + } + } + } + + return { + printSkeletonJointNames: printSkeletonJointNames, + setUp : setUp, + moveHands : moveHands, + tearDown : tearDown + }; +}()); + + +//leapHands.printSkeletonJointNames(); + +leapHands.setUp(); +Script.update.connect(leapHands.moveHands); +Script.scriptEnding.connect(leapHands.tearDown); diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp index 293aa8595f..b62c744fa2 100644 --- a/interface/src/avatar/Avatar.cpp +++ b/interface/src/avatar/Avatar.cpp @@ -830,6 +830,28 @@ glm::quat Avatar::getJointCombinedRotation(const QString& name) const { return rotation; } +const float SCRIPT_PRIORITY = DEFAULT_PRIORITY + 1.0f; + +void Avatar::setJointModelPositionAndOrientation(int index, glm::vec3 position, const glm::quat& rotation) { + if (QThread::currentThread() != thread()) { + QMetaObject::invokeMethod(const_cast(this), "setJointModelPositionAndOrientation", + Qt::BlockingQueuedConnection, Q_ARG(const int, index), Q_ARG(const glm::vec3, position), + Q_ARG(const glm::quat&, rotation)); + } else { + _skeletonModel.inverseKinematics(index, position, rotation, SCRIPT_PRIORITY); + } +} + +void Avatar::setJointModelPositionAndOrientation(const QString& name, glm::vec3 position, const glm::quat& rotation) { + if (QThread::currentThread() != thread()) { + QMetaObject::invokeMethod(const_cast(this), "setJointModelPositionAndOrientation", + Qt::BlockingQueuedConnection, Q_ARG(const QString&, name), Q_ARG(const glm::vec3, position), + Q_ARG(const glm::quat&, rotation)); + } else { + _skeletonModel.inverseKinematics(getJointIndex(name), position, rotation, SCRIPT_PRIORITY); + } +} + 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); diff --git a/interface/src/avatar/Avatar.h b/interface/src/avatar/Avatar.h index 2ec1ce661a..921722504d 100755 --- a/interface/src/avatar/Avatar.h +++ b/interface/src/avatar/Avatar.h @@ -151,6 +151,10 @@ public: Q_INVOKABLE glm::quat getJointCombinedRotation(int index) const; Q_INVOKABLE glm::quat getJointCombinedRotation(const QString& name) const; + Q_INVOKABLE void setJointModelPositionAndOrientation(int index, const glm::vec3 position, const glm::quat& rotation); + Q_INVOKABLE void setJointModelPositionAndOrientation(const QString& name, const glm::vec3 position, + const glm::quat& rotation); + Q_INVOKABLE glm::vec3 getVelocity() const { return _velocity; } Q_INVOKABLE glm::vec3 getAcceleration() const { return _acceleration; } Q_INVOKABLE glm::vec3 getAngularVelocity() const { return _angularVelocity; } diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 9eb6589306..953a5ca03a 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -995,6 +995,7 @@ void MyAvatar::clearJointData(int index) { if (QThread::currentThread() == thread()) { // HACK: ATM only JS scripts call clearJointData() on MyAvatar so we hardcode the priority _skeletonModel.setJointState(index, false, glm::quat(), 0.0f); + _skeletonModel.clearJointAnimationPriority(index); } } diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h index f32d53fcd8..f2d98ac589 100644 --- a/interface/src/renderer/Model.h +++ b/interface/src/renderer/Model.h @@ -183,6 +183,8 @@ public: QVector& getJointStates() { return _jointStates; } const QVector& getJointStates() const { return _jointStates; } + void inverseKinematics(int jointIndex, glm::vec3 position, const glm::quat& rotation, float priority); + protected: QSharedPointer _geometry; @@ -238,8 +240,6 @@ protected: 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. /// \param fraction the fraction of the default position to apply (i.e., 0.25f to slerp one fourth of the way to /// the original position diff --git a/interface/src/scripting/ControllerScriptingInterface.cpp b/interface/src/scripting/ControllerScriptingInterface.cpp index 50d07c1108..f2e65a6e28 100644 --- a/interface/src/scripting/ControllerScriptingInterface.cpp +++ b/interface/src/scripting/ControllerScriptingInterface.cpp @@ -312,7 +312,8 @@ void ControllerScriptingInterface::updateInputControllers() { InputController::InputController(int deviceTrackerId, int subTrackerId, QObject* parent) : AbstractInputController(), _deviceTrackerId(deviceTrackerId), - _subTrackerId(subTrackerId) + _subTrackerId(subTrackerId), + _isActive(false) { }