Add CompositeDiffIk

source/RobotAPI/components/ik_demo/IkDemo.cpp

@@ -5,12 +5,14 @@
 
 #include <VirtualRobot/Robot.h>
 #include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.h>
+#include <VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.h>
+#include <VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h>
 
 #include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
 
 #include <RobotAPI/components/ArViz/Client/Client.h>
-
 #include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
 
 #include "PoseGizmo.h"
@@ -22,6 +24,12 @@ namespace viz = armarx::viz;
 namespace armar6::skills::components::armar6_ik_demo
 {
 
+    enum class IkMethod
+    {
+        SimpleDiffIk,
+        CompositeDiffIk,
+    };
+
     struct Manipulator
     {
         Manipulator()
@@ -64,35 +72,165 @@ namespace armar6::skills::components::armar6_ik_demo
         }
         void runIk(IkDemo::Robot& robot) override
         {
-            Eigen::Matrix4f tcpPoseInRobotFrame =
+            const Eigen::Matrix4f tcpPoseInRobotFrame =
                     simox::math::inverted_pose(robot.robot->getGlobalPose()) * gizmo.getCurrent();
 
-            armarx::SimpleDiffIK ik;
-            armarx::SimpleDiffIK::Result result = ik.CalculateDiffIK(tcpPoseInRobotFrame, nodeSet, tcp);
-
-            if (result.reached)
+            switch (method)
+            {
+            case IkMethod::SimpleDiffIk:
             {
-                gizmo.box.color(simox::Color::kit_green(64));
+                armarx::SimpleDiffIK ik;
+                armarx::SimpleDiffIK::Result result = ik.CalculateDiffIK(tcpPoseInRobotFrame, nodeSet, tcp);
+
+                if (result.reached)
+                {
+                    gizmo.box.color(simox::Color::kit_green(64));
+
+                    std::map<std::string, float> jointValues;
+                    size_t i = 0;
+                    for (const auto& rn : nodeSet->getAllRobotNodes())
+                    {
+                        jointValues[rn->getName()] = result.jointValues(i);
+                        ++i;
+                    }
 
-                std::map<std::string, float> jointValues;
-                size_t i = 0;
-                for (const auto& rn : nodeSet->getAllRobotNodes())
+                    robot.robot->setJointValues(jointValues);
+                }
+                else
                 {
-                    jointValues[rn->getName()] = result.jointValues(i);
-                    ++i;
+                    gizmo.box.color(simox::Color::red(255, 64));
                 }
+            } break;
 
-                robot.robot->setJointValues(jointValues);
-            }
-            else
+            case IkMethod::CompositeDiffIk:
             {
-                gizmo.box.color(simox::Color::red(255, 64));
+                // Code taken from: simox/VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp
+
+                const float jointLimitAvoidance = 0;
+                const float kGainManipulabilityAsNullspace = 0.01;
+                const float kSoechtingAsNullspace = 0.0;
+                const int steps = 100;
+
+                VirtualRobot::CompositeDiffIK ik(nodeSet);
+                Eigen::Matrix4f pose = tcpPoseInRobotFrame;
+
+                const bool ori = true;
+                VirtualRobot::CompositeDiffIK::TargetPtr target1 = ik.addTarget(
+                            tcp, pose, ori ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position);
+
+
+                if (jointLimitAvoidance > 0)
+                {
+                    VirtualRobot::CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(
+                                new VirtualRobot::CompositeDiffIK::NullspaceJointLimitAvoidance(nodeSet));
+                    nsjla->kP = jointLimitAvoidance;
+                    for (auto node : nodeSet->getAllRobotNodes())
+                    {
+                        if (node->isLimitless())
+                        {
+                            nsjla->setWeight(node->getName(), 0);
+                        }
+                    }
+                    ik.addNullspaceGradient(nsjla);
+                }
+
+                VirtualRobot::NullspaceManipulabilityPtr nsman = nullptr;
+                if (kGainManipulabilityAsNullspace > 0)
+                {
+#if 0
+                    std::cout << "Adding manipulability as nullspace target" << std::endl;
+                    auto manipTracking = getManipulabilityTracking(nodeSet, nullptr);
+                    if (!manipTracking)
+                    {
+                        std::cout << "Manip tracking zero!" << std::endl;
+                        return;
+                    }
+                    Eigen::MatrixXd followManip = readFollowManipulability();
+                    if (followManip.rows() != manipTracking->getTaskVars())
+                    {
+                        std::cout << "Wrong manipulability matrix!" << std::endl;
+                        return;
+                    }
+                    nsman = VirtualRobot::NullspaceManipulabilityPtr(new VirtualRobot::NullspaceManipulability(manipTracking, followManip, Eigen::MatrixXd(), true));
+                    nsman->kP = kGainManipulabilityAsNullspace;
+                    ik.addNullspaceGradient(nsman);
+#endif
+                }
+
+                if (kSoechtingAsNullspace > 0)
+                {
+                    if (robot.robot->getName() == "Armar6" and nodeSet->getName() == "RightArm")
+                    {
+                        std::cout << "Adding soechting nullspace" << std::endl;
+                        VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
+                        armjoints.clavicula = robot.robot->getRobotNode("ArmR1_Cla1");
+                        armjoints.shoulder1 = robot.robot->getRobotNode("ArmR2_Sho1");
+                        armjoints.shoulder2 = robot.robot->getRobotNode("ArmR3_Sho2");
+                        armjoints.shoulder3 = robot.robot->getRobotNode("ArmR4_Sho3");
+                        armjoints.elbow = robot.robot->getRobotNode("ArmR5_Elb1");
+
+                        auto gradient = std::make_shared<VirtualRobot::SoechtingNullspaceGradient>(
+                                    target1, "ArmR2_Sho1", VirtualRobot::Soechting::ArmType::Right, armjoints);
+
+                        gradient->kP = kSoechtingAsNullspace;
+                        ik.addNullspaceGradient(gradient);
+                    }
+                    else if (robot.robot->getName() == "Armar6" and nodeSet->getName() == "LeftArm")
+                    {
+                        std::cout << "Adding soechting nullspace" << std::endl;
+                        VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
+                        armjoints.clavicula = robot.robot->getRobotNode("ArmL1_Cla1");
+                        armjoints.shoulder1 = robot.robot->getRobotNode("ArmL2_Sho1");
+                        armjoints.shoulder2 = robot.robot->getRobotNode("ArmL3_Sho2");
+                        armjoints.shoulder3 = robot.robot->getRobotNode("ArmL4_Sho3");
+                        armjoints.elbow = robot.robot->getRobotNode("ArmL5_Elb1");
+
+                        auto gradient = std::make_shared<VirtualRobot::SoechtingNullspaceGradient>(
+                                        target1, "ArmL2_Sho1", VirtualRobot::Soechting::ArmType::Left, armjoints);
+                        gradient->kP = kSoechtingAsNullspace;
+                        ik.addNullspaceGradient(gradient);
+                    }
+                    else
+                    {
+                        ARMARX_INFO << "Soechting currently supports only Armar6 and RightArm/LeftArm robot node set "
+                                       "for first robot node set for demonstration purposes.";
+                    }
+                }
+
+                {
+                    VirtualRobot::CompositeDiffIK::Parameters cp;
+                    cp.resetRnsValues = false;
+                    cp.returnIKSteps = true;
+                    cp.steps = 1;
+                    VirtualRobot::CompositeDiffIK::SolveState state;
+                    ik.solve(cp, state);
+
+                    int i = 0;
+                    while (i < steps or (steps < 0 and not ik.getLastResult().reached and i < 1000))
+                    {
+                        ik.step(cp, state, i);
+                        i++;
+                    }
+
+                    if (ik.getLastResult().reached)
+                    {
+                        gizmo.box.color(simox::Color::kit_green(64));
+                        robot.robot->setJointValues(ik.getRobotNodeSet()->getJointValueMap());
+                    }
+                    else
+                    {
+                        gizmo.box.color(simox::Color::red(255, 64));
+                    }
+                }
+            } break;
             }
         }
 
 
         VirtualRobot::RobotNodeSetPtr nodeSet;
         VirtualRobot::RobotNodePtr tcp;
+
+        IkMethod method = IkMethod::CompositeDiffIk;
     };