started simple grid reachability

source/RobotAPI/libraries/core/CMakeLists.txt

@@ -47,6 +47,7 @@ set(LIB_FILES
     CartesianVelocityRamp.cpp
     JointVelocityRamp.cpp
     CartesianVelocityControllerWithRamp.cpp
+    #SimpleGridReachability.cpp
 )
 
 set(LIB_HEADERS
@@ -90,6 +91,7 @@ set(LIB_HEADERS
     CartesianVelocityRamp.h
     JointVelocityRamp.h
     CartesianVelocityControllerWithRamp.h
+    SimpleGridReachability.h
     EigenHelpers.h
 )
 

source/RobotAPI/libraries/core/SimpleGridReachability.cpp

+#include "CartesianVelocityController.h"
+#include "SimpleGridReachability.h"
+
+using namespace armarx;
+
+
+std::pair<Eigen::VectorXf, bool> SimpleGridReachability::calculateDiffIK(const Eigen::Matrix4f targetPose, const Parameters& params)
+{
+    auto rns = params.nodeSet;
+    auto tcp = params.tcp ? params.tcp : rns->getTCP();
+    CartesianVelocityController velocityController(rns);
+    CartesianPositionController positionController(tcp);
+
+    float stepLength = params.ikStepLength;
+    Eigen::VectorXf initialJV = rns->getJointValuesEigen();
+    for (size_t i = 0; i <= params.steps; i++)
+    {
+        Eigen::Vector3f posDiff = positionController.getPositionDiff(targetPose);
+        Eigen::Vector3f oriDiff = positionController.getOrientationDiff(targetPose);
+
+        //ARMARX_IMPORTANT << VAROUT(posDiff) << VAROUT(oriDiff);
+
+        Eigen::VectorXf cartesialVel(6);
+        cartesialVel << posDiff(0), posDiff(1), posDiff(2), oriDiff(0), oriDiff(1), oriDiff(2);
+        Eigen::VectorXf jnv = params.jointLimitAvoidanceKp * velocityController.calculateJointLimitAvoidance();
+        Eigen::VectorXf jv = velocityController.calculate(cartesialVel, jnv, VirtualRobot::IKSolver::All);
+
+        if (i >= 20)
+        {
+            stepLength = params.ikStepLength * 2;
+        }
+
+        jv = jv * stepLength;
+
+        for (size_t n = 0; n < rns->getSize(); n++)
+        {
+            rns->getNode(n)->setJointValue(rns->getNode(n)->getJointValue() + jv(n));
+        }
+    }
+
+    {
+        Eigen::Vector3f posDiff = positionController.getPositionDiff(targetPose);
+        if (posDiff.norm() > params.maxPosError)
+        {
+            ARMARX_ERROR_S << "cannot solve IK! position error=" << posDiff.norm() << "mm";
+            return std::make_pair(initialJV, false);
+        }
+    }
+
+    {
+        float oriDiff = positionController.getOrientationError(targetPose);
+        if (oriDiff > params.maxOriError)
+        {
+            ARMARX_ERROR_S << "cannot solve IK! orientation error=" << oriDiff << " rad";
+            return std::make_pair(initialJV, false);
+        }
+    }
+
+    return std::make_pair(rns->getJointValuesEigen(), true);
+}
+
+SimpleGridReachability::FindPlatformPosition(const Eigen::Matrix4f targetPose, const SimpleGridReachability::Parameters& params)
+{
+    Eigen::Vector2f startPos = params.nodeSet->getRobot()->getGlobalPosition().head(2);
+    for (size_t i = 0; i < params.gridSteps; i++)
+    {
+
+    }
+}

source/RobotAPI/libraries/core/SimpleGridReachability.h

+#pragma once
+#include "CartesianPositionController.h"
+
+namespace armarx
+{
+    class SimpleGridReachability
+    {
+    public:
+        struct Parameters
+        {
+            VirtualRobot::RobotNodePtr tcp;
+            VirtualRobot::RobotNodeSetPtr nodeSet;
+            // grid params
+            Eigen::Vector2f gridStepSize = Eigen::Vector2f(100, 0);
+            Eigen::Vector2f gridSteps = Eigen::Vector2i(11, 0);
+
+            // IK params
+            float ikStepLength = 0.2f;
+            size_t steps = 25;
+            float maxPosError = 10.f;
+            float maxOriError = 0.05f;
+            float jointLimitAvoidanceKp = 2.0f;
+        };
+
+        /**
+         * @brief calculate
+         * @param targetPose TargetPose in root frame
+         * @param params
+         * @return
+         */
+        static std::pair<Eigen::VectorXf, bool> CalculateDiffIK(const Eigen::Matrix4f targetPose, const Parameters& params);
+        static FindPlatformPosition(const Eigen::Matrix4f targetPose, const Parameters& params);
+    };
+
+}
+