NaturalIKTest.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotAPI::ArmarXObjects::NaturalIKTest
 * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "NaturalIKTest.h"
 
#include <random>
 
#include <VirtualRobot/IK/ConstrainedOptimizationIK.h>
#include <VirtualRobot/IK/constraints/OrientationConstraint.h>
#include <VirtualRobot/IK/constraints/PoseConstraint.h>
#include <VirtualRobot/IK/constraints/PositionConstraint.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/math/Helpers.h>
 
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
 
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <RobotAPI/libraries/diffik/CompositeDiffIK.h>
#include <RobotAPI/libraries/natik/NaturalIK.h>
 
namespace armarx
{
    NaturalIKTestPropertyDefinitions::NaturalIKTestPropertyDefinitions(std::string prefix) :
        armarx::ComponentPropertyDefinitions(prefix)
    {
        //defineRequiredProperty<std::string>("PropertyName", "Description");
        //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
 
        defineOptionalProperty<std::string>(
            "DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on");
        defineOptionalProperty<std::string>(
            "ArVizTopicName", "ArVizTopic", "Layer updates are sent over this topic.");
 
        //defineOptionalProperty<std::string>("MyProxyName", "MyProxy", "Name of the proxy to be used");
    }
 
    std::string
    NaturalIKTest::getDefaultName() const
    {
        return "NaturalIKTest";
    }
 
    void
    NaturalIKTest::onInitComponent()
    {
        // Register offered topices and used proxies here.
        offeringTopicFromProperty("DebugObserverName");
        // debugDrawer.offeringTopic(*this);  // Calls this->offeringTopic().
 
        // Using a proxy will cause the component to wait until the proxy is available.
        // usingProxyFromProperty("MyProxyName");
 
        offeringTopicFromProperty("ArVizTopicName");
    }
 
    void
    NaturalIKTest::onConnectComponent()
    {
        // Get topics and proxies here. Pass the *InterfacePrx type as template argument.
        debugObserver = getTopicFromProperty<DebugObserverInterfacePrx>("DebugObserverName");
        // debugDrawer.getTopic(*this);  // Calls this->getTopic().
 
        // getProxyFromProperty<MyProxyInterfacePrx>("MyProxyName");
 
        createOrUpdateRemoteGuiTab(buildGui(),
                                   [this](RemoteGui::TabProxy& prx) { processGui(prx); });
 
        vizTask = new RunningTask<NaturalIKTest>(this, &NaturalIKTest::vizTaskRun);
        vizTask->start();
    }
 
    void
    NaturalIKTest::onDisconnectComponent()
    {
        vizTask->stop();
        vizTask = nullptr;
    }
 
    RemoteGui::WidgetPtr
    NaturalIKTest::buildGui()
    {
        RemoteGui::detail::GroupBoxBuilder leftBox =
            RemoteGui::makeGroupBox().label("Left").addChild(
                RemoteGui::makeSimpleGridLayout()
                    .cols(2)
                    .addTextLabel("L.X")
                    .addChild(RemoteGui::makeFloatSlider("L.X").min(-600).max(600).value(0))
 
                    .addTextLabel("L.Y")
                    .addChild(RemoteGui::makeFloatSlider("L.Y").min(200).max(1200).value(600))
 
                    .addTextLabel("L.Z")
                    .addChild(RemoteGui::makeFloatSlider("L.Z").min(-600).max(600).value(-200))
 
                    .addTextLabel("L.scale")
                    .addChild(RemoteGui::makeFloatSlider("L.scale").min(0.5).max(2).value(1.3f))
 
                    .addTextLabel("L.minElbZ")
                    .addChild(RemoteGui::makeFloatSlider("L.minElbZ").min(500).max(2000).value(500))
 
                    .addChild(RemoteGui::makeCheckBox("L.setOri").value(true))
                    .addChild(new RemoteGui::HSpacer)
 
                    .addTextLabel("L.rX")
                    .addChild(RemoteGui::makeFloatSlider("L.rX").min(-180).max(180).value(0))
 
                    .addTextLabel("L.rY")
                    .addChild(RemoteGui::makeFloatSlider("L.rY").min(-180).max(180).value(0))
 
                    .addTextLabel("L.rZ")
                    .addChild(RemoteGui::makeFloatSlider("L.rZ").min(-180).max(180).value(0)));
        RemoteGui::detail::GroupBoxBuilder rightBox =
            RemoteGui::makeGroupBox().label("Right").addChild(
                RemoteGui::makeSimpleGridLayout()
                    .cols(2)
                    .addTextLabel("R.X")
                    .addChild(RemoteGui::makeFloatSlider("R.X").min(-600).max(600).value(0))
 
                    .addTextLabel("R.Y")
                    .addChild(RemoteGui::makeFloatSlider("R.Y").min(200).max(1200).value(600))
 
                    .addTextLabel("R.Z")
                    .addChild(RemoteGui::makeFloatSlider("R.Z").min(-600).max(600).value(-200))
 
                    .addTextLabel("R.scale")
                    .addChild(RemoteGui::makeFloatSlider("R.scale").min(0.5).max(2).value(1.3f))
 
                    .addTextLabel("R.minElbZ")
                    .addChild(RemoteGui::makeFloatSlider("R.minElbZ").min(500).max(2000).value(500))
 
                    .addChild(RemoteGui::makeCheckBox("R.setOri").value(true))
                    .addChild(new RemoteGui::HSpacer)
 
                    .addTextLabel("R.rX")
                    .addChild(RemoteGui::makeFloatSlider("R.rX").min(-180).max(180).value(0))
 
                    .addTextLabel("R.rY")
                    .addChild(RemoteGui::makeFloatSlider("R.rY").min(-180).max(180).value(0))
 
                    .addTextLabel("R.rZ")
                    .addChild(RemoteGui::makeFloatSlider("R.rZ").min(-180).max(180).value(0)));
 
        return RemoteGui::makeVBoxLayout()
            .addChild(RemoteGui::makeCheckBox("useCompositeIK").value(false))
            .addChild(
                RemoteGui::makeSimpleGridLayout().cols(2).addChild(leftBox).addChild(rightBox))
            .addChild(RemoteGui::makeSimpleGridLayout()
                          .cols(2)
                          .addTextLabel("elbowKp")
                          .addChild(RemoteGui::makeFloatSlider("elbowKp").min(0).max(1).value(1))
 
                          .addTextLabel("jlaKp")
                          .addChild(RemoteGui::makeFloatSlider("jlaKp").min(0).max(1).value(0)))
            .addChild(RemoteGui::makeLabel("errors").value("<errors>"))
            .addChild(RemoteGui::makeButton("runTest").label("run test"));
    }
 
    void
    NaturalIKTest::processGui(RemoteGui::TabProxy& prx)
    {
        prx.receiveUpdates();
 
        p.useCompositeIK = prx.getValue<bool>("useCompositeIK").get();
 
        float x, y, z, rx, ry, rz;
 
        x = prx.getValue<float>("R.X").get();
        y = prx.getValue<float>("R.Y").get();
        z = prx.getValue<float>("R.Z").get();
        p.p_R.target = Eigen::Vector3f(x, y, z);
        p.p_R.scale = prx.getValue<float>("R.scale").get();
        p.p_R.ikparams.minimumElbowHeight = prx.getValue<float>("R.minElbZ").get();
        p.p_R.setOri = prx.getValue<bool>("R.setOri").get();
        rx = prx.getValue<float>("R.rX").get();
        ry = prx.getValue<float>("R.rY").get();
        rz = prx.getValue<float>("R.rZ").get();
        p.p_R.targetRotation = Eigen::Vector3f(rx, ry, rz);
 
        x = prx.getValue<float>("L.X").get();
        y = prx.getValue<float>("L.Y").get();
        z = prx.getValue<float>("L.Z").get();
        p.p_L.target = Eigen::Vector3f(x, y, z);
        p.p_L.scale = prx.getValue<float>("L.scale").get();
        p.p_L.ikparams.minimumElbowHeight = prx.getValue<float>("L.minElbZ").get();
        p.p_L.setOri = prx.getValue<bool>("L.setOri").get();
        rx = prx.getValue<float>("L.rX").get();
        ry = prx.getValue<float>("L.rY").get();
        rz = prx.getValue<float>("L.rZ").get();
        p.p_L.targetRotation = Eigen::Vector3f(rx, ry, rz);
 
 
        p.p_R.ikparams.diffIKparams.elbowKp = prx.getValue<float>("elbowKp").get();
        p.p_R.ikparams.diffIKparams.jointLimitAvoidanceKp = prx.getValue<float>("jlaKp").get();
        p.p_R.ikparams.diffIKparams.returnIKSteps = true;
 
        p.p_L.ikparams.diffIKparams.elbowKp = prx.getValue<float>("elbowKp").get();
        p.p_L.ikparams.diffIKparams.jointLimitAvoidanceKp = prx.getValue<float>("jlaKp").get();
        p.p_L.ikparams.diffIKparams.returnIKSteps = true;
 
 
        p.targetValid = true;
        std::stringstream ss;
        ss << "err_R: " << p.err_R(0) << " " << p.err_R(1) << " " << p.err_R(2) << "\n";
        ss << "err_L: " << p.err_L(0) << " " << p.err_L(1) << " " << p.err_L(2) << "";
        prx.getValue<std::string>("errors").set(ss.str());
 
        bool runTest = prx.getButtonClicked("runTest");
        if (runTest)
        {
            runTestTask = new RunningTask<NaturalIKTest>(this, &NaturalIKTest::testTaskRun);
            runTestTask->start();
        }
 
 
        prx.sendUpdates();
    }
 
    struct IKStats
    {
        IKStats(const std::string& name) : name(name)
        {
        }
 
        std::string name;
        int solved = 0;
        float duration = 0;
        std::vector<Eigen::Vector3f> elbow;
 
        Eigen::Vector3f
        averageElb()
        {
            Eigen::Vector3f elb = Eigen::Vector3f ::Zero();
            for (auto e : elbow)
            {
                elb += e;
            }
            return elb / elbow.size();
        }
    };
 
    struct SoftPositionConstraint : public VirtualRobot::PositionConstraint
    {
    public:
        SoftPositionConstraint(const VirtualRobot::RobotPtr& robot,
                               const VirtualRobot::RobotNodeSetPtr& nodeSet,
                               const VirtualRobot::SceneObjectPtr& node,
                               const VirtualRobot::SceneObjectPtr& tcp,
                               const Eigen::Vector3f& target,
                               VirtualRobot::IKSolver::CartesianSelection cartesianSelection =
                                   VirtualRobot::IKSolver::All,
                               float tolerance = 3.0f) :
            PositionConstraint(robot, nodeSet, node, target, cartesianSelection, tolerance),
            tcp(tcp)
        {
            optimizationFunctions.clear();
 
            addOptimizationFunction(0, true);
        }
 
        VirtualRobot::SceneObjectPtr tcp;
 
        Eigen::VectorXf
        optimizationGradient(unsigned int id) override
        {
            int size = nodeSet->getSize();
            (void)size;
 
            Eigen::MatrixXf J = ik->getJacobianMatrix(tcp); //.block(0, 0, 3, size);
            //Eigen::Vector3f d = eef->getGlobalPose().block<3,1>(0,3) - target;
 
            Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(J);
            Eigen::MatrixXf nullspace = lu_decomp.kernel();
 
            Eigen::VectorXf grad = PositionConstraint::optimizationGradient(id);
 
            Eigen::VectorXf mapped = Eigen::VectorXf::Zero(grad.size());
            for (int i = 0; i < nullspace.cols(); i++)
            {
                float squaredNorm = nullspace.col(i).squaredNorm();
                // Prevent division by zero
                if (squaredNorm > 1.0e-32f)
                {
                    mapped += nullspace.col(i) * nullspace.col(i).dot(grad) /
                              nullspace.col(i).squaredNorm();
                }
            }
            return mapped;
        }
 
        bool
        checkTolerances() override
        {
            return true;
        }
    };
 
    void
    NaturalIKTest::testTaskRun()
    {
        CMakePackageFinder finder("armar6_rt");
        std::string robotFile =
            finder.getDataDir() + "/armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml";
        ARMARX_IMPORTANT << "loading robot from " << robotFile;
        VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFile);
 
        VirtualRobot::RobotNodeSetPtr rns_R = robot->getRobotNodeSet("RightArm");
        VirtualRobot::RobotNodeSetPtr rns_L = robot->getRobotNodeSet("LeftArm");
 
        VirtualRobot::RobotNodePtr sho1_R = rns_R->getNode(1);
        VirtualRobot::RobotNodePtr sho1_L = rns_L->getNode(1);
 
        VirtualRobot::RobotNodePtr elb_R = rns_R->getNode(4);
        VirtualRobot::RobotNodePtr wri1_R = rns_R->getNode(6);
        VirtualRobot::RobotNodePtr tcp_R = rns_R->getTCP();
 
 
        float upper_arm_length =
            (sho1_R->getPositionInRootFrame() - elb_R->getPositionInRootFrame()).norm();
        //float lower_arm_length = (elb_R->getPositionInRootFrame() - wri1_R->getPositionInRootFrame()).norm();
        float lower_arm_length =
            (elb_R->getPositionInRootFrame() - rns_R->getTCP()->getPositionInRootFrame()).norm();
 
 
        Eigen::Vector3f shoulder_R = sho1_R->getPositionInRootFrame();
        Eigen::Vector3f shoulder_L = sho1_L->getPositionInRootFrame();
 
        Eigen::Vector3f offset = (shoulder_R + shoulder_L) / 2;
        (void)offset;
 
        NaturalIK ik_R("Right", shoulder_R, 1);
        NaturalIK ik_L("Left", shoulder_L, 1);
 
        ik_R.setUpperArmLength(upper_arm_length);
        ik_R.setLowerArmLength(lower_arm_length);
        ik_L.setUpperArmLength(upper_arm_length);
        ik_L.setLowerArmLength(lower_arm_length);
 
 
        NaturalIK::ArmJoints arm_R;
        arm_R.rns = rns_R;
        arm_R.elbow = elb_R;
        arm_R.tcp = rns_R->getTCP();
 
        std::vector<NaturalIK::Parameters> ikParamList;
        std::vector<IKStats> ikStats = {IKStats("NaturalIK"),
                                        IKStats("SimpleDiffIK"),
                                        IKStats("OptIK"),
                                        IKStats("DiffIK"),
                                        IKStats("CompositeIK")};
 
        NaturalIK::Parameters p0;
        p0.diffIKparams.resetRnsValues = false;
        p0.diffIKparams.ikStepLengthInitial = 1;
        p0.diffIKparams.ikStepLengthFineTune = 1;
        p0.diffIKparams.maxJointAngleStep = 0.2f;
 
        {
            NaturalIK::Parameters p = p0;
            p.diffIKparams.resetRnsValues = false;
            p.diffIKparams.elbowKp = 1;
            p.diffIKparams.jointLimitAvoidanceKp = 0.1;
            p.diffIKparams.ikStepLengthInitial = 1;
            ikParamList.emplace_back(p);
        }
        {
            NaturalIK::Parameters p = p0;
            p.diffIKparams.resetRnsValues = false;
            p.diffIKparams.elbowKp = 0;
            p.diffIKparams.jointLimitAvoidanceKp = 1;
            ikParamList.emplace_back(p);
        }
 
        CompositeDiffIK::Parameters pc;
        pc.resetRnsValues = false;
 
 
        std::random_device rd; //Will be used to obtain a seed for the random number engine
        std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
        std::uniform_real_distribution<> dis(0.0, 1.0);
 
        std::vector<Eigen::VectorXf> initialConfigurations;
        for (VirtualRobot::RobotNodePtr rn : rns_R->getAllRobotNodes())
        {
            if (rn->isLimitless())
            {
                rn->setJointValue(0);
            }
            else
            {
                rn->setJointValue((rn->getJointLimitHi() + rn->getJointLimitLo()) / 2);
            }
        }
 
        rns_R->getNode(1)->setJointValue(0);
        rns_R->getNode(3)->setJointValue(0);
        rns_R->getNode(5)->setJointValue(0);
        initialConfigurations.emplace_back(rns_R->getJointValuesEigen());
        rns_R->getNode(1)->setJointValue(0);
        rns_R->getNode(3)->setJointValue(0);
        rns_R->getNode(5)->setJointValue(M_PI);
        initialConfigurations.emplace_back(rns_R->getJointValuesEigen());
        rns_R->getNode(1)->setJointValue(0);
        rns_R->getNode(3)->setJointValue(M_PI);
        rns_R->getNode(5)->setJointValue(0);
        initialConfigurations.emplace_back(rns_R->getJointValuesEigen());
        rns_R->getNode(1)->setJointValue(0);
        rns_R->getNode(3)->setJointValue(M_PI);
        rns_R->getNode(5)->setJointValue(M_PI);
        initialConfigurations.emplace_back(rns_R->getJointValuesEigen());
 
 
        std::vector<Eigen::Matrix4f> targets;
        while (targets.size() < 100)
        {
            for (size_t i = 0; i < rns_R->getSize(); i++)
            {
                if (rns_R->getNode(i)->isLimitless())
                {
                    rns_R->getNode(i)->setJointValue(dis(gen) * 2 * M_PI);
                }
                else
                {
                    float lo = rns_R->getNode(i)->getJointLimitLo();
                    float hi = rns_R->getNode(i)->getJointLimitHi();
                    rns_R->getNode(i)->setJointValue(dis(gen) * (hi - lo) + lo);
                }
            }
            Eigen::Vector3f tcpPos = tcp_R->getPositionInRootFrame();
            if (tcpPos.y() > 0)
            {
                targets.emplace_back(tcp_R->getPoseInRootFrame());
            }
        }
 
 
        IceUtil::Time start;
        for (size_t i = 0; i < ikParamList.size(); i++)
        {
            start = TimeUtil::GetTime();
            for (size_t n = 0; n < targets.size(); n++)
            {
                for (const Eigen::VectorXf& initjv : initialConfigurations)
                {
                    rns_R->setJointValues(initjv);
                    NaturalDiffIK::Result ikResult =
                        ik_R.calculateIK(targets.at(n), arm_R, ikParamList.at(i));
                    if (ikResult.reached)
                    {
                        ikStats.at(i).solved = ikStats.at(i).solved + 1;
                        ikStats.at(i).elbow.emplace_back(elb_R->getPositionInRootFrame());
                        break;
                    }
                }
            }
            ikStats.at(i).duration = (TimeUtil::GetTime() - start).toSecondsDouble();
        }
        start = TimeUtil::GetTime();
 
        std::vector<VirtualRobot::RobotNodePtr> elbJoints_R;
        for (int i = 0; i < 4; i++)
        {
            elbJoints_R.push_back(rns_R->getNode(i));
        }
        VirtualRobot::RobotNodeSetPtr rnsElb_R = VirtualRobot::RobotNodeSet::createRobotNodeSet(
            robot, "ELB_R", elbJoints_R, rns_R->getKinematicRoot(), elb_R);
        for (size_t n = 0; n < targets.size(); n++)
        {
            rns_R->setJointValues(initialConfigurations.at(0));
            VirtualRobot::ConstrainedOptimizationIK optIK(robot, rns_R);
            //VirtualRobot::ConstraintPtr conPose(new VirtualRobot::PoseConstraint(robot, rns_R, tcp_R, targets.at(n)));
            //optIK.addConstraint(conPose);
            VirtualRobot::ConstraintPtr posConstraint(new VirtualRobot::PositionConstraint(
                robot,
                rns_R,
                tcp_R,
                math::Helpers::GetPosition(targets.at(n)),
                VirtualRobot::IKSolver::CartesianSelection::All));
            posConstraint->setOptimizationFunctionFactor(1);
 
            VirtualRobot::ConstraintPtr oriConstraint(new VirtualRobot::OrientationConstraint(
                robot,
                rns_R,
                tcp_R,
                math::Helpers::GetOrientation(targets.at(n)),
                VirtualRobot::IKSolver::CartesianSelection::All,
                2.f / 180 * M_PI)); // was VirtualRobot::MathTools::deg2rad(...)
            oriConstraint->setOptimizationFunctionFactor(1000);
 
 
            Eigen::Vector3f elbowPos =
                ik_R.solveSoechtingIK(math::Helpers::GetPosition(targets.at(n))).elbow;
 
            VirtualRobot::ConstraintPtr elbConstraint(
                new SoftPositionConstraint(robot,
                                           rns_R,
                                           elb_R,
                                           tcp_R,
                                           elbowPos,
                                           VirtualRobot::IKSolver::CartesianSelection::All));
            elbConstraint->setOptimizationFunctionFactor(0.01);
            //elbConstraint->
 
            //ARMARX_IMPORTANT << VAROUT(elbConstraint->optimizationFunction(0));
            //ARMARX_IMPORTANT << VAROUT(elbConstraint->optimizationGradient(0));
 
            optIK.addConstraint(posConstraint);
            optIK.addConstraint(oriConstraint);
            //optIK.addConstraint(elbConstraint);
 
            optIK.initialize();
            bool reached = optIK.solve();
            if (reached)
            {
                ikStats.at(2).solved = ikStats.at(2).solved + 1;
                ikStats.at(2).elbow.emplace_back(elb_R->getPositionInRootFrame());
                //float perr = CartesianPositionController::GetPositionError(targets.at(n), tcp_R);
                //float oerr = 180 / M_PI * CartesianPositionController::GetOrientationError(targets.at(n), tcp_R);
                //ARMARX_IMPORTANT << VAROUT(perr) << VAROUT(oerr);
                //ARMARX_IMPORTANT << VAROUT(targets.at(n)) << VAROUT(tcp_R->getPoseInRootFrame());
            }
        }
        ikStats.at(2).duration = (TimeUtil::GetTime() - start).toSecondsDouble();
 
        start = TimeUtil::GetTime();
        for (size_t n = 0; n < targets.size(); n++)
        {
            rns_R->setJointValues(initialConfigurations.at(0));
            VirtualRobot::DifferentialIK diffIK(rns_R);
            diffIK.setGoal(targets.at(n));
            bool reached = diffIK.solveIK();
            if (reached)
            {
                ikStats.at(3).solved = ikStats.at(3).solved + 1;
                ikStats.at(3).elbow.emplace_back(elb_R->getPositionInRootFrame());
            }
        }
        ikStats.at(3).duration = (TimeUtil::GetTime() - start).toSecondsDouble();
 
        start = TimeUtil::GetTime();
        for (size_t n = 0; n < targets.size(); n++)
        {
            for (const Eigen::VectorXf& initjv : initialConfigurations)
            {
                rns_R->setJointValues(initjv);
                CompositeDiffIK ik(rns_R);
                CompositeDiffIK::TargetPtr t(new CompositeDiffIK::Target(
                    rns_R, tcp_R, targets.at(n), VirtualRobot::IKSolver::CartesianSelection::All));
                ik.addTarget(t);
                CompositeDiffIK::NullspaceJointTargetPtr nsjt(
                    new CompositeDiffIK::NullspaceJointTarget(rns_R));
                nsjt->set(2, 0.2, 1);
                ik.addNullspaceGradient(nsjt);
                CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(
                    new CompositeDiffIK::NullspaceJointLimitAvoidance(rns_R));
                nsjla->setWeight(2, 0);
                ik.addNullspaceGradient(nsjla);
                CompositeDiffIK::Result result = ik.solve(pc);
                if (result.reached)
                {
                    ikStats.at(4).solved = ikStats.at(4).solved + 1;
                    ikStats.at(4).elbow.emplace_back(elb_R->getPositionInRootFrame());
                    break;
                }
            }
        }
        ikStats.at(4).duration = (TimeUtil::GetTime() - start).toSecondsDouble();
 
        for (size_t i = 0; i < ikStats.size(); i++)
        {
            ARMARX_IMPORTANT << ikStats.at(i).name << " solved: " << ikStats.at(i).solved
                             << ", T: " << ikStats.at(i).duration;
        }
 
        //ARMARX_IMPORTANT << "NaturalIK solved: " << ikStats.at(0).solved << ", T: " << durations.at(0);
        //ARMARX_IMPORTANT << "SimpleDiffIK solved: " << ikStats.at(1).solved;
        //ARMARX_IMPORTANT << "OptIK solved: " << ikStats.at(2).solved;
        //ARMARX_IMPORTANT << "DiffIK solved: " << ikStats.at(3).solved;
 
        //ARMARX_IMPORTANT << VAROUT(ikStats.at(0).averageElb());
        //ARMARX_IMPORTANT << VAROUT(ikStats.at(1).solved);
        //ARMARX_IMPORTANT << VAROUT(ikStats.at(1).averageElb());
    }
 
    struct Side
    {
        viz::Layer layer;
        Eigen::Vector3f shoulder;
    };
 
    struct ShoulderAngles
    {
        float SE, SR;
        NaturalIK::SoechtingForwardPositions fwd;
    };
 
    void
    NaturalIKTest::vizTaskRun()
    {
        //ARMARX_IMPORTANT << "vizTask starts";
        viz::Client arviz(*this);
 
        viz::Layer layer_iksteps = arviz.layer("ikSteps");
 
        viz::Layer layer_robot = arviz.layer("Robot");
        viz::Robot vizrobot =
            viz::Robot("robot")
                .position(Eigen::Vector3f::Zero())
                .file("armar6_rt", "armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml");
        vizrobot.useFullModel();
        layer_robot.add(vizrobot);
 
        CMakePackageFinder finder("armar6_rt");
        std::string robotFile =
            finder.getDataDir() + "/armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml";
        ARMARX_IMPORTANT << "loading robot from " << robotFile;
        VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFile);
 
        VirtualRobot::RobotNodeSetPtr rns_R = robot->getRobotNodeSet("RightArm");
        VirtualRobot::RobotNodeSetPtr rns_L = robot->getRobotNodeSet("LeftArm");
 
        VirtualRobot::RobotNodePtr sho1_R = rns_R->getNode(1);
        VirtualRobot::RobotNodePtr sho1_L = rns_L->getNode(1);
 
        VirtualRobot::RobotNodePtr elb_R = rns_R->getNode(4);
        VirtualRobot::RobotNodePtr wri1_R = rns_R->getNode(6);
 
        VirtualRobot::RobotNodePtr elb_L = rns_L->getNode(4);
 
 
        //float value = 0.5f * (1.0f + std::sin(timeInSeconds));
        //robot.joint("ArmR2_Sho1", value);
        //robot.joint("ArmR3_Sho2", value);
 
        //layer.add(robot);
 
        float upper_arm_length =
            (sho1_R->getPositionInRootFrame() - elb_R->getPositionInRootFrame()).norm();
        //float lower_arm_length = (elb_R->getPositionInRootFrame() - wri1_R->getPositionInRootFrame()).norm();
        float lower_arm_length =
            (elb_R->getPositionInRootFrame() - rns_R->getTCP()->getPositionInRootFrame()).norm();
 
 
        viz::Layer layer_R = arviz.layer("Right");
        viz::Layer layer_L = arviz.layer("Left");
 
        //Eigen::Vector3f shoulder_R(NaturalIK::SoechtingAngles::MMM_SHOULDER_POS, 0, 0);
        //Eigen::Vector3f shoulder_L(-NaturalIK::SoechtingAngles::MMM_SHOULDER_POS, 0, 0);
        Eigen::Vector3f shoulder_R = sho1_R->getPositionInRootFrame();
        Eigen::Vector3f shoulder_L = sho1_L->getPositionInRootFrame();
 
        Eigen::Vector3f offset = (shoulder_R + shoulder_L) / 2;
 
        NaturalIK ik_R("Right", shoulder_R, 1);
        NaturalIK ik_L("Left", shoulder_L, 1);
 
        ik_R.setUpperArmLength(upper_arm_length);
        ik_R.setLowerArmLength(lower_arm_length);
        ik_L.setUpperArmLength(upper_arm_length);
        ik_L.setLowerArmLength(lower_arm_length);
 
 
        NaturalIK::ArmJoints arm_R;
        arm_R.rns = rns_R;
        arm_R.elbow = elb_R;
        arm_R.tcp = rns_R->getTCP();
 
        NaturalIK::ArmJoints arm_L;
        arm_L.rns = rns_L;
        arm_L.elbow = elb_L;
        arm_L.tcp = rns_L->getTCP();
 
 
        auto makeVizSide =
            [&](viz::Layer& layer, NaturalIK& ik, Eigen::Vector3f target, Eigen::Vector3f& err)
        {
            ik.setScale(p.p_R.scale);
 
            Eigen::Vector3f vtgt = target;
            NaturalIK::SoechtingAngles soechtingAngles = ik.CalculateSoechtingAngles(vtgt);
            NaturalIK::SoechtingForwardPositions fwd = ik.forwardKinematics(soechtingAngles);
            err = target - fwd.wrist;
            vtgt = vtgt + 1.0 * err;
 
            layer.add(viz::Box("Wrist_orig")
                          .position(fwd.wrist)
                          .size(20)
                          .color(viz::Color::fromRGBA(0, 0, 120)));
 
            fwd = ik.forwardKinematics(ik.CalculateSoechtingAngles(vtgt));
            err = target - fwd.wrist;
 
 
            layer.clear();
 
            layer.add(viz::Box("Shoulder")
                          .position(ik.getShoulderPos())
                          .size(20)
                          .color(viz::Color::fromRGBA(255, 0, 0)));
            layer.add(viz::Box("Elbow").position(fwd.elbow).size(20).color(
                viz::Color::fromRGBA(255, 0, 255)));
            layer.add(viz::Box("Wrist").position(fwd.wrist).size(20).color(
                viz::Color::fromRGBA(0, 0, 255)));
 
            layer.add(viz::Box("Target").position(target).size(20).color(
                viz::Color::fromRGBA(255, 165, 0)));
 
            viz::Cylinder arrUpperArm = viz::Cylinder("UpperArm");
            arrUpperArm.fromTo(ik.getShoulderPos(), fwd.elbow);
            arrUpperArm.color(viz::Color::blue());
 
            viz::Arrow arrLowerArm = viz::Arrow("LowerArm");
            arrLowerArm.fromTo(fwd.elbow, fwd.wrist);
            arrLowerArm.color(viz::Color::red());
 
            layer.add(arrUpperArm);
            layer.add(arrLowerArm);
        };
 
        CycleUtil c(20);
        while (!vizTask->isStopped())
        {
            if (!p.targetValid)
            {
                c.waitForCycleDuration();
                continue;
            }
 
            makeVizSide(layer_R, ik_R, p.p_R.target + offset, p.err_R);
            makeVizSide(layer_L, ik_L, p.p_L.target + offset, p.err_L);
 
 
            //Eigen::Quaternionf targetOri(0.70029222965240479,
            //                             -0.6757887601852417,
            //                             0.036805182695388794,
            //                             0.22703713178634644);
            //Eigen::Matrix4f target_R = math::Helpers::Pose(target + offset, targetOri.toRotationMatrix());
 
            //Eigen::Quaternionf referenceOri(0,
            //                                 0.70710688,
            //                                -0.70710682,
            //                                0);
            Eigen::Quaternionf referenceOri_R(0.70029222965240479,
                                              -0.6757887601852417,
                                              0.036805182695388794,
                                              0.22703713178634644);
            Eigen::Quaternionf referenceOri_L = mirrorOri(referenceOri_R);
 
            Eigen::AngleAxisf aa_R(p.p_R.targetRotation.norm() / 180 * M_PI,
                                   p.p_R.targetRotation.normalized());
            Eigen::Matrix3f targetOri_R =
                referenceOri_R.toRotationMatrix() * aa_R.toRotationMatrix();
            Eigen::Matrix4f target_R = math::Helpers::Pose(p.p_R.target + offset, targetOri_R);
 
            Eigen::AngleAxisf aa_L(p.p_L.targetRotation.norm() / 180 * M_PI,
                                   p.p_L.targetRotation.normalized());
            Eigen::Matrix3f targetOri_L =
                referenceOri_L.toRotationMatrix() * aa_L.toRotationMatrix();
            Eigen::Matrix4f target_L = math::Helpers::Pose(p.p_L.target + offset, targetOri_L);
 
            // X_Platform
            // Y_Platform
            // Yaw_Platform
            // ArmL1_Cla1
            // ArmL2_Sho1
            // ArmL3_Sho2
            // ArmL4_Sho3
            // ArmL5_Elb1
            // ArmL6_Elb2
            // ArmL7_Wri1
            // ArmL8_Wri2
            // ArmR1_Cla1
            // ArmR2_Sho1
            // ArmR3_Sho2
            // ArmR4_Sho3
            // ArmR5_Elb1
            // ArmR6_Elb2
            // ArmR7_Wri1
            // ArmR8_Wri2
 
            auto calcShoulderAngles = [&](NaturalIK& natik, Eigen::Matrix4f target)
            {
                NaturalIK::SoechtingForwardPositions fwd =
                    natik.solveSoechtingIK(math::Helpers::Position(target));
                Eigen::Vector3f elb = fwd.elbow - fwd.shoulder;
                float SE = fwd.soechtingAngles.SE;
                elb = Eigen::AngleAxisf(-SE, Eigen::Vector3f::UnitX()) * elb;
                float SR = std::atan2(elb(0), -elb(2));
                SR = std::max(-0.1f, SR);
                ShoulderAngles sa;
                sa.SE = SE;
                sa.SR = SR;
                sa.fwd = fwd;
                return sa;
            };
 
            //VirtualRobot::RobotNodeSetPtr rnsP_R = robot->getRobotNodeSet("PlatformPlanningRightArm");
            VirtualRobot::RobotNodeSetPtr rnsP = robot->getRobotNodeSet("PlatformPlanningBothArms");
            if (p.useCompositeIK)
            {
                CompositeDiffIK cik(rnsP);
 
 
                VirtualRobot::IKSolver::CartesianSelection mode_R =
                    p.p_R.setOri ? VirtualRobot::IKSolver::CartesianSelection::All
                                 : VirtualRobot::IKSolver::CartesianSelection::Position;
                VirtualRobot::IKSolver::CartesianSelection mode_L =
                    p.p_L.setOri ? VirtualRobot::IKSolver::CartesianSelection::All
                                 : VirtualRobot::IKSolver::CartesianSelection::Position;
                CompositeDiffIK::TargetPtr tgt_R = cik.addTarget(arm_R.tcp, target_R, mode_R);
                CompositeDiffIK::TargetPtr tgt_L = cik.addTarget(arm_L.tcp, target_L, mode_L);
 
                CompositeDiffIK::NullspaceJointTargetPtr nsjt(
                    new CompositeDiffIK::NullspaceJointTarget(rnsP));
                //NaturalIK::SoechtingForwardPositions fwd_R = ik_R.solveSoechtingIK(math::Helpers::Position(target_R));
                //Eigen::Vector3f elb_R = fwd_R.elbow - fwd_R.shoulder;
                //float SE = fwd_R.soechtingAngles.SE;
                //elb_R = Eigen::AngleAxisf(-SE, Eigen::Vector3f::UnitX()) * elb;
                //float SR = std::atan2(elb_R(0), -elb_R(2));
                //SR = std::max(-0.1f, SR);
 
                ShoulderAngles sa_R = calcShoulderAngles(ik_R, target_R);
                ShoulderAngles sa_L = calcShoulderAngles(ik_L, target_L);
 
                nsjt->set("ArmR2_Sho1", +sa_R.SE, 1);
                nsjt->set("ArmR3_Sho2", +sa_R.SR, 2);
                nsjt->set("ArmR4_Sho3", -M_PI / 4, 0.5f);
 
                nsjt->set("ArmL2_Sho1", -sa_L.SE, 1);
                nsjt->set("ArmL3_Sho2", +sa_L.SR, 2);
                nsjt->set("ArmL4_Sho3", +M_PI / 4, 0.5f);
 
                nsjt->set("ArmR1_Cla1", 0, 0.5f);
                nsjt->set("ArmL1_Cla1", 0, 0.5f);
 
 
                //ARMARX_IMPORTANT << VAROUT(SE) << VAROUT(SR);
                nsjt->kP = 1;
                cik.addNullspaceGradient(nsjt);
 
                //CompositeDiffIK::NullspaceTargetPtr nst_R(new CompositeDiffIK::NullspaceTarget(rnsP, arm_R.elbow,
                //                                        math::Helpers::CreatePose(fwd_R.elbow, Eigen::Matrix3f::Identity()),
                //                                        VirtualRobot::IKSolver::CartesianSelection::Position));
                //nst_R->kP = 0;
                //cik.addNullspaceGradient(nst_R);
                CompositeDiffIK::NullspaceTargetPtr nst_R =
                    cik.addNullspacePositionTarget(arm_R.elbow, sa_R.fwd.elbow);
                nst_R->kP = 0;
                CompositeDiffIK::NullspaceTargetPtr nst_L =
                    cik.addNullspacePositionTarget(arm_L.elbow, sa_L.fwd.elbow);
                nst_L->kP = 0;
 
                CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(
                    new CompositeDiffIK::NullspaceJointLimitAvoidance(rnsP));
                nsjla->setWeight(0, 0);
                nsjla->setWeight(1, 0);
                nsjla->setWeight(2, 0);
                nsjla->setWeight("ArmR4_Sho3", 0);
                nsjla->setWeight("ArmL4_Sho3", 0);
                nsjla->kP = 0.1;
                cik.addNullspaceGradient(nsjla);
                CompositeDiffIK::Parameters cp;
                cp.resetRnsValues = true;
                cp.returnIKSteps = true;
                CompositeDiffIK::Result cikResult = cik.solve(cp);
 
                layer_iksteps.clear();
                int i = 0;
                for (const CompositeDiffIK::TargetStep& s : tgt_R->ikSteps)
                {
                    layer_iksteps.add(viz::Box("tcp_" + std::to_string(i))
                                          .size(20)
                                          .pose(s.tcpPose)
                                          .color(viz::Color::blue()));
                    i++;
                }
                for (const CompositeDiffIK::TargetStep& s : tgt_L->ikSteps)
                {
                    layer_iksteps.add(viz::Box("tcp_" + std::to_string(i))
                                          .size(20)
                                          .pose(s.tcpPose)
                                          .color(viz::Color::blue()));
                    i++;
                }
                for (const CompositeDiffIK::NullspaceTargetStep& s : nst_R->ikSteps)
                {
                    layer_iksteps.add(viz::Box("elb_" + std::to_string(i))
                                          .size(20)
                                          .pose(s.tcpPose)
                                          .color(viz::Color::blue()));
                    i++;
                }
                for (const CompositeDiffIK::NullspaceTargetStep& s : nst_L->ikSteps)
                {
                    layer_iksteps.add(viz::Box("elb_" + std::to_string(i))
                                          .size(20)
                                          .pose(s.tcpPose)
                                          .color(viz::Color::blue()));
                    i++;
                }
            }
            else
            {
                rnsP->getNode(0)->setJointValue(0);
                rnsP->getNode(1)->setJointValue(0);
                rnsP->getNode(2)->setJointValue(0);
                NaturalDiffIK::Result ikResult;
                if (p.p_R.setOri)
                {
                    ikResult = ik_R.calculateIK(target_R, arm_R, p.p_R.ikparams);
                }
                else
                {
                    ikResult = ik_R.calculateIKpos(
                        math::Helpers::Position(target_R), arm_R, p.p_R.ikparams);
                }
 
                layer_iksteps.clear();
                std::stringstream ss;
                int i = 0;
                for (const NaturalDiffIK::IKStep& s : ikResult.ikSteps)
                {
                    ss << s.pdTcp.norm() << "; " << s.odTcp.norm() << "; " << s.pdElb.norm()
                       << "\n";
                    layer_iksteps.add(viz::Box("tcp_" + std::to_string(i))
                                          .size(20)
                                          .pose(s.tcpPose)
                                          .color(viz::Color::blue()));
                    layer_iksteps.add(viz::Box("elb_" + std::to_string(i))
                                          .size(20)
                                          .pose(s.elbPose)
                                          .color(viz::Color::blue()));
                    i++;
                }
                //ARMARX_IMPORTANT << ss.str();
            }
 
 
            vizrobot.joints(rnsP->getJointValueMap());
 
 
            arviz.commit({layer_R, layer_L, layer_robot, layer_iksteps});
            //ARMARX_IMPORTANT << "arviz.commit";
 
 
            c.waitForCycleDuration();
        }
    }
 
    Eigen::Matrix4f
    NaturalIKTest::mirrorPose(Eigen::Matrix4f oldPose)
    {
        Eigen::Quaternionf oriOld(oldPose.block<3, 3>(0, 0));
        Eigen::Quaternionf ori;
 
        ori.w() = oriOld.z();
        ori.x() = oriOld.y() * -1;
        ori.y() = oriOld.x() * -1;
        ori.z() = oriOld.w();
 
        Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
        pose.block<3, 3>(0, 0) =
            (ori * Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())).toRotationMatrix();
        pose(0, 3) = -oldPose(0, 3);
        pose(1, 3) = oldPose(1, 3);
        pose(2, 3) = oldPose(2, 3);
 
        return pose;
    }
 
    Eigen::Quaternionf
    NaturalIKTest::mirrorOri(Eigen::Quaternionf oriOld)
    {
        Eigen::Quaternionf ori;
 
        ori.w() = oriOld.z();
        ori.x() = oriOld.y() * -1;
        ori.y() = oriOld.x() * -1;
        ori.z() = oriOld.w();
 
        return (ori * Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ()));
    }
 
    void
    NaturalIKTest::onExitComponent()
    {
    }
 
    armarx::PropertyDefinitionsPtr
    NaturalIKTest::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(
            new NaturalIKTestPropertyDefinitions(getConfigIdentifier()));
    }
} // namespace armarx