ExecuteTrajectory.cpp

Go to the documentation of this file.

#include "ExecuteTrajectory.h"
 
#include <RobotAPI/libraries/SimpleTrajectory/Trajectory.h>
 
#include <armarx/control/common/aron_conversions.h>
//#include <armarx/control/common/mp/MP.h>
//#include <armarx/control/common/mp/aron/MPConfig.aron.generated.h>
//#include <armarx/control/common/utils.h>
 
#include <SimoxUtility/json/json.hpp>
 
//#include <armarx/control/interface/ConfigurableNJointControllerInterface.h>
#include <armarx/control/njoint_mp_controller/controller_descriptions.h>
//#include <armarx/control/njoint_mp_controller/task_space/MixedImpedanceVelocityController.h>
//#include <armarx/control/skills/constants/constants.h>
#include <armarx/control/skills/skills/ShapeHand.h>
 
namespace armarx::control::skills::skills
{
 
    ExecuteTrajectory::ExecuteTrajectory(const Services& srv) :
        ::armarx::skills::SimpleSpecializedSkill<ParamType>(GetSkillDescription()), srv_(srv)
    {
    }
 
    ::armarx::skills::Skill::ExitResult
    ExecuteTrajectory::exit(const SpecializedExitInput& in)
    {
 
        return {::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    ::armarx::skills::Skill::MainResult
    ExecuteTrajectory::main(const SpecializedMainInput& in)
    {
        ARMARX_INFO << "in main of execute trajectory";
        ARMARX_TRACE;
 
        // Currently controller only works for right arm
        // ARMARX_CHECK(in.parameters.robotNodeset == "" or in.parameters.robotNodeset == "RightArm")
        //     << "Controller currently only works for right arm.";
 
        // std::string robotNodesetName =
        //     in.parameters.robotNodeset == "" ? "RightArm" : in.parameters.robotNodeset;
 
        // create controller
        std::string controllerNamePrefix = "executeTrajectory";
 
        // auto controlType = armarx::control::common::ControllerType::TSMPMixImpVel;
        // if (in.parameters.inSimulation)
        // {
        //     controlType = armarx::control::common::ControllerType::TSMPVel;
        // }
        auto builder =
            srv_.controlComponentPluginUser
                // ->createControllerBuilder<armarx::control::common::ControllerType::TSMPVel>();
                // ->createControllerBuilder<armarx::control::common::ControllerType::TSMPMixImpVel>();
                ->createControllerBuilder<armarx::control::common::ControllerType::TSMPMixImpVelCol>();
       // auto ctrlWrapper = builder.createTSComplianceCtrl(
       //     controllerNamePrefix, nullptr, in.parameters.controllerConfigPath);
       // auto ctrlWrapper = builder.createTSComplianceMPCtrl(controllerNamePrefix,
       //                                                     in.parameters.mpConfigList.toAronDTO(),
       //                                                     nullptr,
       //                                                     in.parameters.controllerConfigPath);
 
 
       // armarx::control::NJointTSMixedImpedanceVelocityMPControllerInterfacePrx ctrl =
       //     armarx::control::NJointTSMixedImpedanceVelocityMPControllerInterfacePrx::checkedCast(
       //         ctrlWrapper->ctrl());
       // ARMARX_TRACE;
       // ARMARX_CHECK_NOT_NULL(ctrl);
       //
       // ctrl->updateMPConfig(in.parameters.mpConfigList.toAronDTO());
       // ctrl->trainMP();
       //
       // // start controller
       // ARMARX_CHECK_NOT_NULL(ctrl);
       // ctrl->startAll();
 
        // wait until execution is finished, or termination is requested
        //        ARMARX_TRACE;
 
        //        /// add hand controller
        //        armarx::skills::manager::dto::SkillExecutionRequest exec;
        //        exec.skillId = {
        //            {::armarx::control::skills::constants::CONTROL_SKILL_PROVIDER_NAME},
        //            ::armarx::control::skills::skills::ShapeHand::GetSkillDescription().skillId.skillName};
 
        //        ::armarx::control::skills::skills::ShapeHand::ParamType shapeHandParamsContact;
        //        ::armarx::control::skills::arondto::Hand hand;
        //        if (robotNodesetName.find("Left") != std::string::npos)
        //        {
        //            hand = ::armarx::control::skills::arondto::Hand::Hand::Left;
        //        }
        //        else
        //        {
        //            hand = ::armarx::control::skills::arondto::Hand::Hand::Right;
        //        }
        //        toAron(shapeHandParamsContact.hand, hand);
 
 
        //        ///
        //        ARMARX_INFO << "-- parsing hand via points";
        //        std::vector<std::pair<int, double>> indexCanValueList;
        //        for (size_t i = 0; i < in.parameters.viapointListHandShape.size(); ++i)
        //        {
        //            indexCanValueList.push_back(
        //                std::make_pair(i, in.parameters.viapointListHandShape.at(i).canVal));
        //        }
        //        std::sort(indexCanValueList.begin(),
        //                  indexCanValueList.end(),
        //                  [](const std::pair<int, double>& a, const std::pair<int, double>& b)
        //                  { return a.second > b.second; });
        //        ARMARX_INFO << "-- size: " << in.parameters.viapointListHandShape.size();
        //        ARMARX_INFO << "-- size: " << indexCanValueList.size();
 
        auto ctrl = builder.createGetTSComplianceMPCtrl(controllerNamePrefix,
                                                        in.parameters.mpConfigList.toAronDTO(),
                                                        nullptr,
                                                        in.parameters.controllerConfigPath);
        std::string mainCanValName;
        std::string nodeSetName;
        for (auto& mpCfg : in.parameters.mpConfigList.mpList)
        {
            if (mpCfg.role == "taskspace")
            {
                mainCanValName = mpCfg.name;
                nodeSetName = mpCfg.nodeSetName;
                break;
            }
        }
        ARMARX_CHECK_NOT_NULL(ctrl);
        throwIfSkillShouldTerminate();
        //        size_t handViaPointIdx = 0;
        // bool forceTorqueGuardToggled = false;
        while (!ctrl->isFinishedAll() and not shouldSkillTerminate())
        {
            auto canVal = ctrl->getCanVal(mainCanValName);
            //            ARMARX_INFO << "-- at " << canVal;
            //            if (handViaPointIdx < in.parameters.viapointListHandShape.size() and
            //                canVal < indexCanValueList.at(handViaPointIdx).second)
            //            {
            //                ARMARX_INFO << "-- at " << canVal << " with idx: " << handViaPointIdx;
            //                shapeHandParamsContact.shapeName =
            //                    in.parameters.viapointListHandShape
            //                        .at(indexCanValueList.at(handViaPointIdx).first)
            //                        .handShapeName;
            //                exec.parameters = shapeHandParamsContact.toAron()->toAronDictDTO();
 
            //                manager->begin_executeSkill(exec);
            //                ARMARX_INFO << "-- index: " << handViaPointIdx
            //                            << "\n-- shapeName: " << shapeHandParamsContact.shapeName.value()
            //                            << "\n-- canVal: " << indexCanValueList.at(handViaPointIdx).second;
            //                handViaPointIdx++;
            //            }
 
            //// TODO add force torque guard in the mp controllers, see MPPool.cpp
            // if (not forceTorqueGuardToggled and canVal < in.parameters.forceTorqueGuardCanVal)
            // {
            //     ARMARX_IMPORTANT << "-- enable force torque guard at " << canVal;
            //     forceTorqueGuardToggled = true;
            //     ctrl->enableSafeGuardForceTorque(robotNodesetName, true, false);
            // }
            if (not ctrl->isSafeForceTorque(nodeSetName))
            {
                ARMARX_IMPORTANT << "!!!! Force Torque Guard Triggered Stopping !!! at " << canVal;
                ctrl->pauseAll();
                break;
            }
            armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(2));
        }
        armarx::Clock::WaitFor(armarx::Duration::Seconds(2));
        //        auto canVal = ctrl->getCanVal("default");
        //        ARMARX_INFO << "--- done with force torque guard safe status <<< "
        //                    << ctrl->isSafeForceTorque(robotNodesetName) << " >>> with canVal " << canVal;
        //        if (not shouldSkillTerminate() and
        //            handViaPointIdx < in.parameters.viapointListHandShape.size())
        //        {
        //            ARMARX_INFO << "--- execute the last hand shape";
        //            shapeHandParamsContact.shapeName =
        //                in.parameters.viapointListHandShape.at(indexCanValueList.at(handViaPointIdx).first)
        //                    .handShapeName;
        //            exec.parameters = shapeHandParamsContact.toAron()->toAronDictDTO();
        //            manager->begin_executeSkill(exec);
        //        }
        //        ARMARX_INFO << "-- deleting controller";
 
        // while (ctrl->isControllerActive())
        // {
        //     ctrl->deactivateController();
        //     usleep(10000);
        // }
        //        ctrl->deleteController();
 
        return MakeSucceededResult();
    }
 
 
} // namespace armarx::control::skills::skills