PlatformLocalTrajectoryController.cpp

Go to the documentation of this file.

#include "PlatformLocalTrajectoryController.h"
 
#include <string>
 
#include <Ice/Current.h>
#include <IceUtil/Time.h>
 
#include <VirtualRobot/VirtualRobot.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 <ArmarXCore/interface/core/ManagedIceObjectDefinitions.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
#include <ArmarXCore/interface/serialization/Eigen/Eigen_fdi.h>
#include <ArmarXCore/observers/variant/Variant.h>
 
#include "RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h"
#include <RobotAPI/components/units/RobotUnit/ControlModes.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/interface/units/RobotUnit/NJointController.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
#include <armarx/control/interface/ConfigurableNJointControllerInterface.h> // IWYU pragma: keep
#include <armarx/navigation/common/controller_types.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/platform_controller/aron/PlatformLocalTrajectoryControllerConfig.aron.generated.h>
#include <armarx/navigation/platform_controller/aron_conversions.h>
 
namespace armarx::navigation::platform_controller::platform_local_trajectory
{
    const NJointControllerRegistration<Controller> Registration(
        common::ControllerTypeNames.to_name(common::ControllerType::PlatformLocalTrajectory));
 
    Controller::Controller(const RobotUnitPtr& robotUnit,
                           const NJointControllerConfigPtr& config,
                           const VirtualRobot::RobotPtr&)
    {
        ARMARX_IMPORTANT << "Creating "
                         << common::ControllerTypeNames.to_name(
                                common::ControllerType::PlatformLocalTrajectory);
        // 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(),
                                          ControlModes::HolonomicPlatformVelocity)
                             ->asA<ControlTargetHolonomicPlatformVelocity>();
        ARMARX_CHECK_EXPRESSION(platformTarget)
            << "The actuator " << robotUnit->getRobotPlatformName() << " 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::PlatformLocalTrajectory);
    }
 
    void
    Controller::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                      const IceUtil::Time& timeSinceLastIteration)
    {
        rtUpdateControlStruct();
 
        // update control devices
        platformTarget->velocityX = rtGetControlStruct().twistCommanded.linear.x();
        platformTarget->velocityY = rtGetControlStruct().twistCommanded.linear.y();
        platformTarget->velocityRotation = rtGetControlStruct().twistCommanded.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_INFO << "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_VERBOSE << "Trajectory with " << updateConfig.targets.trajectory.points().size();
 
        ARMARX_VERBOSE << "done Controller::updateConfig";
    }
 
    void
    Controller::additionalTask()
    {
        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 << "Trajectory is empty!";
 
            filteredTwist.reset();
 
            getWriterControlStruct().twistCommanded.reset();
            getWriterControlStruct().twistRaw = core::Twist::Zero();
            writeControlStruct();
            return;
        }
 
        ARMARX_TRACE;
 
        // make sure the parameters are up to date
        // i.e. velocityFactor and limits, as they can be changed on the fly
        trajectoryFollowingController->updateParams(configBuffer.params);
 
        // update controller
        const traj_ctrl::local::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
        {
            auto& writeBuffer = getWriterControlStruct();
            writeBuffer.twistRaw = result.twist;
            writeBuffer.twistCommanded = filteredTwist;
            writeBuffer.result = result;
        }
        writeControlStruct();
 
        // store results (onPublish)
        {
            auto& writeBuffer = targetBuffer_additionalTaskToOnPublish.getWriteBuffer();
            writeBuffer.twistRaw = result.twist;
            writeBuffer.twistCommanded = filteredTwist;
            writeBuffer.result = result;
        }
        targetBuffer_additionalTaskToOnPublish.commitWrite();
    }
 
    void
    Controller::onPublish(const SensorAndControl& sac,
                          const DebugDrawerInterfacePrx& debugDrawer,
                          const DebugObserverInterfacePrx& debugObservers)
    {
        StringVariantBaseMap datafields;
 
        const auto& targets = targetBuffer_additionalTaskToOnPublish.getUpToDateReadBuffer();
 
        datafields["vx"] = new Variant(targets.twistCommanded.linear.x());
        datafields["vy"] = new Variant(targets.twistCommanded.linear.y());
        datafields["v_linear"] = new Variant(targets.twistCommanded.linear.norm());
        datafields["vyaw"] = new Variant(targets.twistCommanded.angular);
 
        datafields["vx_raw"] = new Variant(targets.twistRaw.linear.x());
        datafields["vy_raw"] = new Variant(targets.twistRaw.linear.y());
        datafields["v_linear_raw"] = new Variant(targets.twistRaw.linear.norm());
        datafields["vyaw_raw"] = new Variant(targets.twistRaw.angular.z());
 
        datafields["idx"] = new Variant(targets.result.target.idx);
 
        if(not configBuffer_updateConfigToOnPublish.getUpToDateReadBuffer().targets.trajectory.points().empty())
        {
            datafields["dt_front"] = new Variant((armarx::Clock::Now() - configBuffer_updateConfigToOnPublish.getUpToDateReadBuffer().targets.trajectory.points().front().timestamp).toSecondsDouble());
        }
 
        datafields["trajectory_points"] =
            new Variant(configBuffer_updateConfigToOnPublish.getUpToDateReadBuffer()
                            .targets.trajectory.points()
                            .size());
 
        debugObservers->setDebugChannel(
            common::ControllerTypeNames.to_name(common::ControllerType::PlatformLocalTrajectory),
            datafields);
    }
 
    void
    Controller::onInitNJointController()
    {
        runTask("PlatformTrajectoryControllerAdditionalTask",
                [&]
                {
                    CycleUtil c(10);
                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
                    ARMARX_IMPORTANT << "Create a new thread alone PlatformTrajectory controller";
                    while (getState() == eManagedIceObjectStarted)
                    {
                        if (isControllerActive() and rtReady.load())
                        {
                            ARMARX_VERBOSE << "additional task";
                            additionalTask();
                        }
                        c.waitForCycleDuration();
                    }
                });
 
        ARMARX_INFO << "PlatformTrajectoryVelocityController::onInitNJointController";
    }
 
    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;
 
    ::armarx::aron::data::dto::DictPtr
    Controller::getConfig(const ::Ice::Current&)
    {
        ARMARX_ERROR << "NYI";
        return nullptr;
    }
} // namespace armarx::navigation::platform_controller::platform_local_trajectory