SimpleDiffIK: Update all joint values at once and not per node

Improves performance significantly, since for every joint angle update Simox will recursively update all node poses.
Now, this update is only done once

source/RobotAPI/libraries/core/SimpleDiffIK.cpp

@@ -35,6 +35,7 @@ SimpleDiffIK::Result SimpleDiffIK::CalculateDiffIK(const Eigen::Matrix4f targetP
     CartesianVelocityController velocityController(rns);
     CartesianPositionController positionController(tcp);
 
+    Eigen::VectorXf currentJointValues = rns->getJointValuesEigen();
     for (size_t i = 0; i <= params.stepsInitial + params.stepsFineTune; i++)
     {
         Eigen::Vector3f posDiff = positionController.getPositionDiff(targetPose);
@@ -51,10 +52,9 @@ SimpleDiffIK::Result SimpleDiffIK::CalculateDiffIK(const Eigen::Matrix4f targetP
         float stepLength = i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
         jv = jv * stepLength;
 
-        for (size_t n = 0; n < rns->getSize(); n++)
-        {
-            rns->getNode(n)->setJointValue(rns->getNode(n)->getJointValue() + jv(n));
-        }
+        Eigen::VectorXf newJointValues = currentJointValues + jv;
+        rns->setJointValues(newJointValues);
+        currentJointValues = newJointValues;
     }
 
     Result result;