IkDemo.cpp

Go to the documentation of this file.

#include "IkDemo.h"
 
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/math/pose/invert.h>
#include <SimoxUtility/meta/EnumNames.hpp>
#include <VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.h>
#include <VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.h>
#include <VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/XML/RobotIO.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);
    }
 
} // namespace armar6::skills::components::armar6_ik_demo