extended headik: multiple kinematic chains can be given and they are used one...

extended headik: multiple kinematic chains can be given and they are used one after another to find a solution

source/RobotAPI/components/units/HeadIKUnit.cpp

@@ -114,7 +114,11 @@ namespace armarx
     {
         ScopedLock lock(accessMutex);
 
-        this->robotNodeSetName = robotNodeSetName;
+        this->robotNodeSetNames = armarx::Split(robotNodeSetName, ",");
+        for (auto& setName : robotNodeSetNames)
+        {
+            boost::trim(setName);
+        }
         this->targetPosition->x = targetPosition->x;
         this->targetPosition->y = targetPosition->y;
         this->targetPosition->z = targetPosition->z;
@@ -247,53 +251,95 @@ namespace armarx
         if (doCalculation)
         {
             ScopedLock lock(accessMutex);
-            RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponentPrx);
-            //VirtualRobot::RobotPtr robotSnapshot(new RemoteRobot(remoteRobotPrx));
-            //localRobot->setConfig(robotSnapshot->getConfig());
 
-            VirtualRobot::RobotNodeSetPtr kinematicChain = localRobot->getRobotNodeSet(robotNodeSetName);
-            VirtualRobot::RobotNodePrismaticPtr virtualJoint;
-
-            for (unsigned int i = 0; i < kinematicChain->getSize(); i++)
+            VirtualRobot::RobotNodeSetPtr kinematicChain;
+            bool foundSolution = false;
+            NameValueMap targetJointAngles;
+            NameControlModeMap controlModes;
+            std::set<std::string> possiblyInvolvedJointNames;
+            //            // set all involved joints initially to zero
+            for (auto robotNodeSetName : robotNodeSetNames)
             {
-                if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") == 0)
+                kinematicChain = localRobot->getRobotNodeSet(robotNodeSetName);
+                for (unsigned int i = 0; i < kinematicChain->getSize(); i++)
                 {
-                    virtualJoint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(kinematicChain->getNode(i));
+                    possiblyInvolvedJointNames.insert(kinematicChain->getNode(i)->getName());
+                    //                                kinematicChain->getNode(i)->setJointValue(0.0f);
+                    //                                targetJointAngles[kinematicChain->getNode(i)->getName()] = 0.0f;
+                    //                                controlModes[kinematicChain->getNode(i)->getName()] = ePositionControl;
                 }
             }
-
-            ARMARX_DEBUG << deactivateSpam(5) << VAROUT(virtualJoint->getName()) << " " << VAROUT(kinematicChain->getName());
-            VirtualRobot::GazeIK ikSolver(kinematicChain, virtualJoint);
-            ikSolver.enableJointLimitAvoidance(true);
-            ikSolver.setup(10, 30, 20);
-            //            ikSolver.setVerbose(true);
-            auto globalPos = targetPosition->toGlobal(localRobot);
-            ARMARX_DEBUG << "Calculating IK for target position " << globalPos->output();
-            auto start = IceUtil::Time::now();
-            bool solutionFound = ikSolver.solve(globalPos->toEigen());
-            auto duration = (IceUtil::Time::now() - start);
-
-            if (duration.toMilliSeconds() > 500)
+            float error = -1;
+            //VirtualRobot::RobotPtr robotSnapshot(new RemoteRobot(remoteRobotPrx));
+            //localRobot->setConfig(robotSnapshot->getConfig());
+            std::string selectedRobotNodeSetName;
+            for (auto robotNodeSetName : robotNodeSetNames)
             {
-                ARMARX_INFO << "Calculating Gaze IK took " << duration.toMilliSeconds() << " ms";
-            }
+                RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponentPrx);
+                kinematicChain = localRobot->getRobotNodeSet(robotNodeSetName);
 
-            if (!solutionFound)
-            {
+                VirtualRobot::RobotNodePrismaticPtr virtualJoint;
+
+                for (unsigned int i = 0; i < kinematicChain->getSize(); i++)
+                {
+                    if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") == 0)
+                    {
+                        virtualJoint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(kinematicChain->getNode(i));
+                    }
+
+                }
+                // set other not-used joints to 0
+                for (auto& nodeName : possiblyInvolvedJointNames)
+                {
+                    if (!kinematicChain->hasRobotNode(nodeName))
+                    {
+                        localRobot->getRobotNode(nodeName)->setJointValue(0.0f);
+                        targetJointAngles[nodeName] = 0.0f;
+                        controlModes[nodeName] = ePositionControl;
+                    }
+                }
+
+                ARMARX_DEBUG << deactivateSpam(5) << VAROUT(virtualJoint->getName()) << " " << VAROUT(kinematicChain->getName());
+                VirtualRobot::GazeIK ikSolver(kinematicChain, virtualJoint);
+                ikSolver.enableJointLimitAvoidance(true);
+                ikSolver.setup(10, 30, 20);
+                //            ikSolver.setVerbose(true);
+                auto globalPos = targetPosition->toGlobal(localRobot);
+                ARMARX_DEBUG << "Calculating IK for target position " << globalPos->output();
+                auto start = IceUtil::Time::now();
+                bool solutionFound = ikSolver.solve(globalPos->toEigen());
+                auto duration = (IceUtil::Time::now() - start);
+
+                if (duration.toMilliSeconds() > 500)
+                {
+                    ARMARX_INFO << "Calculating Gaze IK took " << duration.toMilliSeconds() << " ms";
+                }
                 Eigen::Vector3f position = globalPos->toEigen() - kinematicChain->getTCP()->getGlobalPose().block(0, 3, 3, 1);
-                float error = position.norm();
-                if (error < 200)
+                error = position.norm();
+                if (!solutionFound)
                 {
-                    ARMARX_INFO << "IKSolver found no solution! applying best solution with " << error << "mm error";
+
+                    if (error < 150)
+                    {
+                        foundSolution = true;
+                        selectedRobotNodeSetName = robotNodeSetName;
+                        ARMARX_INFO << "IKSolver found no solution! applying best solution with " << error << "mm error";
+                        break;
+                    }
                 }
                 else
                 {
-                    ARMARX_WARNING << "IKSolver found no solution!  " << error << "mm error";
-                    return;
+                    foundSolution = true;
+                    selectedRobotNodeSetName = robotNodeSetName;
+                    break;
                 }
             }
-
-            ARMARX_DEBUG << "Solution found";
+            if (!foundSolution)
+            {
+                ARMARX_WARNING << "IKSolver found no solution!  " << error << "mm error";
+                return;
+            }
+            ARMARX_DEBUG << "Solution found with " << selectedRobotNodeSetName << " - remaining error: " << error << " mm";
 
             if (drawer && localRobot->hasRobotNode("Cameras") && getProperty<bool>("VisualizeIKTarget").getValue())
             {
@@ -304,10 +350,6 @@ namespace armarx
                                                 17);
             }
 
-
-            NameValueMap targetJointAngles;
-            NameControlModeMap controlModes;
-
             for (int i = 0; i < (signed int)kinematicChain->getSize(); i++)
             {
                 if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") != 0)

source/RobotAPI/components/units/HeadIKUnit.h

@@ -110,7 +110,7 @@ namespace armarx
         VirtualRobot::RobotPtr localRobot;
         DebugDrawerInterfacePrx drawer;
 
-        std::string robotNodeSetName;
+        Ice::StringSeq robotNodeSetNames;
         FramedPositionPtr targetPosition;
         bool newTargetSet;