Fix the desired angular velocity in IK to prevent flip motion.

2016-10-05 14:24:49 -07:00
parent ae5844d4fb
commit 0d58ebb311
2 changed files with 6 additions and 1 deletions
--- a/examples/SharedMemory/IKTrajectoryHelper.cpp
+++ b/examples/SharedMemory/IKTrajectoryHelper.cpp
@@ -82,6 +82,12 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
        btQuaternion deltaQ = endQ*startQ.inverse();
        float angle = deltaQ.getAngle();
        btVector3 axis = deltaQ.getAxis();
+        if (angle > PI) {
+            angle -= 2.0*PI;
+        }
+        else if (angle < -PI) {
+            angle += 2.0*PI;
+        }
        float angleDot = angle;
        btVector3 angularVel = angleDot*axis.normalize();
        for (int i = 0; i < 3; ++i)