Get IK error and stand up based on it

interface/src/avatar/MyAvatar.cpp

@@ -2517,6 +2517,10 @@ bool MyAvatar::clearPinOnJoint(int index) {
     return false;
 }
 
+float MyAvatar::getIKErrorOnLastSolve() const {
+    return _rig->getIKErrorOnLastSolve();
+}
+
 // thread-safe
 void MyAvatar::addHoldAction(AvatarActionHold* holdAction) {
     std::lock_guard<std::mutex> guard(_holdActionsMutex);

interface/src/avatar/MyAvatar.h

@@ -219,6 +219,8 @@ public:
     Q_INVOKABLE bool pinJoint(int index, const glm::vec3& position, const glm::quat& orientation);
     Q_INVOKABLE bool clearPinOnJoint(int index);
 
+    Q_INVOKABLE float getIKErrorOnLastSolve() const;
+
     Q_INVOKABLE void useFullAvatarURL(const QUrl& fullAvatarURL, const QString& modelName = QString());
     Q_INVOKABLE QUrl getFullAvatarURLFromPreferences() const { return _fullAvatarURLFromPreferences; }
     Q_INVOKABLE QString getFullAvatarModelName() const { return _fullAvatarModelName; }

libraries/animation/src/AnimInverseKinematics.cpp

@@ -189,6 +189,7 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
             }
         }
     }
+    _maxErrorOnLastSolve = maxError;
 
     // finally set the relative rotation of each tip to agree with absolute target rotation
     for (auto& target: targets) {

libraries/animation/src/AnimInverseKinematics.h

@@ -41,6 +41,8 @@ public:
 
     void setMaxHipsOffsetLength(float maxLength);
 
+    float getMaxErrorOnLastSolve() { return _maxErrorOnLastSolve; }
+
 protected:
     void computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets, const AnimPoseVec& underPoses);
     void solveWithCyclicCoordinateDescent(const std::vector<IKTarget>& targets);
@@ -93,6 +95,8 @@ protected:
     // _maxTargetIndex is tracked to help optimize the recalculation of absolute poses
     // during the the cyclic coordinate descent algorithm
     int _maxTargetIndex { 0 };
+
+    float _maxErrorOnLastSolve { FLT_MAX };
 };
 
 #endif // hifi_AnimInverseKinematics_h

libraries/animation/src/Rig.cpp

@@ -337,6 +337,21 @@ float Rig::getMaxHipsOffsetLength() const {
     return _maxHipsOffsetLength;
 }
 
+float Rig::getIKErrorOnLastSolve() const {
+    float result = 0.0f;
+
+    if (_animNode) {
+        _animNode->traverse([&](AnimNode::Pointer node) {
+            auto ikNode = std::dynamic_pointer_cast<AnimInverseKinematics>(node);
+            if (ikNode) {
+                result = ikNode->getMaxErrorOnLastSolve();
+            }
+            return true;
+        });
+    }
+    return result;
+}
+
 int Rig::getJointParentIndex(int childIndex) const {
     if (_animSkeleton && isIndexValid(childIndex)) {
         return _animSkeleton->getParentIndex(childIndex);

libraries/animation/src/Rig.h

@@ -107,6 +107,8 @@ public:
     void setMaxHipsOffsetLength(float maxLength);
     float getMaxHipsOffsetLength() const;
 
+    float getIKErrorOnLastSolve() const;
+
     int getJointParentIndex(int childIndex) const;
 
     // geometry space
@@ -321,6 +323,7 @@ protected:
 
     mutable uint32_t _jointNameWarningCount { 0 };
     float _maxHipsOffsetLength { 1.0f };
+    float _maxErrorOnLastSolve { 0.0f };
 
 private:
     QMap<int, StateHandler> _stateHandlers;

scripts/tutorials/entity_scripts/sit.js

@@ -9,6 +9,7 @@
   var RELEASE_KEYS = ['w', 'a', 's', 'd', 'UP', 'LEFT', 'DOWN', 'RIGHT'];
   var RELEASE_TIME = 500; // ms
   var RELEASE_DISTANCE = 0.2; // meters
+  var MAX_IK_ERROR = 15;
 
   this.entityID = null;
   this.timers = {};
@@ -21,7 +22,10 @@
   this.update = function(dt) {
     if (MyAvatar.getParentID() === this.entityID) {
       var properties = Entities.getEntityProperties(this.entityID, ["position"]);
-      if (Vec3.distance(MyAvatar.position, properties.position) > RELEASE_DISTANCE) {
+      var avatarDistance = Vec3.distance(MyAvatar.position, properties.position);
+      var ikError = MyAvatar.getIKErrorOnLastSolve();
+      print("IK error: " + ikError);
+      if (avatarDistance > RELEASE_DISTANCE || ikError > MAX_IK_ERROR) {
         this.sitUp(this.entityID);
       }
     }