PlatformFollowerController.cpp

Go to the documentation of this file.

#include "PlatformFollowerController.h"
 
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/math/Helpers.h>
 
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/time/CycleUtil.h"
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/time/Clock.h>
 
#include "RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h"
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
 
#include <armarx/control/common/type.h>
#include <armarx/control/ethercat/RTUtility.h>
#include <armarx/control/njoint_controller/platform/platform_follower_controller/aron/PlatformFollowerControllerConfig.aron.generated.h>
#include <armarx/control/njoint_controller/platform/platform_follower_controller/aron_conversions.h>
 
namespace armarx::control::njoint_controller::platform::platform_follower_controller
{
    const std::string NAME = "PlatformFollowerController";
 
    const armarx::NJointControllerRegistration<Controller> Registration(NAME);
 
    Controller::Controller(const armarx::RobotUnitPtr& robotUnit,
                           const armarx::NJointControllerConfigPtr& config,
                           const VirtualRobot::RobotPtr&)
    {
        // config
        ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
        ARMARX_CHECK_EXPRESSION(cfg);
        // ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
 
        ARMARX_CHECK_EXPRESSION(robotUnit);
 
        const auto robot = useSynchronizedRtRobot();
 
        platformTarget = useControlTarget(robotUnit->getRobotPlatformName(),
                                          armarx::ControlModes::HolonomicPlatformVelocity)
                             ->asA<armarx::ControlTargetHolonomicPlatformVelocity>();
        ARMARX_CHECK_EXPRESSION(platformTarget)
            << "The actuator " << robotUnit->getRobotPlatformName() << " has no control mode "
            << armarx::ControlModes::HolonomicPlatformVelocity;
 
        const Config configData = ::armarx::fromAron<arondto::Config, Config>(cfg->config);
 
        reinitTripleBuffer({});
        configBuffer.reinitAllBuffers(configData);
 
        // TODO init tcps
        const VirtualRobot::RobotNodeSetPtr rnsRight =
            robot->getRobotNodeSet(configData.params.nodeSetNameRight);
        ARMARX_CHECK(rnsRight);
        const VirtualRobot::RobotNodeSetPtr rnsLeft =
            robot->getRobotNodeSet(configData.params.nodeSetNameLeft);
        ARMARX_CHECK(rnsLeft);
        tcpRight = rnsRight->getTCP();
        tcpLeft = rnsLeft->getTCP();
    }
 
    std::string
    Controller::getClassName(const Ice::Current& iceCurrent) const
    {
        return NAME;
    }
 
    void
    Controller::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                      const IceUtil::Time& timeSinceLastIteration)
    {
        // update state
        stateBuffer.getWriteBuffer().handPositionRight = tcpRight->getPositionInRootFrame();
        stateBuffer.getWriteBuffer().handPositionLeft = tcpLeft->getPositionInRootFrame();
        stateBuffer.getWriteBuffer().time = armarx::Clock::Now();
        stateBuffer.getWriteBuffer().currentVel.setZero();
        stateBuffer.commitWrite();
 
        // write control targets
        rtUpdateControlStruct();
 
        // FIXME remove
        //platformTarget->velocityX = 0;
        //platformTarget->velocityY = 0;
        //platformTarget->velocityRotation = 0;
 
        // FIXME uncomment
        platformTarget->velocityX = rtGetControlStruct().platformVelocityTargets.x;
        platformTarget->velocityY = rtGetControlStruct().platformVelocityTargets.y;
        platformTarget->velocityRotation = rtGetControlStruct().platformVelocityTargets.yaw;
    }
 
    void
    Controller::updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
                             const Ice::Current& iceCurrent)
    {
        ARMARX_IMPORTANT << "Controller::updateConfig";
 
        // TODO maybe update pid controller
 
        auto updateConfig = ::armarx::fromAron<arondto::Config, Config>(dto);
        configBuffer.reinitAllBuffers(updateConfig);
    }
 
    void
    Controller::additionalTask()
    {
        if (not rtReady.load())
        {
            // store result
            getWriterControlStruct().platformVelocityTargets.x = 0;
            getWriterControlStruct().platformVelocityTargets.y = 0;
            getWriterControlStruct().platformVelocityTargets.yaw = 0;
 
            writeControlStruct();
        }
 
        configBuffer.updateReadBuffer();
        stateBuffer.updateReadBuffer();
 
        const auto state = stateBuffer.getReadBuffer();
        const auto params = configBuffer.getReadBuffer().params;
 
        const double deltaT = (state.time - additionalTaskData.lastTime).toSecondsDouble();
        additionalTaskData.lastTime = state.time;
 
        if (deltaT == 0)
            return;
 
        const Eigen::Vector3f currentVel = state.currentVel;
        const Eigen::Vector3f currentPositionRight = state.handPositionRight;
        const Eigen::Vector3f currentPositionLeft = state.handPositionLeft;
        const Eigen::Vector3f positionDeltaRight =
            currentPositionRight - initialState.handPositionRight;
        const Eigen::Vector3f positionDeltaLeft =
            currentPositionLeft - initialState.handPositionLeft;
 
        Eigen::Vector2f positionDelta = ((positionDeltaLeft + positionDeltaRight) / 2).head<2>();
        Eigen::Vector2f velocity = Eigen::Vector2f::Zero();
        if (positionDelta.norm() >= params.minPosTolerance)
        {
            positionDelta =
                positionDelta - params.minPosTolerance * positionDelta / positionDelta.norm();
            velocity = positionDelta * params.kPos - currentVel.head<2>() * params.dPos;
        }
        const float Tstar = 1 / ((params.filterTimeConstant / deltaT) + 1);
        additionalTaskData.filteredLinearVel =
            Tstar * (velocity - additionalTaskData.filteredLinearVel) +
            additionalTaskData.filteredLinearVel;
        if (!std::isfinite(additionalTaskData.filteredLinearVel(0)))
        {
            additionalTaskData.filteredLinearVel(0) = 0;
        }
        if (!std::isfinite(additionalTaskData.filteredLinearVel(1)))
        {
            additionalTaskData.filteredLinearVel(1) = 0;
        }
        velocity = additionalTaskData.filteredLinearVel.norm() > params.maxPosVel
                       ? additionalTaskData.filteredLinearVel.normalized() * params.maxPosVel
                       : additionalTaskData.filteredLinearVel;
 
 
        const Eigen::Vector3f currentDirection = state.direction();
        const float cosAlpha = currentDirection.dot(initialState.direction());
        const Eigen::Vector3f directionDiff = currentDirection - initialState.direction();
        float alpha = 0;
        if (directionDiff(1) < 0)
        {
            alpha = -acos(cosAlpha);
        }
        else
        {
            alpha = acos(cosAlpha);
        }
        float angularVelocity;
        if (fabs(alpha) < params.minAngleTolerance)
        {
            angularVelocity = 0;
        }
        else
        {
            if (alpha > 0)
            {
                alpha -= params.minAngleTolerance;
            }
            else
            {
                alpha += params.minAngleTolerance;
            }
            angularVelocity = params.kAngle * alpha - params.dAngle * currentVel.z();
        }
        additionalTaskData.filteredAngularVel =
            Tstar * (angularVelocity - additionalTaskData.filteredAngularVel) +
            additionalTaskData.filteredAngularVel;
        if (!std::isfinite(additionalTaskData.filteredAngularVel))
        {
            additionalTaskData.filteredAngularVel = 0;
        }
        angularVelocity = std::min(additionalTaskData.filteredAngularVel, params.maxAngleVel);
 
 
        {
            auto& onPublishData = onPublishBuffer.getWriteBuffer();
 
            onPublishData.target.platformVelocityTargets.x = velocity.x();
            onPublishData.target.platformVelocityTargets.y = velocity.y();
            onPublishData.target.platformVelocityTargets.yaw = angularVelocity;
 
            onPublishData.position_error_left = positionDeltaLeft;
            onPublishData.position_error_right = positionDeltaRight;
            onPublishBuffer.commitWrite();
        }
 
 
        // store result
        getWriterControlStruct().platformVelocityTargets.x = velocity.x();
        getWriterControlStruct().platformVelocityTargets.y = velocity.y();
        getWriterControlStruct().platformVelocityTargets.yaw = angularVelocity;
 
        writeControlStruct();
    }
 
    void
    Controller::onPublish(const armarx::SensorAndControl& /*sac*/,
                          const armarx::DebugDrawerInterfacePrx& /*debugDrawer*/,
                          const armarx::DebugObserverInterfacePrx& debugObservers)
    {
        armarx::StringVariantBaseMap datafields;
 
        onPublishBuffer.updateReadBuffer();
        const auto& data = onPublishBuffer.getReadBuffer();
 
        datafields["vx"] = new armarx::Variant(data.target.platformVelocityTargets.x);
        datafields["vy"] = new armarx::Variant(data.target.platformVelocityTargets.y);
        datafields["vyaw"] = new armarx::Variant(data.target.platformVelocityTargets.yaw);
 
        datafields["position_error_left_x"] = new armarx::Variant(data.position_error_left.x());
        datafields["position_error_left_y"] = new armarx::Variant(data.position_error_left.y());
        datafields["position_error_left_z"] = new armarx::Variant(data.position_error_left.z());
        datafields["position_error_right_x"] = new armarx::Variant(data.position_error_right.x());
        datafields["position_error_right_y"] = new armarx::Variant(data.position_error_right.y());
        datafields["position_error_right_z"] = new armarx::Variant(data.position_error_right.z());
 
        debugObservers->setDebugChannel(NAME, datafields);
    }
 
    void
    Controller::onInitNJointController()
    {
        runTask(NAME + "AdditionalTask",
                [&]
                {
                    armarx::control::ethercat::RTUtility::pinThreadToCPU(
                        4); // please don't set this to 0 as it is the rtRun/control thread
                    armarx::control::ethercat::RTUtility::elevateThreadPriority(
                        armarx::control::ethercat::RTUtility::RT_THREAD_PRIORITY);
 
                    armarx::CycleUtil c(10);
                    getObjectScheduler()->waitForObjectStateMinimum(
                        armarx::eManagedIceObjectStarted);
                    ARMARX_IMPORTANT << "Create a new thread alone PlatformTrajectory controller";
                    while (getState() == armarx::eManagedIceObjectStarted)
                    {
                        if (isControllerActive() and rtReady.load())
                        {
                            ARMARX_VERBOSE << "additional task";
                            additionalTask();
                        }
                        c.waitForCycleDuration();
                    }
                });
 
        ARMARX_INFO << "PlatformTrajectoryVelocityController::onInitNJointController";
    }
 
    void
    Controller::rtPreActivateController()
    {
        initialState.handPositionRight = tcpRight->getPositionInRootFrame();
        initialState.handPositionLeft = tcpLeft->getPositionInRootFrame();
        initialState.time = armarx::core::time::DateTime::Now();
 
        ARMARX_INFO << "Init done.";
 
        rtReady.store(true);
    }
 
    Controller::~Controller() = default;
 
} // namespace armarx::control::njoint_controller::platform::platform_follower_controller