extended CartesianVelocityController to consider joint limits and disable the...

extended CartesianVelocityController to consider joint limits and disable the joint limits if the they would run over a joint limit

source/RobotAPI/libraries/core/CartesianVelocityController.cpp

@@ -32,8 +32,9 @@
 
 using namespace armarx;
 
-CartesianVelocityController::CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod)
-    : rns(rns)
+CartesianVelocityController::CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod, bool considerJointLimits)
+    : rns(rns),
+      considerJointLimits(considerJointLimits)
 {
     //ARMARX_IMPORTANT << VAROUT(rns->getRobot()->getRootNode());
     ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), invJacMethod));
@@ -55,9 +56,6 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
     //ARMARX_IMPORTANT << VAROUT(tcp.get());
     //ARMARX_IMPORTANT << VAROUT(ik.get());
     jacobi = ik->getJacobianMatrix(tcp, mode);
-    //ARMARX_IMPORTANT << "hi";
-    //ARMARX_IMPORTANT << jacobi;
-    inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
 
     //    ARMARX_IMPORTANT << "nullspaceVel " << nullspaceVel.transpose();
     Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
@@ -79,12 +77,22 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
     //nsv /= kernel.cols();
 
 
-    Eigen::VectorXf jointVel = inv * cartesianVel;
 
+
+    inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
+
+
+    if (considerJointLimits)
+    {
+        adjustJacobiForJointsAtJointLimits(mode, cartesianVel);
+    }
+
+
+    Eigen::VectorXf jointVel = inv * cartesianVel;
     jointVel += nsv;
 
 
-    if(maximumJointVelocities.rows() > 0)
+    if (maximumJointVelocities.rows() > 0)
     {
         jointVel = ::math::Helpers::LimitVectorLength(jointVel, maximumJointVelocities);
     }
@@ -155,3 +163,51 @@ Eigen::VectorXf CartesianVelocityController::calculateRegularization(VirtualRobo
     }
     return regularization.topRows(i);
 }
+
+bool CartesianVelocityController::adjustJacobiForJointsAtJointLimits(VirtualRobot::IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, float jointLimitCheckAccuracy)
+{
+
+    bool modifiedJacobi = false;
+
+    Eigen::VectorXf jointVel = inv * cartesianVel;
+    size_t size = rns->getSize();
+
+    for (size_t i = 0; i < size; ++i)
+    {
+        auto& node = rns->getNode(i);
+
+        if (std::abs(jointVel(i)) < 0.001f)
+        {
+            continue;
+        }
+
+        if ((std::abs(node->getJointValue() - node->getJointLimitHigh()) < jointLimitCheckAccuracy && jointVel(i) > 0)
+            || (std::abs(node->getJointValue() - node->getJointLimitLow()) < jointLimitCheckAccuracy && jointVel(i) < 0))
+        {
+            for (int k = 0; k < jacobi.rows(); ++k) // memory allocation free  resetting of column
+            {
+                jacobi(k, i) = 0.0f;
+            }
+            //                ARMARX_INFO << deactivateSpam(0.5, node->getName()) << " joint " << node->getName() << " is at limit -\n" << inv << "\n initial inv jacobi:\n" << initialInvJac << "\n target cart vel:\n" << cartesianVel  << "\njacobi:\n" << jacobi;
+            modifiedJacobi = true;
+        }
+    }
+
+    if (modifiedJacobi)
+    {
+        // calculate inverse jacobi again since atleast one joint would be driven into the limit
+        inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
+    }
+
+    return modifiedJacobi;
+}
+
+bool CartesianVelocityController::getConsiderJointLimits() const
+{
+    return considerJointLimits;
+}
+
+void CartesianVelocityController::setConsiderJointLimits(bool value)
+{
+    considerJointLimits = value;
+}

source/RobotAPI/libraries/core/CartesianVelocityController.h

@@ -40,7 +40,8 @@ namespace armarx
     {
     public:
         CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp = nullptr,
-                                    const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = VirtualRobot::JacobiProvider::eSVDDamped);
+                                    const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = VirtualRobot::JacobiProvider::eSVDDamped,
+                                    bool considerJointLimits = true);
 
         Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, float KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection mode);
         Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode);
@@ -49,6 +50,9 @@ namespace armarx
 
         void setCartesianRegularization(float cartesianMMRegularization, float cartesianRadianRegularization);
 
+        bool getConsiderJointLimits() const;
+        void setConsiderJointLimits(bool value);
+
         Eigen::MatrixXf jacobi;
         Eigen::MatrixXf inv;
         VirtualRobot::RobotNodeSetPtr rns;
@@ -58,7 +62,8 @@ namespace armarx
 
     private:
         Eigen::VectorXf calculateRegularization(VirtualRobot::IKSolver::CartesianSelection mode);
-
+        bool adjustJacobiForJointsAtJointLimits(VirtualRobot::IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, float jointLimitCheckAccuracy = 0.001f);
+        bool considerJointLimits = true;
         float cartesianMMRegularization;
         float cartesianRadianRegularization;
     };

source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp

@@ -76,3 +76,82 @@ BOOST_AUTO_TEST_CASE(test1)
     BOOST_CHECK_LE((targetTcpVel - tcpVel).norm(), 0.01f);
 
 }
+
+BOOST_AUTO_TEST_CASE(testJointLimitAwareness)
+{
+    const std::string project = "RobotAPI";
+    armarx::CMakePackageFinder finder(project);
+
+    if (!finder.packageFound())
+    {
+        ARMARX_ERROR_S << "ArmarX Package " << project << " has not been found!";
+    }
+    else
+    {
+        armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
+    }
+    std::string fn = "RobotAPI/robots/Armar3/ArmarIII.xml";
+    ArmarXDataPath::getAbsolutePath(fn, fn);
+    std::string robotFilename = fn;
+    VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFilename, VirtualRobot::RobotIO::eStructure);
+    VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet("TorsoRightArm");
+    CartesianVelocityControllerPtr h(new CartesianVelocityController(rns, nullptr, VirtualRobot::JacobiProvider::eSVDDamped));
+    const Eigen::VectorXf oldJV = rns->getJointValuesEigen();
+    ARMARX_INFO << VAROUT(oldJV);
+    Eigen::VectorXf cartesianVel(6);
+    srand(123);
+
+    for (int i = 0; i < 10000; ++i)
+    {
+
+
+        cartesianVel << rand() % 100, rand() % 100, rand() % 100, 0.01 * (rand() % 100), 0.01 * (rand() % 100), 0.01 * (rand() % 100);
+        //        cartesianVel << 100,0,0;//, 0.01*(rand()%100), 0.01*(rand()%100), 0.01*(rand()%100);
+        auto mode = VirtualRobot::IKSolver::CartesianSelection::All;
+        Eigen::MatrixXf jacobi = h->ik->getJacobianMatrix(rns->getTCP(), mode);
+        Eigen::MatrixXf inv = h->ik->computePseudoInverseJacobianMatrix(jacobi, h->ik->getJacobiRegularization(mode));
+        int jointIndex = rand() % rns->getSize();
+        jacobi.block(0, jointIndex, jacobi.rows(), 1) = Eigen::VectorXf::Zero(jacobi.rows());
+        Eigen::MatrixXf inv2 = h->ik->computePseudoInverseJacobianMatrix(jacobi, h->ik->getJacobiRegularization(mode));
+
+        //    ARMARX_INFO << "diff\n" << (inv-inv2);
+        rns->setJointValues(oldJV);
+        Eigen::Vector3f posBefore = h->tcp->getPositionInRootFrame();
+
+        float accuracy = 0.001f;
+
+        Eigen::VectorXf jointVel = inv * cartesianVel;
+        rns->setJointValues(oldJV + jointVel * accuracy);
+        Eigen::VectorXf resultCartesianVel = ((h->tcp->getPositionInRootFrame() - posBefore) / accuracy);
+
+
+
+        Eigen::VectorXf jointVel2 = inv2 * cartesianVel;
+        rns->setJointValues(oldJV + jointVel2 * accuracy);
+        Eigen::VectorXf resultCartesianVel2 = ((h->tcp->getPositionInRootFrame() - posBefore) / accuracy);
+
+        Eigen::Vector3f diff = (resultCartesianVel - cartesianVel);
+        Eigen::Vector3f diff2 = (resultCartesianVel2 - cartesianVel);
+
+        if (!((diff - diff2).norm() < 0.5 || diff2.norm() < diff.norm()))
+        {
+            ARMARX_INFO << "Target cartesian velo:\n" << cartesianVel;
+            ARMARX_INFO << "jacobi\n" << jacobi;
+            ARMARX_INFO << "inv\n" << inv;
+            ARMARX_INFO << "inv2\n" << inv2;
+            ARMARX_INFO << "\n\nResulting joint cart velocity: " << resultCartesianVel << "\n jointVel:\n" << jointVel;
+            ARMARX_INFO << "\n\nResulting joint cart velocity with joint limit avoidance: " << resultCartesianVel2 << "\n jointVel:\n" << jointVel2;
+
+            ARMARX_INFO << "Diff:\n" << diff;
+            ARMARX_INFO << "Diff2:\n" << diff2;
+        }
+        else
+        {
+            ARMARX_INFO << "Success for \n" << cartesianVel;
+        }
+
+
+        BOOST_REQUIRE((diff - diff2).norm() < 1 || diff2.norm() < diff.norm());
+
+    }
+}