add foot IK targets as actions

interface/resources/controllers/kinect.json

@@ -2,6 +2,8 @@
     "name": "Kinect to Standard",
     "channels": [
         { "from": "Kinect.LeftHand", "to": "Standard.LeftHand" },
-        { "from": "Kinect.RightHand", "to": "Standard.RightHand" }
+        { "from": "Kinect.RightHand", "to": "Standard.RightHand" },
+        { "from": "Kinect.LeftFoot", "to": "Standard.LeftFoot" },
+        { "from": "Kinect.RightFoot", "to": "Standard.RightFoot" }
     ]
 }

interface/resources/controllers/standard.json

@@ -58,6 +58,9 @@
         { "from": "Standard.RT", "to": "Actions.RightHandClick" },
 
         { "from": "Standard.LeftHand", "to": "Actions.LeftHand" },
-        { "from": "Standard.RightHand", "to": "Actions.RightHand" }
+        { "from": "Standard.RightHand", "to": "Actions.RightHand" },
+
+        { "from": "Standard.LeftFoot", "to": "Actions.LeftFoot" },
+        { "from": "Standard.RightFoot", "to": "Actions.RightFoot" }
     ]
 }

interface/src/Application.cpp

@@ -4386,6 +4386,10 @@ void Application::update(float deltaTime) {
     auto avatarToSensorMatrix = worldToSensorMatrix * myAvatarMatrix;
     myAvatar->setHandControllerPosesInSensorFrame(leftHandPose.transform(avatarToSensorMatrix), rightHandPose.transform(avatarToSensorMatrix));
 
+    controller::Pose leftFootPose = userInputMapper->getPoseState(controller::Action::LEFT_FOOT);
+    controller::Pose rightFootPose = userInputMapper->getPoseState(controller::Action::RIGHT_FOOT);
+    myAvatar->setFootControllerPosesInSensorFrame(leftFootPose.transform(avatarToSensorMatrix), rightFootPose.transform(avatarToSensorMatrix));
+
     updateThreads(deltaTime); // If running non-threaded, then give the threads some time to process...
     updateDialogs(deltaTime); // update various stats dialogs if present
 

interface/src/avatar/MyAvatar.cpp

@@ -1345,6 +1345,45 @@ controller::Pose MyAvatar::getRightHandControllerPoseInAvatarFrame() const {
     return getRightHandControllerPoseInWorldFrame().transform(invAvatarMatrix);
 }
 
+void MyAvatar::setFootControllerPosesInSensorFrame(const controller::Pose& left, const controller::Pose& right) {
+    if (controller::InputDevice::getLowVelocityFilter()) {
+        auto oldLeftPose = getLeftFootControllerPoseInSensorFrame();
+        auto oldRightPose = getRightFootControllerPoseInSensorFrame();
+        _leftFootControllerPoseInSensorFrameCache.set(applyLowVelocityFilter(oldLeftPose, left));
+        _rightFootControllerPoseInSensorFrameCache.set(applyLowVelocityFilter(oldRightPose, right));
+    } else {
+        _leftFootControllerPoseInSensorFrameCache.set(left);
+        _rightFootControllerPoseInSensorFrameCache.set(right);
+    }
+}
+
+controller::Pose MyAvatar::getLeftFootControllerPoseInSensorFrame() const {
+    return _leftFootControllerPoseInSensorFrameCache.get();
+}
+
+controller::Pose MyAvatar::getRightFootControllerPoseInSensorFrame() const {
+    return _rightFootControllerPoseInSensorFrameCache.get();
+}
+
+controller::Pose MyAvatar::getLeftFootControllerPoseInWorldFrame() const {
+    return _leftFootControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix());
+}
+
+controller::Pose MyAvatar::getRightFootControllerPoseInWorldFrame() const {
+    return _rightFootControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix());
+}
+
+controller::Pose MyAvatar::getLeftFootControllerPoseInAvatarFrame() const {
+    glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition()));
+    return getLeftFootControllerPoseInWorldFrame().transform(invAvatarMatrix);
+}
+
+controller::Pose MyAvatar::getRightFootControllerPoseInAvatarFrame() const {
+    glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition()));
+    return getRightFootControllerPoseInWorldFrame().transform(invAvatarMatrix);
+}
+
+
 void MyAvatar::updateMotors() {
     _characterController.clearMotors();
     glm::quat motorRotation;

interface/src/avatar/MyAvatar.h

@@ -445,6 +445,14 @@ public:
     controller::Pose getLeftHandControllerPoseInAvatarFrame() const;
     controller::Pose getRightHandControllerPoseInAvatarFrame() const;
 
+    void setFootControllerPosesInSensorFrame(const controller::Pose& left, const controller::Pose& right);
+    controller::Pose getLeftFootControllerPoseInSensorFrame() const;
+    controller::Pose getRightFootControllerPoseInSensorFrame() const;
+    controller::Pose getLeftFootControllerPoseInWorldFrame() const;
+    controller::Pose getRightFootControllerPoseInWorldFrame() const;
+    controller::Pose getLeftFootControllerPoseInAvatarFrame() const;
+    controller::Pose getRightFootControllerPoseInAvatarFrame() const;
+
     bool hasDriveInput() const;
 
     Q_INVOKABLE void setCharacterControllerEnabled(bool enabled);
@@ -684,6 +692,9 @@ private:
     ThreadSafeValueCache<controller::Pose> _leftHandControllerPoseInSensorFrameCache { controller::Pose() };
     ThreadSafeValueCache<controller::Pose> _rightHandControllerPoseInSensorFrameCache { controller::Pose() };
 
+    ThreadSafeValueCache<controller::Pose> _leftFootControllerPoseInSensorFrameCache{ controller::Pose() };
+    ThreadSafeValueCache<controller::Pose> _rightFootControllerPoseInSensorFrameCache{ controller::Pose() };
+
     bool _hmdLeanRecenterEnabled = true;
 
     AnimPose _prePhysicsRoomPose;

interface/src/avatar/SkeletonModel.cpp

@@ -132,31 +132,49 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
 
         _rig->updateFromHeadParameters(headParams, deltaTime);
 
-        Rig::HandParameters handParams;
+        Rig::HandAndFeetParameters handAndFeetParams;
 
         auto leftPose = myAvatar->getLeftHandControllerPoseInAvatarFrame();
         if (leftPose.isValid()) {
-            handParams.isLeftEnabled = true;
-            handParams.leftPosition = Quaternions::Y_180 * leftPose.getTranslation();
-            handParams.leftOrientation = Quaternions::Y_180 * leftPose.getRotation();
+            handAndFeetParams.isLeftEnabled = true;
+            handAndFeetParams.leftPosition = Quaternions::Y_180 * leftPose.getTranslation();
+            handAndFeetParams.leftOrientation = Quaternions::Y_180 * leftPose.getRotation();
         } else {
-            handParams.isLeftEnabled = false;
+            handAndFeetParams.isLeftEnabled = false;
         }
 
         auto rightPose = myAvatar->getRightHandControllerPoseInAvatarFrame();
         if (rightPose.isValid()) {
-            handParams.isRightEnabled = true;
-            handParams.rightPosition = Quaternions::Y_180 * rightPose.getTranslation();
-            handParams.rightOrientation = Quaternions::Y_180 * rightPose.getRotation();
+            handAndFeetParams.isRightEnabled = true;
+            handAndFeetParams.rightPosition = Quaternions::Y_180 * rightPose.getTranslation();
+            handAndFeetParams.rightOrientation = Quaternions::Y_180 * rightPose.getRotation();
         } else {
-            handParams.isRightEnabled = false;
+            handAndFeetParams.isRightEnabled = false;
         }
 
-        handParams.bodyCapsuleRadius = myAvatar->getCharacterController()->getCapsuleRadius();
-        handParams.bodyCapsuleHalfHeight = myAvatar->getCharacterController()->getCapsuleHalfHeight();
-        handParams.bodyCapsuleLocalOffset = myAvatar->getCharacterController()->getCapsuleLocalOffset();
+        auto leftFootPose = myAvatar->getLeftFootControllerPoseInAvatarFrame();
+        if (leftFootPose.isValid()) {
+            handAndFeetParams.isLeftFootEnabled = true;
+            handAndFeetParams.leftFootPosition = Quaternions::Y_180 * leftFootPose.getTranslation();
+            handAndFeetParams.leftFootOrientation = Quaternions::Y_180 * leftFootPose.getRotation();
+        } else {
+            handAndFeetParams.isLeftFootEnabled = false;
+        }
 
-        _rig->updateFromHandParameters(handParams, deltaTime);
+        auto rightFootPose = myAvatar->getRightFootControllerPoseInAvatarFrame();
+        if (rightFootPose.isValid()) {
+            handAndFeetParams.isRightFootEnabled = true;
+            handAndFeetParams.rightFootPosition = Quaternions::Y_180 * rightFootPose.getTranslation();
+            handAndFeetParams.rightFootOrientation = Quaternions::Y_180 * rightFootPose.getRotation();
+        } else {
+            handAndFeetParams.isRightFootEnabled = false;
+        }
+
+        handAndFeetParams.bodyCapsuleRadius = myAvatar->getCharacterController()->getCapsuleRadius();
+        handAndFeetParams.bodyCapsuleHalfHeight = myAvatar->getCharacterController()->getCapsuleHalfHeight();
+        handAndFeetParams.bodyCapsuleLocalOffset = myAvatar->getCharacterController()->getCapsuleLocalOffset();
+
+        _rig->updateFromHandAndFeetParameters(handAndFeetParams, deltaTime);
 
         Rig::CharacterControllerState ccState = convertCharacterControllerState(myAvatar->getCharacterController()->getState());
 

libraries/animation/src/Rig.cpp

@@ -1146,9 +1146,8 @@ void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm
     }
 }
 
-void Rig::updateFromHandParameters(const HandParameters& params, float dt) {
+void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, float dt) {
     if (_animSkeleton && _animNode) {
-
         const float HAND_RADIUS = 0.05f;
         int hipsIndex = indexOfJoint("Hips");
         glm::vec3 hipsTrans;
@@ -1197,6 +1196,27 @@ void Rig::updateFromHandParameters(const HandParameters& params, float dt) {
             _animVars.unset("rightHandRotation");
             _animVars.set("rightHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
         }
+
+        if (params.isLeftFootEnabled) {
+            _animVars.set("leftFootPosition", params.leftFootPosition);
+            _animVars.set("leftFootRotation", params.leftFootOrientation);
+            _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition);
+        } else {
+            _animVars.unset("leftFootPosition");
+            _animVars.unset("leftFootRotation");
+            _animVars.set("leftFootType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
+        }
+
+        if (params.isRightFootEnabled) {
+            _animVars.set("rightFootPosition", params.rightFootPosition);
+            _animVars.set("rightFootRotation", params.rightFootOrientation);
+            _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition);
+        } else {
+            _animVars.unset("rightFootPosition");
+            _animVars.unset("rightFootRotation");
+            _animVars.set("rightFootType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
+        }
+
     }
 }
 

libraries/animation/src/Rig.h

@@ -60,7 +60,7 @@ public:
         int rightEyeJointIndex = -1;
     };
 
-    struct HandParameters {
+    struct HandAndFeetParameters {
         bool isLeftEnabled;
         bool isRightEnabled;
         float bodyCapsuleRadius;
@@ -70,6 +70,13 @@ public:
         glm::quat leftOrientation = glm::quat();  // rig space (z forward)
         glm::vec3 rightPosition = glm::vec3();    // rig space
         glm::quat rightOrientation = glm::quat(); // rig space (z forward)
+
+        bool isLeftFootEnabled;
+        bool isRightFootEnabled;
+        glm::vec3 leftFootPosition = glm::vec3();     // rig space
+        glm::quat leftFootOrientation = glm::quat();  // rig space (z forward)
+        glm::vec3 rightFootPosition = glm::vec3();    // rig space
+        glm::quat rightFootOrientation = glm::quat(); // rig space (z forward)
     };
 
     enum class CharacterControllerState {
@@ -185,7 +192,7 @@ public:
 
     void updateFromHeadParameters(const HeadParameters& params, float dt);
     void updateFromEyeParameters(const EyeParameters& params);
-    void updateFromHandParameters(const HandParameters& params, float dt);
+    void updateFromHandAndFeetParameters(const HandAndFeetParameters& params, float dt);
 
     void initAnimGraph(const QUrl& url);
 

libraries/controllers/src/controllers/Actions.cpp

@@ -51,6 +51,8 @@ namespace controller {
 
             makePosePair(Action::LEFT_HAND, "LeftHand"),
             makePosePair(Action::RIGHT_HAND, "RightHand"),
+            makePosePair(Action::LEFT_FOOT, "LeftFoot"),
+            makePosePair(Action::RIGHT_FOOT, "RightFoot"),
 
             makeButtonPair(Action::LEFT_HAND_CLICK, "LeftHandClick"),
             makeButtonPair(Action::RIGHT_HAND_CLICK, "RightHandClick"),

libraries/controllers/src/controllers/Actions.h

@@ -42,6 +42,8 @@ enum class Action {
 
     LEFT_HAND = NUM_COMBINED_AXES,
     RIGHT_HAND,
+    LEFT_FOOT,
+    RIGHT_FOOT,
 
     LEFT_HAND_CLICK,
     RIGHT_HAND_CLICK,

libraries/controllers/src/controllers/StandardController.cpp

@@ -102,6 +102,8 @@ Input::NamedVector StandardController::getAvailableInputs() const {
         // Poses
         makePair(LEFT_HAND, "LeftHand"),
         makePair(RIGHT_HAND, "RightHand"),
+        makePair(LEFT_FOOT, "LeftFoot"),
+        makePair(RIGHT_FOOT, "RightFoot"),
 
         // Aliases, PlayStation style names
         makePair(LB, "L1"),

script-archive/controllers/proceduralFootPoseExample.js

@@ -0,0 +1,81 @@
+//
+//  proceduralFootPoseExample.js
+//  examples/controllers
+//
+//  Created by Brad Hefta-Gaub on 2015/12/15
+//  Copyright 2015 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+
+var MAPPING_NAME = "com.highfidelity.examples.proceduralFootPose";
+var mapping = Controller.newMapping(MAPPING_NAME);
+var translation = { x: 0, y: 0.1, z: 0 };
+var translationDx = 0.01;
+var translationDy = 0.01;
+var translationDz = -0.01;
+var TRANSLATION_LIMIT = 0.5;
+
+var pitch = 45;
+var yaw = 0;
+var roll = 45;
+var pitchDelta = 1;
+var yawDelta = -1;
+var rollDelta = 1;
+var ROTATION_MIN = -90;
+var ROTATION_MAX = 90;
+
+mapping.from(function() {
+
+    // adjust the hand translation in a periodic back and forth motion for each of the 3 axes
+    translation.x = translation.x + translationDx;
+    translation.y = translation.y + translationDy;
+    translation.z = translation.z + translationDz;
+    if ((translation.x > TRANSLATION_LIMIT) || (translation.x < (-1 * TRANSLATION_LIMIT))) {
+        translationDx = translationDx * -1;
+    }
+    if ((translation.y > TRANSLATION_LIMIT) || (translation.y < (-1 * TRANSLATION_LIMIT))) {
+        translationDy = translationDy * -1;
+    }
+    if ((translation.z > TRANSLATION_LIMIT) || (translation.z < (-1 * TRANSLATION_LIMIT))) {
+        translationDz = translationDz * -1;
+    }
+
+    // adjust the hand rotation in a periodic back and forth motion for each of pitch/yaw/roll
+    pitch = pitch + pitchDelta;
+    yaw = yaw + yawDelta;
+    roll = roll + rollDelta;
+    if ((pitch > ROTATION_MAX) || (pitch < ROTATION_MIN)) {
+        pitchDelta = pitchDelta * -1;
+    }
+    if ((yaw > ROTATION_MAX) || (yaw < ROTATION_MIN)) {
+        yawDelta = yawDelta * -1;
+    }
+    if ((roll > ROTATION_MAX) || (roll < ROTATION_MIN)) {
+        rollDelta = rollDelta * -1;
+    }
+
+    var rotation = Quat.fromPitchYawRollDegrees(pitch, yaw, roll);
+
+    var pose = {
+            translation: translation,
+            rotation: rotation,
+            velocity: { x: 0, y: 0, z: 0 },
+            angularVelocity: { x: 0, y: 0, z: 0 }
+        };
+
+    Vec3.print("foot translation:", translation);
+    return pose;
+}).debug(true).to(Controller.Standard.LeftFoot);
+
+//mapping.from(Controller.Standard.LeftFoot).debug(true).to(Controller.Actions.LeftFoot);
+
+
+Controller.enableMapping(MAPPING_NAME);
+
+
+Script.scriptEnding.connect(function(){
+    mapping.disable();
+});

script-archive/controllers/puppetFeet2.js

@@ -0,0 +1,35 @@
+//
+//  proceduralFootPoseExample.js
+//  examples/controllers
+//
+//  Created by Brad Hefta-Gaub on 2015/12/15
+//  Copyright 2015 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+
+var MAPPING_NAME = "com.highfidelity.examples.proceduralFootPose";
+var mapping = Controller.newMapping(MAPPING_NAME);
+var puppetOffset = { x: 0, y: -1, z: 0 };
+
+mapping.from(function() {
+    var leftHandPose = Controller.getPoseValue(Controller.Standard.LeftHand);
+
+    var pose = {
+            translation: Vec3.sum(leftHandPose.translation, puppetOffset),
+            rotation: { x: 0, y: 0, z: 0, w: 0 }, //leftHandPose.rotation,
+            velocity: { x: 0, y: 0, z: 0 },
+            angularVelocity: { x: 0, y: 0, z: 0 }
+        };
+    return pose;
+}).to(Controller.Standard.LeftFoot);
+
+
+Controller.enableMapping(MAPPING_NAME);
+
+
+Script.scriptEnding.connect(function(){
+    mapping.disable();
+});