HeadIK uses also not so good results

source/RobotAPI/components/units/HeadIKUnit.cpp

@@ -280,47 +280,56 @@ namespace armarx
 
             if (!solutionFound)
             {
-                ARMARX_WARNING << "IKSolver found no solution!";
-            }
-            else
-            {
-                ARMARX_DEBUG << "Solution found";
-
-                if (drawer && localRobot->hasRobotNode("Cameras"))
+                Eigen::Vector3f position = globalPos->toEigen() - kinematicChain->getTCP()->getGlobalPose().block(0, 3, 3, 1);
+                float error = position.norm();
+                if (error < 200)
+                {
+                    ARMARX_INFO << "IKSolver found no solution! applying best solution with " << error << "mm error";
+                }
+                else
                 {
-                    Vector3Ptr startPos = new Vector3(localRobot->getRobotNode("Cameras")->getGlobalPose());
-                    drawer->setSphereDebugLayerVisu("HeadViewTargetSolution",
-                                                    startPos,
-                                                    DrawColor {0, 1, 1, 0.2},
-                                                    17);
+                    ARMARX_WARNING << "IKSolver found no solution!  " << error << "mm error";
+                    return;
                 }
+            }
 
+            ARMARX_DEBUG << "Solution found";
 
-                NameValueMap targetJointAngles;
-                NameControlModeMap controlModes;
+            if (drawer && localRobot->hasRobotNode("Cameras"))
+            {
+                Vector3Ptr startPos = new Vector3(localRobot->getRobotNode("Cameras")->getGlobalPose());
+                drawer->setSphereDebugLayerVisu("HeadViewTargetSolution",
+                                                startPos,
+                                                DrawColor {0, 1, 1, 0.2},
+                                                17);
+            }
 
-                for (int i = 0; i < (signed int)kinematicChain->getSize(); i++)
-                {
-                    if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") != 0)
-                    {
-                        targetJointAngles[kinematicChain->getNode(i)->getName()] = kinematicChain->getNode(i)->getJointValue();
-                        controlModes[kinematicChain->getNode(i)->getName()] = ePositionControl;
-                    }
 
-                    ARMARX_DEBUG << kinematicChain->getNode(i)->getName() << ": " << kinematicChain->getNode(i)->getJointValue();
-                }
+            NameValueMap targetJointAngles;
+            NameControlModeMap controlModes;
 
-                try
-                {
-                    kinematicUnitPrx->switchControlMode(controlModes);
-                    ARMARX_DEBUG << "setting new joint angles" << targetJointAngles;
-                    kinematicUnitPrx->setJointAngles(targetJointAngles);
-                }
-                catch (...)
+            for (int i = 0; i < (signed int)kinematicChain->getSize(); i++)
+            {
+                if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") != 0)
                 {
-                    ARMARX_IMPORTANT << "setJointAngles failed";
+                    targetJointAngles[kinematicChain->getNode(i)->getName()] = kinematicChain->getNode(i)->getJointValue();
+                    controlModes[kinematicChain->getNode(i)->getName()] = ePositionControl;
                 }
+
+                ARMARX_DEBUG << kinematicChain->getNode(i)->getName() << ": " << kinematicChain->getNode(i)->getJointValue();
+            }
+
+            try
+            {
+                kinematicUnitPrx->switchControlMode(controlModes);
+                ARMARX_DEBUG << "setting new joint angles" << targetJointAngles;
+                kinematicUnitPrx->setJointAngles(targetJointAngles);
             }
+            catch (...)
+            {
+                ARMARX_IMPORTANT << "setJointAngles failed";
+            }
+
         }
     }