Follower.cpp

Go to the documentation of this file.

#include "Follower.h"
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <SimoxUtility/math/convert.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.h>
 
#include <armarx/control/client/ControllerBuilder.h>
#include <armarx/control/common/type.h>
#include <armarx/control/njoint_controller/controller_descriptions.h>
#include <armarx/control/njoint_controller/platform/platform_follower_controller/controller_descriptions.h>
#include <armarx/control/njoint_controller/task_space/ImpedanceController.h>
#include <armarx/control/skills/skills/platform_follower_controller/aron/FollowerParams.aron.generated.h>
 
namespace armarx::control::skills::skills::platform_follower_controller
{
 
    ::armarx::skills::SkillDescription
    Follower::GetSkillDescription()
    {
        ParamType defaultParams;
        defaultParams.platformControlConfig.nodeSetNameLeft = "LeftArm";
        defaultParams.platformControlConfig.nodeSetNameRight = "RightArm";
        defaultParams.platformControlConfig.kPos = 12;
        defaultParams.platformControlConfig.kAngle = 8;
        defaultParams.platformControlConfig.dPos = 0;
        defaultParams.platformControlConfig.dAngle = 0;
 
        return ::armarx::skills::SkillDescription{.skillId = {.skillName = "Follower"},
                                                  .description =
                                                      "TODO: Description of skill Follower.",
                                                  .rootProfileDefaults = defaultParams.toAron(),
                                                  .timeout = ::armarx::Duration::Days(1),
                                                  .parametersType = ParamType::ToAronType()};
    }
 
    Follower::Follower(const Services& services, const Parameters& parameters) :
        ::armarx::skills::SimpleSpecializedSkill<ParamType>(GetSkillDescription()),
        // ::armarx::skills::mixin::RobotReadingSkillMixin(services.mns),
        services_(services),
        parameters_(parameters)
    {
        ARMARX_CHECK_NOT_NULL(services_.controlComponentPlugin);
 
        // auto robotUnit = controlComponentPlugin->getRobotUnitPlugin().getRobotUnit();
        // robotUnit->loadLibFromPackage("armarx_control", "armarx_control_njoint_mp_controller");
        // robotUnit->loadLibFromPackage("armarx_control", "armarx_control_njoint_controller");
        // robotUnit->loadLibFromPackage("eurobin_wp2_skills", "eurobin_wp2_skills_platform_follower_controller");
    }
 
    /*
    bool
    Follower::isAvailable(const SpecializedInitInput&) const
    {
        return true;
    }
    */
 
    /*
    ::armarx::skills::Skill::InitResult
    Follower::init(const SpecializedInitInput&)
    {
        return InitResult{.status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
    */
 
 
    ::armarx::skills::Skill::MainResult
    Follower::main(const SpecializedMainInput& in)
    {
        // robotReader.connect();
        VirtualRobot::RobotPtr robot = services_.robotReader.getRobotWaiting(parameters_.robotName);
        ARMARX_CHECK(
            services_.robotReader.synchronizeRobot(*robot, ::armarx::core::time::DateTime::Now()));
 
        VirtualRobot::RobotNodeSetPtr rnsRight = robot->getRobotNodeSet("RightArm"); // TODO
        VirtualRobot::RobotNodeSetPtr rnsLeft = robot->getRobotNodeSet("LeftArm"); // TODO
 
        std::map<std::string,
                 std::unique_ptr<armarx::control::client::ControllerWrapper<
                     armarx::control::common::ControllerType::TSImp>>>
            controllers;
        for (const auto& rns : {rnsRight, rnsLeft})
        {
            /// create controllers with controller builder
            auto builder =
                services_.controlComponentPlugin
                    ->createControllerBuilder<armarx::control::common::ControllerType::TSImp>();
 
            builder.withNodeSet(rns->getName()).withNamePrefix(rns->getName());
 
            /// load predefined config from file
            {
                if (in.parameters.jointConfigFileName.empty())
                {
                    const std::string controllerClassName =
                        armarx::control::common::ControllerTypeNames.to_name(
                            armarx::control::common::ControllerType::TSImp);
                    const armarx::PackagePath configPath(
                        "armarx_control",
                        "controller_config/NJointTaskspaceImpedanceController/"
                        "bimanual_transport_default.json");
                    ARMARX_IMPORTANT << "Using config `" << configPath.toSystemPath() << "`.";
                    ARMARX_CHECK(std::filesystem::exists(configPath.toSystemPath()))
                        << configPath.toSystemPath();
 
                    builder.withConfig(configPath.toSystemPath());
                }
                else
                {
                    builder.withConfig(in.parameters.jointConfigFileName);
                }
 
                ARMARX_CHECK(builder.config().limbs.count("LeftArm") > 0);
                ARMARX_CHECK(builder.config().limbs.count("RightArm") > 0);
 
                // const auto& leftConfig = builder.config().limbs.at("LeftArm");
                // const auto& rightConfig = builder.config().limbs.at("RightArm");
 
                // /// to be removed
                // ARMARX_INFO << VAROUT(armConfig.kpImpedance);
                // ARMARX_INFO << VAROUT(armConfig.kdImpedance);
                // ARMARX_INFO << VAROUT(armConfig.kpNullspaceTorque);
                // ARMARX_INFO << VAROUT(armConfig.kdNullspaceTorque);
                // ARMARX_INFO << VAROUT(armConfig.desiredPose);
                // ARMARX_INFO << VAROUT(armConfig.desiredTwist);
                // if (armConfig.desiredNullspaceJointAngles.has_value())
                // {
                //     ARMARX_INFO << VAROUT(armConfig.desiredNullspaceJointAngles.value());
                // }
                // ARMARX_INFO << VAROUT(armConfig.torqueLimit);
                // ARMARX_INFO << VAROUT(armConfig.qvelFilter);
            }
 
 
            ARMARX_INFO << "Setting desired pose for controller with nodeset " << rns->getName()
                        << ".";
            auto& armConfig = builder.config().limbs.at(rns->getName());
 
 
            // TODO add offset in global z-direction
            armConfig.desiredPose = rns->getTCP()->getPoseInRootFrame();
            ARMARX_IMPORTANT << VAROUT(armConfig.desiredPose);
 
            ARMARX_TRACE;
            ARMARX_INFO << "creating controller";
 
            auto ctrlWrapper = builder.create();
            const std::string controllerName = builder.getControllerName();
 
            // The controller is scoped by default, which means that if the ControllerWrapper goes out of scope, it will deactivate
            // and delete the NJointController. To prevent this, you can use the daemonize(true) method if needed.
            ctrlWrapper->daemonize(false);
 
            ARMARX_TRACE;
            ARMARX_CHECK_NOT_NULL(ctrlWrapper);
 
            controllers.emplace(rns->getName(), std::move(ctrlWrapper));
        }
 
        armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(300));
 
        ARMARX_INFO << "Activating controller";
        for (auto& c : controllers)
        {
            c.second->updateConfig();
 
            if (not services_.controlComponentPlugin->getRobotUnitPlugin()
                        .getRobotUnit()
                        ->isSimulation())
            {
                c.second->activate();
            }
        }
 
 
        // TODO sleep for in.getMinExecutionTime() usleep(10000);
 
        // TODO Trigger speech output
 
        // TOOD start platform following
        const auto platformControlConfig = in.parameters.platformControlConfig;
 
        // TODO stop platform unit -> trigger placing
 
        constexpr auto ControllerType = ::armarx::control::njoint_controller::platform::
            ControllerType::PlatformFollowerController;
 
        auto platformControllerBuilder =
            services_.controlComponentPlugin->createControllerBuilder<ControllerType>();
        platformControllerBuilder.withNodeSet("Platform").withNamePrefix("Andre");
 
 
        {
            const std::string controllerClassName =
                armarx::control::common::ControllerTypeNames.to_name(
                    armarx::control::common::ControllerType::TSImp);
            const armarx::PackagePath configPath(
                "armarx_control", "controller_config/PlatformFollowerController/default.json");
            ARMARX_IMPORTANT << "Using config `" << configPath.toSystemPath() << "`.";
            ARMARX_CHECK(std::filesystem::exists(configPath.toSystemPath()));
 
            platformControllerBuilder.withConfig(configPath.toSystemPath());
        }
        // TODO add support of using a diffeerent default parameters, for different robots.
        //        platformControllerBuilder.config().params.kPos = in.parameters.platformControlConfig.kPos;
        //        platformControllerBuilder.config().params.kAngle = in.parameters.platformControlConfig.kAngle;
        //        platformControllerBuilder.config().params.dPos = in.parameters.platformControlConfig.dPos;
        //        platformControllerBuilder.config().params.dAngle = in.parameters.platformControlConfig.dAngle;
 
        //        platformControllerBuilder.config().params.maxAngleVel = in.parameters.platformControlConfig.maxAngleVel;
        //        platformControllerBuilder.config().params.maxPosVel = in.parameters.platformControlConfig.maxPosVel;
 
        //        platformControllerBuilder.config().params.minAngleTolerance = in.parameters.platformControlConfig.minAngleTolerance;
        //        platformControllerBuilder.config().params.minPosTolerance = in.parameters.platformControlConfig.minPosTolerance;
 
        auto platformCtrlWrapper = platformControllerBuilder.create();
 
        ARMARX_INFO << "Activating platform controller.";
        platformCtrlWrapper->activate();
 
        while (not shouldSkillTerminate())
        {
            armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(100));
        }
 
        ARMARX_INFO << "Stop requested. Deactivating controllers.";
        for (auto& c : controllers)
        {
            c.second->deactivate();
        }
        platformCtrlWrapper->deactivate();
 
 
        return MainResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded,
            .data = nullptr,
        };
    }
 
    /*
    ::armarx::skills::Skill::ExitResult
    Follower::exit(const SpecializedExitInput&)
    {
        return ExitResult{.status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
    */
 
} // namespace armarx::control::skills::skills::platform_follower_controller