minor fix of DiffIK (failed when tcp was not a RobotNode)

git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@770 042f3d55-54a8-47e9-b7fb-15903f145c44

VirtualRobot/IK/DifferentialIK.cpp

@@ -321,18 +321,6 @@ namespace VirtualRobot
         //  THROW_VR_EXCEPTION_IF(!tcp,boost::format("No tcp defined in node set \"%1%\" of robot %2% (DifferentialIK::%3% )") % this->rns->getName() % this->rns->getRobot()->getName() % BOOST_CURRENT_FUNCTION);
 
         RobotNodePtr tcpRN = boost::dynamic_pointer_cast<RobotNode>(tcp);
-
-        if (parents.find(tcpRN) == parents.end())
-        {
-            parents[tcpRN] = tcpRN->getAllParents(rns);
-            parents[tcpRN].push_back(tcpRN);
-        }
-
-        Eigen::Vector3f axis;
-        Eigen::Vector3f toTCP;
-        tmpUpdateJacobianPosition.setZero();
-        tmpUpdateJacobianOrientation.setZero();
-
         if (!tcpRN)
         {
             if (!tcp->getParent())
@@ -352,6 +340,18 @@ namespace VirtualRobot
             }
         }
 
+        if (parents.find(tcpRN) == parents.end())
+        {
+            parents[tcpRN] = tcpRN->getAllParents(rns);
+            parents[tcpRN].push_back(tcpRN);
+        }
+
+        Eigen::Vector3f axis;
+        Eigen::Vector3f toTCP;
+        tmpUpdateJacobianPosition.setZero();
+        tmpUpdateJacobianOrientation.setZero();
+
+
         // Iterate over all degrees of freedom
         for (size_t i = 0; i < nDoF; i++)
         {