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;
 
        // create controller
        std::string controllerNamePrefix = "executeTrajectory";
        auto builder =
            srv_.controlComponentPluginUser
                // ->createControllerBuilder<armarx::control::common::ControllerType::TSMPVel>();
                //->createControllerBuilder<armarx::control::common::ControllerType::TSMPImpCol>();
                //->createControllerBuilder<armarx::control::common::ControllerType::TSMPMixImpVel>();
                ->createControllerBuilder<
                    armarx::control::common::ControllerType::TSMPMixImpVelCol>();
 
        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();
        auto start = IceUtil::Time::now().toSecondsDouble();
        while (!ctrl->isFinishedAll() and not shouldSkillTerminate())
        {
            // auto canVal = ctrl->getCanVal(mainCanValName);
            // if (not ctrl->isSafeForceTorque(nodeSetName))
            // {
            //     ARMARX_IMPORTANT << "!!!! Force Torque Guard Triggered Stopping !!! at " << canVal;
            //     ctrl->pauseAll();
            //     break;
            // }
            armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(10));
        }
        auto duration = IceUtil::Time::now().toSecondsDouble() - start;
        ARMARX_DEBUG << VAROUT(duration);
        if (shouldSkillTerminate())
        {
            ARMARX_INFO << "Execute trajectory skill is terminated";
            ctrl->requestStop("all");
        }
        // armarx::Clock::WaitFor(armarx::Duration::Seconds(2));
 
        // while (ctrl->isControllerActive())
        // {
        //     ctrl->deactivateController();
        //     usleep(10000);
        // }
        // ctrl->deleteController();
 
        return MakeSucceededResult();
    }
 
 
} // namespace armarx::control::skills::skills