fixed velocity calculation in robotstateobserver

source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp

@@ -22,6 +22,7 @@
 
 #include "RobotStateObserver.h"
 #include <RobotAPI/interface/core/RobotState.h>
+#include <ArmarXCore/core/util/algorithm.h>
 #include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
 #include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
 #include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
@@ -35,6 +36,7 @@
 #include <VirtualRobot/RobotConfig.h>
 #include <VirtualRobot/VirtualRobot.h>
 #include <ArmarXCore/observers/variant/DatafieldRef.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
 
 #include <boost/algorithm/string/trim.hpp>
 
@@ -206,53 +208,38 @@ void RobotStateObserver::updateNodeVelocities(const NameValueMap& jointVel, long
         tcpPoses[tcpName] = new FramedPose(currentPose, GlobalFrame, "");
 
     }
-    IceUtil::Time currentTimestamp = IceUtil::Time::microSeconds(timestampMicroSeconds);
 
-    double tDelta = 0.0f;
-    if (lastVelocityUpdate.toMicroSeconds() > 0)
-    {
-        tDelta = (lastVelocityUpdate - currentTimestamp).toMilliSecondsDouble();
-    }
-
-    for (NameValueMap::iterator it = tempJointAngles.begin(); it != tempJointAngles.end(); it++)
-    {
-        NameValueMap::const_iterator itSrc = jointVel.find(it->first);
-
-        if (itSrc != jointVel.end())
-        {
-            it->second += itSrc->second * tDelta;
-        }
-    }
 
     velocityReportRobot->setJointValues(tempJointAngles);
     velocityReportRobot->setGlobalPose(robot->getGlobalPose());
-    //    ARMARX_INFO << deactivateSpam(1) << "duration: " << (IceUtil::Time::now()-start).toMicroSecondsDouble();
-    //    start = IceUtil::Time::now();
+
     Eigen::Matrix4f mat;
     Eigen::Vector3f rpy;
 
-    for (unsigned int i = 0; i < nodesToReport.size(); i++)
+    auto keys = armarx::getMapKeys(jointVel);
+    Eigen::VectorXf jointVelocities(jointVel.size());
+    auto rootFrameName = velocityReportRobot->getRootNode()->getName();
+    RobotNodeSetPtr rns = RobotNodeSet::createRobotNodeSet(velocityReportRobot, "All_Nodes", keys, rootFrameName);
+    for (size_t i = 0; i < rns->getSize(); ++i)
     {
-        RobotNodePtr node = velocityReportRobot->getRobotNode(nodesToReport.at(i).first->getName());
-        const std::string& tcpName  = node->getName();
-        const Eigen::Matrix4f& currentPose = node->getGlobalPose();
-
+        jointVelocities(i) = jointVel.at(rns->getNode(i)->getName());
+    }
+    DifferentialIKPtr j(new DifferentialIK(rns));
 
-        FramedPosePtr lastPose = FramedPosePtr::dynamicCast(tcpPoses[tcpName]);
 
-        tcpTranslationVelocities[tcpName] = new FramedDirection((currentPose.block(0, 3, 3, 1) - lastPose->toEigen().block(0, 3, 3, 1)) / tDelta, GlobalFrame, "");
-        FramedDirectionPtr::dynamicCast(tcpTranslationVelocities[tcpName])->changeFrame(velocityReportRobot, nodesToReport.at(i).second);
-        mat = currentPose * lastPose->toEigen().inverse();
+    auto robotName = velocityReportRobot->getName();
+    for (unsigned int i = 0; i < nodesToReport.size(); i++)
+    {
+        RobotNodePtr node = velocityReportRobot->getRobotNode(nodesToReport.at(i).first->getName());
+        Eigen::MatrixXf jac = j->getJacobianMatrix(node, IKSolver::All);
+        Eigen::VectorXf nodeVel = jac * jointVelocities;
 
-        VirtualRobot::MathTools::eigen4f2rpy(mat, rpy);
+        const std::string tcpName  = node->getName();
+        tcpTranslationVelocities[tcpName] = new FramedDirection(nodeVel.block<3, 1>(0, 0), rootFrameName, robotName);
+        tcpOrientationVelocities[tcpName] = new FramedDirection(nodeVel.block<3, 1>(3, 0), rootFrameName, robotName);
 
-        tcpOrientationVelocities[tcpName] = new FramedDirection(rpy / tDelta, GlobalFrame, "");
-        FramedDirectionPtr::dynamicCast(tcpOrientationVelocities[tcpName])->changeFrame(velocityReportRobot, nodesToReport.at(i).second);
 
     }
-
-    //    ARMARX_INFO << deactivateSpam(1) << "duration2: " << (IceUtil::Time::now()-start).toMicroSecondsDouble();
-    //    ARMARX_INFO << deactivateSpam(5) << "TCP Pose Vel Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
     updateVelocityDatafields(tcpTranslationVelocities, tcpOrientationVelocities);
 }