IkDemo.cpp

Go to the documentation of this file.

#include "IkDemo.h"
 
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/meta/EnumNames.hpp>
#include <SimoxUtility/math/pose/invert.h>
 
#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"
 
 
namespace viz = armarx::viz;
 
 
namespace armar6::skills::components::armar6_ik_demo
{
 
    enum class IkMethod
    {
        SimpleDiffIk,
        CompositeDiffIk,
    };
    const simox::meta::EnumNames<IkMethod> IkMethodNames =
    {
        { IkMethod::SimpleDiffIk, "Simple Diff IK" },
        { IkMethod::CompositeDiffIk, "Composite Diff IK" },
    };
 
    struct Manipulator
    {
        Manipulator()
        {
            gizmo.box.color(simox::Color::cyan(255, 64));
        }
        virtual ~Manipulator()
        {
        }
 
        virtual bool handle(viz::InteractionFeedback const& interaction, viz::StagedCommit* stage)
        {
            bool updated = false;
            if (interaction.layer() == gizmo.layer.data_.name)
            {
                updated |= gizmo.handleInteraction(interaction, stage);
            }
            return updated;
        }
 
        virtual void visualize(viz::Client& arviz) = 0;
        virtual void runIk(IkDemo::Robot& robot) = 0;
 
        viz::PoseGizmo gizmo;
    };
 
 
    struct TcpManipulator : public Manipulator
    {
        TcpManipulator()
        {
            std::vector<std::string> options;
            for (IkMethod method : IkMethodNames.values())
            {
                options.push_back("Use " + IkMethodNames.to_name(method));
            }
            gizmo.box
                    .size({75, 100, 200})
                    .enable(viz::interaction().selection().transform().hideDuringTransform()
                            .contextMenu(options));
        };
 
        void visualize(viz::Client& arviz) override
        {
            gizmo.setLayer(arviz.layer(tcp->getName()));
            gizmo.update();
        }
 
        bool handle(viz::InteractionFeedback const& interaction, viz::StagedCommit* stage) override
        {
            bool updated = Manipulator::handle(interaction, stage);
 
            if (interaction.layer() == gizmo.layer.data_.name)
            {
                switch (interaction.type())
                {
                case viz::InteractionFeedbackType::ContextMenuChosen:
                {
                    int i = 0;
                    for (IkMethod method : IkMethodNames.values())
                    {
                        if (i == interaction.chosenContextMenuEntry())
                        {
                            this->method = method;
                            updated |= true;
                            ARMARX_IMPORTANT << "[" << tcp->getName() << "] Using " << IkMethodNames.to_name(method);
                            break;
                        }
                        ++i;
                    }
                } break;
                default:
                    break;
                }
            }
 
            return updated;
        }
 
        void runIk(IkDemo::Robot& robot) override
        {
            const Eigen::Matrix4f tcpPoseInRobotFrame =
                    simox::math::inverted_pose(robot.robot->getGlobalPose()) * gizmo.getCurrent();
 
            switch (method)
            {
            case IkMethod::SimpleDiffIk:
            {
                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;
                    }
 
                    robot.robot->setJointValues(jointValues);
                }
                else
                {
                    gizmo.box.color(simox::Color::red(255, 64));
                }
            } break;
 
            case IkMethod::CompositeDiffIk:
            {
                // 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::SimpleDiffIk;
    };
 
 
    struct PlatformManipulator : public Manipulator
    {
        PlatformManipulator()
        {
            gizmo.box.size({1000, 1000, 100});
        }
 
        void visualize(viz::Client& arviz) override
        {
            gizmo.setLayer(arviz.layer(root->getName()));
            gizmo.update();
        }
        void runIk(IkDemo::Robot& robot) override
        {
            robot.robot->setGlobalPose(gizmo.getCurrent());
        }
 
        VirtualRobot::RobotNodePtr root;
    };
 
 
 
    IkDemo::IkDemo()
    {
    }
 
    IkDemo::~IkDemo()
    {
    }
 
 
    IkDemo::Params::Params() :
        robotFile("armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml"),
        robotNodeSetNamesStr(simox::alg::join({"LeftArm", "RightArm"}, "; "))
    {
    }
 
 
    std::vector<std::string> IkDemo::Params::robotNodeSetNames() const
    {
        bool trim = true;
        return simox::alg::split(robotNodeSetNamesStr, ";", trim);
    }
 
 
    void IkDemo::defineProperties(armarx::PropertyDefinitionContainer& defs)
    {
        defs.optional(params.robotFile, "p.robotFile",
                      "The ArmarX data path to the robot XML file."
                      "\n  For ARMAR-III: 'RobotAPI/robots/Armar3/ArmarIII.xml'"
                      "\n  For ARMAR-6: 'armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml'"
                      );
        defs.optional(params.robotNodeSetNamesStr, "p.robotNodeSetNames",
                      "Names of robot node sets for TCPs.");
    }
 
 
    void IkDemo::start()
    {
        this->stage.reset();
 
        {
            params.robotFile = armarx::ArmarXDataPath::resolvePath(params.robotFile);
            robot.robot = VirtualRobot::RobotIO::loadRobot(params.robotFile, VirtualRobot::RobotIO::eStructure);
 
            robot.layer = remote.arviz->layer("Robot");
 
            robot.visu.file("", robot.robot->getFilename()).joints(robot.robot->getJointValues());
            robot.layer.add(robot.visu);
            stage.add(robot.layer);
        }
 
        {
            auto root = robot.robot->getRootNode();
 
            std::unique_ptr<PlatformManipulator> manip = std::make_unique<PlatformManipulator>();
 
            manip->gizmo.initial = root->getGlobalPose();
            manip->root = root;
 
            manipulators.emplace_back(std::move(manip));
        }
 
        for (const std::string& rnsName : params.robotNodeSetNames())
        {
            ARMARX_CHECK(robot.robot->hasRobotNodeSet(rnsName)) << VAROUT(rnsName) << " must exist.";
 
            const auto& rns = robot.robot->getRobotNodeSet(rnsName);
            const auto tcp = rns->getTCP();
            ARMARX_CHECK_NOT_NULL(tcp) << VAROUT(rnsName) << " must have a TCP.";
 
            std::unique_ptr<TcpManipulator> manip = std::make_unique<TcpManipulator>();
 
            manip->gizmo.initial = tcp->getGlobalPose();
            manip->tcp = tcp;
            manip->nodeSet = rns;
 
            manipulators.emplace_back(std::move(manip));
        }
 
        for (auto& manip : manipulators)
        {
            manip->visualize(remote.arviz.value());
            stage.add(manip->gizmo.layer);
        }
 
        runIk();
 
        viz::CommitResult result = remote.arviz->commit(stage);
        ARMARX_VERBOSE << "Initial commit at revision: " << result.revision();
    }
 
 
    void IkDemo::update()
    {
        viz::CommitResult result = remote.arviz->commit(stage);
 
        // Reset the stage, so that it can be rebuild during the interaction handling
        stage.reset();
 
        for (auto& manip : manipulators)
        {
            stage.requestInteraction(manip->gizmo.layer);
        }
 
        viz::InteractionFeedbackRange interactions = result.interactions();
 
        bool updated = false;
        for (viz::InteractionFeedback const& inter : interactions)
        {
            for (auto& manip : manipulators)
            {
                updated |= manip->handle(inter, &stage);
            }
        }
        if (updated)
        {
            runIk();
        }
    }
 
 
    void IkDemo::runIk()
    {
        for (auto& manip : manipulators)
        {
            manip->runIk(robot);
        }
 
        robot.visu.pose(robot.robot->getGlobalPose());
        robot.visu.joints(robot.robot->getJointValues());
        stage.add(robot.layer);
    }
 
}