diff --git a/source/RobotAPI/components/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp index 0ab897d7df74739d7cd7921a6cf0f81a1c25a8e2..46dcd0ab50fc6bca4e71b3015614707f949fde63 100644 --- a/source/RobotAPI/components/units/HeadIKUnit.cpp +++ b/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"; + } + } }