the friction coeffitient of the floor plane can be modified; more meaningful...

the friction coeffitient of the floor plane can be modified; more meaningful error messages for Hierachical IK

SimDynamics/DynamicsWorld.cpp

@@ -104,9 +104,9 @@ namespace SimDynamics
         return factory->createObject(o);
     }
 
-    void DynamicsWorld::createFloorPlane(const Eigen::Vector3f& pos /*= Eigen::Vector3f(0,0,0)*/, const Eigen::Vector3f& up /*= Eigen::Vector3f(0,0,1.0f)*/)
+    void DynamicsWorld::createFloorPlane(const Eigen::Vector3f& pos /*= Eigen::Vector3f(0,0,0)*/, const Eigen::Vector3f& up /*= Eigen::Vector3f(0,0,1.0f)*/, float friction)
     {
-        engine->createFloorPlane(pos, up);
+        engine->createFloorPlane(pos, up, friction);
     }
 
     bool DynamicsWorld::addRobot(DynamicsRobotPtr r)

SimDynamics/DynamicsWorld.h

@@ -94,7 +94,7 @@ namespace SimDynamics
             \param pos The displacement
             \param up The up vector. Currently supported: (+/-1,0,0), (0,+/-1,0) or (0,0,+/-1).
         */
-        void createFloorPlane(const Eigen::Vector3f& pos = Eigen::Vector3f(0, 0, 0), const Eigen::Vector3f& up = Eigen::Vector3f(0, 0, 1.0f));
+        void createFloorPlane(const Eigen::Vector3f& pos = Eigen::Vector3f(0, 0, 0), const Eigen::Vector3f& up = Eigen::Vector3f(0, 0, 1.0f), float friction = -1);
 
         /*!
             Since VirtualRobot usually uses MM and most physics engines like M, the models are automatically converted if this bool is set.

VirtualRobot/IK/ConstrainedHierarchicalIK.cpp

@@ -41,10 +41,17 @@ bool ConstrainedHierarchicalIK::solveStep()
     // Check if any of the hard constraints has increased error
     for (auto & constraint : constraints)
     {
-        if (hardConstraints[constraint] && (constraint->getErrorDifference() < -raiseEpsilon))
+        if (hardConstraints[constraint])
         {
-            VR_INFO << "Constrained IK failed due to error raise for hard constraint " << std::endl;
-            return false;
+            auto diff = constraint->getErrorDifference();
+            if (diff < -raiseEpsilon)
+            {
+                VR_INFO << "Constrained IK failed due to error raise for hard constraint. "
+                        << "hardConstraints[constraint]" << hardConstraints[constraint]
+                        << " getErrorDifference()=" << diff
+                        << " raiseEpsilon=" << raiseEpsilon << std::endl;
+                return false;
+            }
         }
     }
 

VirtualRobot/IK/ConstrainedIK.cpp

@@ -116,6 +116,7 @@ bool ConstrainedIK::solve(bool stepwise)
             if (hardConstraints[constraint] && !constraint->checkTolerances())
             {
                 goalReached = false;
+                break;
             }
         }
 
@@ -134,6 +135,7 @@ bool ConstrainedIK::solve(bool stepwise)
         }
     }
 
+    VR_INFO << "A hard contraints failed to me the tolerance criterion after the maximum number of iterations" << std::endl;
     running = false;
     return false;
 }

VirtualRobot/RobotNodeSet.cpp

@@ -30,8 +30,10 @@ namespace VirtualRobot
 
         if (!isKinematicRoot(this->kinematicRoot))
         {
-            VR_WARNING << "RobotNodeSet " << name << " initialized with invalid kinematic root '" << kinematicRoot->getName() << "': Falling back to robot root node" << endl;
             this->kinematicRoot = rob->getRootNode();
+            VR_WARNING << "RobotNodeSet " << name << " initialized with invalid kinematic root '"
+                       << kinematicRoot->getName() << "': Falling back to robot root node '"
+                       <<  this->kinematicRoot->getName() << "'" << endl;
         }
 
         if (!tcp && robotNodes.size() > 0)