PlatformGlobalTrajectoryController.cpp

Go to the documentation of this file.

#include "PlatformGlobalTrajectoryController.h"
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.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/navigation/common/controller_types.h>
#include <armarx/navigation/platform_controller/aron/PlatformGlobalTrajectoryControllerConfig.aron.generated.h>
#include <armarx/navigation/platform_controller/aron_conversions.h>
 
namespace armarx::navigation::platform_controller::platform_global_trajectory
{
    const NJointControllerRegistration<Controller> Registration(
        common::ControllerTypeNames.to_name(common::ControllerType::PlatformGlobalTrajectory));
 
    Controller::Controller(const RobotUnitPtr& robotUnit,
                           const NJointControllerConfigPtr& config,
                           const VirtualRobot::RobotPtr&)
    {
        ARMARX_IMPORTANT << "Creating "
                         << common::ControllerTypeNames.to_name(
                                common::ControllerType::PlatformGlobalTrajectory);
        // config
        ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
        ARMARX_CHECK_EXPRESSION(cfg);
        ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
 
        ARMARX_CHECK_EXPRESSION(robotUnit);
 
        const auto robot = useSynchronizedRtRobot();
            
        // Control target
        {
            const std::string controlTargetName = robotUnit->getRobotPlatformName();
 
            ARMARX_INFO << "Using control target " << controlTargetName;
            auto* ct = useControlTarget(controlTargetName,
                                            ControlModes::HolonomicPlatformVelocity);
            ARMARX_CHECK_NOT_NULL(ct) << "Cannot use control target `" << controlTargetName << "`"; 
 
            platformTarget = ct->asA<ControlTargetHolonomicPlatformVelocity>();
 
            ARMARX_CHECK_EXPRESSION(platformTarget)
                << "The actuator " << controlTargetName << " has no control mode "
                << ControlModes::HolonomicPlatformVelocity;
        }
 
 
        const auto configData = ::armarx::fromAron<arondto::Config, Config>(cfg->config);
        const auto trajectoryFollowingControllerParams = configData.params;
 
        reinitTripleBuffer({});
        configBuffer_updateConfigToAdditionalTask.reinitAllBuffers(configData);
        configBuffer_updateConfigToOnPublish.reinitAllBuffers(configData);
 
        trajectoryFollowingController.emplace(trajectoryFollowingControllerParams);
 
        ARMARX_INFO << "Init done.";
    }
 
    std::string
    Controller::getClassName(const Ice::Current& iceCurrent) const
    {
        return armarx::navigation::common::ControllerTypeNames.to_name(
            armarx::navigation::common::ControllerType::PlatformGlobalTrajectory);
    }
 
    void
    Controller::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                      const IceUtil::Time& timeSinceLastIteration)
    {
        rtUpdateControlStruct();
 
        // update control devices
        platformTarget->velocityX = rtGetControlStruct().linear.x();
        platformTarget->velocityY = rtGetControlStruct().linear.y();
        platformTarget->velocityRotation = rtGetControlStruct().angular;
 
        // read data (for non-rt)
        robotStateBuffer_rtToAdditionalTask.getWriteBuffer().global_T_robot.matrix() =
            rtGetRobot()->getGlobalPose();
        robotStateBuffer_rtToAdditionalTask.commitWrite();
    }
 
    void
    Controller::updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
                             const Ice::Current& iceCurrent)
    {
        ARMARX_IMPORTANT << "Controller::updateConfig";
 
        // TODO maybe update pid controller
 
        const auto updateConfig = ::armarx::fromAron<arondto::Config, Config>(dto);
 
        configBuffer_updateConfigToAdditionalTask.getWriteBuffer() = updateConfig;
        configBuffer_updateConfigToAdditionalTask.commitWrite();
 
        configBuffer_updateConfigToOnPublish.getWriteBuffer() = updateConfig;
        configBuffer_updateConfigToOnPublish.commitWrite();
 
        ARMARX_INFO << "Trajectory with " << updateConfig.targets.trajectory.points().size();
 
        ARMARX_IMPORTANT << "done Controller::updateConfig";
    }
 
    void
    Controller::additionalTask()
    {
        ARMARX_TRACE;
        ARMARX_CHECK(trajectoryFollowingController.has_value());
 
        const auto& configBuffer =
            configBuffer_updateConfigToAdditionalTask.getUpToDateReadBuffer();
 
 
        // if trajectory is empty, set velocity to 0
        if (configBuffer.targets.trajectory.points().empty())
        {
            ARMARX_INFO << deactivateSpam(1) << "Trajectory is empty!";
 
            filteredTwist.reset();
 
            getWriterControlStruct().reset();
            writeControlStruct();
            return;
        }
 
        // update controller
        ARMARX_TRACE;
 
        const armarx::navigation::traj_ctrl::global::TrajectoryControllerResult result =
            trajectoryFollowingController->control(
                configBuffer.targets.trajectory,
                robotStateBuffer_rtToAdditionalTask.getUpToDateReadBuffer().global_T_robot);
 
        // low-pass filter
        {
            const float alpha = configBuffer.params.alpha;
            filteredTwist.linear =
                alpha * filteredTwist.linear + (1. - alpha) * result.twist.linear.head<2>();
            filteredTwist.angular =
                alpha * filteredTwist.angular + (1. - alpha) * result.twist.angular.z();
        }
 
        // store result
        getWriterControlStruct() = filteredTwist;
        ARMARX_TRACE;
        writeControlStruct();
 
        // store results (onPublish)
        targetBuffer_additionalTaskToOnPublish.getWriteBuffer().target = filteredTwist;
        targetBuffer_additionalTaskToOnPublish.getWriteBuffer().dropPointVelocity =
            result.dropPoint.velocity;
        targetBuffer_additionalTaskToOnPublish.getWriteBuffer().currentOrientation =
            result.currentOrientation;
        targetBuffer_additionalTaskToOnPublish.getWriteBuffer().desiredOrientation =
            result.desiredOrientation;
        targetBuffer_additionalTaskToOnPublish.getWriteBuffer().orientationError =
            result.orientationError;
            targetBuffer_additionalTaskToOnPublish.getWriteBuffer().positionError =
            result.positionError;
        targetBuffer_additionalTaskToOnPublish.getWriteBuffer().isFinalSegment =
            result.isFinalSegment;
        targetBuffer_additionalTaskToOnPublish.commitWrite();
    }
 
    void
    Controller::onPublish(const SensorAndControl& /*sac*/,
                          const DebugDrawerInterfacePrx& /*debugDrawer*/,
                          const DebugObserverInterfacePrx& debugObservers)
    {
        StringVariantBaseMap datafields;
 
        const auto& debugStuff = targetBuffer_additionalTaskToOnPublish.getUpToDateReadBuffer();
 
 
        datafields["vx"] = new Variant(debugStuff.target.linear.x());
        datafields["vy"] = new Variant(debugStuff.target.linear.y());
        datafields["vyaw"] = new Variant(debugStuff.target.angular);
        datafields["trajectory_points"] =
            new Variant(configBuffer_updateConfigToOnPublish.getUpToDateReadBuffer()
                            .targets.trajectory.points()
                            .size());
 
        datafields["drop_point_velocity"] = new Variant(debugStuff.dropPointVelocity);
 
        datafields["orientationError"] = new Variant(debugStuff.orientationError);
        datafields["desiredOrientation"] = new Variant(debugStuff.desiredOrientation);
        datafields["currentOrientation"] = new Variant(debugStuff.currentOrientation);
        datafields["isFinalSegment"] = new Variant(debugStuff.isFinalSegment);
 
        datafields["positionError"] = new Variant(debugStuff.positionError);
 
        debugObservers->setDebugChannel(
            common::ControllerTypeNames.to_name(common::ControllerType::PlatformGlobalTrajectory),
            datafields);
    }
 
    void
    Controller::onInitNJointController()
    {
        ARMARX_TRACE;
        ARMARX_INFO << "PlatformGlobalTrajectoryController::onInitNJointController";
 
        runTask("PlatformGlobalTrajectoryControllerAdditionalTask",
                [&]
                {
                    CycleUtil c(10);
                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
                    ARMARX_IMPORTANT << "Create a new thread alone PlatformGlobalTrajectoryController controller";
                    while (getState() == eManagedIceObjectStarted)
                    {
                        if (isControllerActive() and rtReady.load())
                        {
                            ARMARX_VERBOSE << "additional task";
                            additionalTask();
                        }
                        c.waitForCycleDuration();
                    }
                });
 
        ARMARX_INFO << "PlatformGlobalTrajectoryController::onInitNJointController done.";
            
 
    }
 
    void
    Controller::rtPreActivateController()
    {
        // additionalTask() is not executed. Thus, we can access lastTwist without mutex.
        filteredTwist.reset();
 
        robotStateBuffer_rtToAdditionalTask.getWriteBuffer().global_T_robot.matrix() =
            rtGetRobot()->getGlobalPose();
        robotStateBuffer_rtToAdditionalTask.commitWrite();
 
        rtReady.store(true);
    }
 
    void
    Controller::rtPostDeactivateController()
    {
        rtReady.store(false);
    }
 
    Controller::~Controller() = default;
 
} // namespace armarx::navigation::platform_controller::platform_global_trajectory