From 0241b0cde8a15c9e413d7e4b5cdf6e72466f762b Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Mon, 18 Jan 2016 14:47:02 +0100
Subject: [PATCH] HeadIK uses also not so good results
---
.../RobotAPI/components/units/HeadIKUnit.cpp | 71 +++++++++++--------
1 file changed, 40 insertions(+), 31 deletions(-)
diff --git a/source/RobotAPI/components/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp
index 0ab897d7d..46dcd0ab5 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";
+ }
+
}
}
--
GitLab