PlatformGlobalTrajectoryController.cpp

Go to the documentation of this file.

#include "PlatformGlobalTrajectoryController.h"
 
#include <string>
 
#include <Ice/Current.h>
#include <IceUtil/Time.h>
 
#include <SimoxUtility/math/convert/mat4f_to_rpy.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/observers/variant/Variant.h>
#include <ArmarXCore/util/CPPUtility/trace.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/platform_controller/aron/PlatformGlobalTrajectoryControllerConfig.aron.generated.h>
#include <armarx/navigation/platform_controller/aron_conversions.h>
#include <armarx/navigation/trajectory_control/global/TrajectoryController.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 " << QUOTED(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)
    {
        // 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();
    }
 
    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;
 
        // 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);
 
        // run the controller, resulting in the required twist and other values
        const armarx::navigation::traj_ctrl::global::TrajectoryControllerResult result =
            trajectoryFollowingController->control(
                configBuffer.targets.trajectory,
                robotStateBuffer_rtToAdditionalTask.getUpToDateReadBuffer().global_T_robot);
 
        // low-pass filter the twist
        {
            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.getWriteBuffer().global_T_robot =
            robotStateBuffer_rtToAdditionalTask.getReadBuffer().global_T_robot;
        targetBuffer_additionalTaskToOnPublish.commitWrite();
    }
 
    void
    Controller::onPublish(const SensorAndControl& /*sac*/,
                          const DebugDrawerInterfacePrx& /*debugDrawer*/,
                          const DebugObserverInterfacePrx& debugObservers)
    {
        StringVariantBaseMap datafields;
 
        const auto& debugStuff = targetBuffer_additionalTaskToOnPublish.getUpToDateReadBuffer();
        const auto& config = configBuffer_updateConfigToOnPublish.getUpToDateReadBuffer();
 
 
        datafields["vx"] = new Variant(debugStuff.target.linear.x());
        datafields["vy"] = new Variant(debugStuff.target.linear.y());
        datafields["v_linear"] = new Variant(debugStuff.target.linear.norm());
        datafields["vyaw"] = new Variant(debugStuff.target.angular);
        datafields["trajectory_points"] = new Variant(config.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);
 
        datafields["global_T_robot.x"] = new Variant(debugStuff.global_T_robot.translation().x());
        datafields["global_T_robot.y"] = new Variant(debugStuff.global_T_robot.translation().y());
        datafields["global_T_robot.o"] =
            new Variant(simox::math::mat4f_to_rpy(debugStuff.global_T_robot.matrix()).z());
 
        datafields["limitLinear"] = new Variant(config.params.limits.linear);
        datafields["limitAngular"] = new Variant(config.params.limits.angular);
 
        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;
 
    ::armarx::aron::data::dto::DictPtr
    Controller::getConfig(const ::Ice::Current&)
    {
        ARMARX_ERROR << "NYI";
        return nullptr;
    }
} // namespace armarx::navigation::platform_controller::platform_global_trajectory