added refName to TSController to allow different coordinate system for IK solver

source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice

@@ -60,6 +60,7 @@ module armarx
         // velocity controller configuration
         string nodeSetName = "";
         string tcpName = "";
+        string frameName = "";
         NJointTaskSpaceDMPControllerMode::CartesianSelection mode = NJointTaskSpaceDMPControllerMode::eAll;
 
         double maxLinearVel;

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp

@@ -41,6 +41,7 @@ namespace armarx
         };
 
         tcp = (cfg->tcpName.empty()) ? rns->getTCP() : rtGetRobot()->getRobotNode(cfg->tcpName);
+        refFrame = (cfg->frameName.empty()) ? rns->getRobot()->getRootNode() : rtGetRobot()->getRobotNode(cfg->frameName);
         ARMARX_CHECK_EXPRESSION_W_HINT(tcp, cfg->tcpName);
 
         // set tcp controller
@@ -48,7 +49,7 @@ namespace armarx
         nodeSetName = cfg->nodeSetName;
         torquePIDs.resize(tcpController->rns->getSize(), pidController());
 
-        ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+        ik.reset(new VirtualRobot::DifferentialIK(rns, refFrame, VirtualRobot::JacobiProvider::eSVDDamped));
 
 
         finished = false;
@@ -103,7 +104,9 @@ namespace armarx
             initData.targetTSVel(i) = 0;
             targetVels(i) = 0;
         }
-        initData.targetPose = tcp->getPoseInRootFrame();
+        //        initData.targetPose = tcp->getPoseInRootFrame();
+        initData.targetPose = refFrame->toLocalCoordinateSystem(tcp->getGlobalPose());
+
         initData.nullspaceJointVelocities.resize(tcpController->rns->getSize(), 0);
         initData.torqueKp.resize(tcpController->rns->getSize(), 0);
         initData.torqueKd.resize(tcpController->rns->getSize(), 0);
@@ -212,10 +215,36 @@ namespace armarx
         writeControlStruct();
     }
 
+    Eigen::VectorXf NJointTSDMPController::calcIK(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel,  VirtualRobot::IKSolver::CartesianSelection mode)
+    {
+        Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, mode);
+
+        Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
+
+        Eigen::MatrixXf nullspace = lu_decomp.kernel();
+        Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
+        for (int i = 0; i < nullspace.cols(); i++)
+        {
+            float squaredNorm = nullspace.col(i).squaredNorm();
+            // Prevent division by zero
+            if (squaredNorm > 1.0e-32f)
+            {
+                nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
+            }
+        }
+
+        Eigen::MatrixXf inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
+        ARMARX_INFO << "inv: " << inv;
+        Eigen::VectorXf jointVel = inv * cartesianVel;
+        //        jointVel += nsv;
+        return jointVel;
+    }
 
     void NJointTSDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
     {
-        Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
+        //        Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
+        //
+        Eigen::Matrix4f currentPose = refFrame->toLocalCoordinateSystem(tcp->getGlobalPose());
         rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
         rt2UserData.commitWrite();
 
@@ -315,7 +344,9 @@ namespace armarx
                 jnv(i) += rtGetControlStruct().nullspaceJointVelocities.at(i);
             }
 
-            jointTargetVelocities = tcpController->calculate(x, jnv, VirtualRobot::IKSolver::CartesianSelection::All);
+            //            jointTargetVelocities = tcpController->calculate(x, jnv, VirtualRobot::IKSolver::CartesianSelection::All);
+            jointTargetVelocities = calcIK(x, jnv, VirtualRobot::IKSolver::CartesianSelection::All);
+
             ARMARX_CHECK_EXPRESSION(!targets.empty());
             ARMARX_CHECK_LESS(targets.size(), 1000);
         }

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h

@@ -106,6 +106,7 @@ namespace armarx
         std::vector<double> createDMPFromString(const std::string& dmpString, const Ice::Current&) override;
 
         VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
+        Eigen::VectorXf calcIK(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspace, VirtualRobot::IKSolver::CartesianSelection mode);
     protected:
         void rtPreActivateController() override;
         void rtPostDeactivateController() override;
@@ -183,6 +184,7 @@ namespace armarx
 
         VirtualRobot::DifferentialIKPtr ik;
         VirtualRobot::RobotNodePtr tcp;
+        VirtualRobot::RobotNodePtr refFrame;
         Eigen::VectorXf targetVels;
         Eigen::Matrix4f targetPose;