From 5532a311eed5c15daad931031e8b4bc2b0377a68 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 13 Jul 2022 13:29:35 +0200
Subject: [PATCH] platform_controller: tested in simulation
---
.../PlatformTrajectoryController.cpp | 79 +++++++++++++++++--
1 file changed, 74 insertions(+), 5 deletions(-)
diff --git a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
index b1fd2da7..91106344 100644
--- a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
+++ b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
@@ -1,21 +1,31 @@
#include "PlatformTrajectoryController.h"
+#include "ArmarXCore/core/ArmarXObjectScheduler.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/navigation/common/controller_types.h>
-#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
-#include <armarx/navigation/platform_controller/aron_conversions.h>
#include <armarx/control/common/aron_conversions.h>
#include <armarx/control/common/type.h>
#include <armarx/control/common/utils.h>
+#include <armarx/navigation/common/controller_types.h>
+#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
+#include <armarx/navigation/platform_controller/aron_conversions.h>
namespace armarx::navigation::platform_controller::platform_trajectory
{
+ const NJointControllerRegistration<Controller> Registration(
+ common::ControllerTypeNames.to_name(common::ControllerType::PlatformTrajectory));
+
Controller::Controller(const RobotUnitPtr& robotUnit,
const NJointControllerConfigPtr& config,
const VirtualRobot::RobotPtr&)
{
-
+ ARMARX_IMPORTANT << "Creating "
+ << common::ControllerTypeNames.to_name(
+ common::ControllerType::PlatformTrajectory);
// config
ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
ARMARX_CHECK_EXPRESSION(cfg);
@@ -32,7 +42,6 @@ namespace armarx::navigation::platform_controller::platform_trajectory
<< "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;
@@ -55,6 +64,11 @@ namespace armarx::navigation::platform_controller::platform_trajectory
Controller::rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
{
+ rtUpdateControlStruct();
+
+ platformTarget->velocityX = rtGetControlStruct().platformVelocityTargets.x;
+ platformTarget->velocityY = rtGetControlStruct().platformVelocityTargets.y;
+ platformTarget->velocityRotation = rtGetControlStruct().platformVelocityTargets.yaw;
}
void
@@ -68,12 +82,39 @@ namespace armarx::navigation::platform_controller::platform_trajectory
auto updateConfig = ::armarx::fromAron<arondto::Config, Config>(dto);
configBuffer.reinitAllBuffers(updateConfig);
+ ARMARX_INFO << "Trajectory with " << updateConfig.targets.trajectory.points().size();
+
ARMARX_IMPORTANT << "done Controller::updateConfig";
}
void
Controller::additionalTask()
{
+ ARMARX_CHECK(trajectoryFollowingController.has_value());
+
+ // if trajectory is empty, set velocity to 0
+ if (configBuffer.getUpToDateReadBuffer().targets.trajectory.points().empty())
+ {
+ ARMARX_INFO << deactivateSpam(1) << "Trajectory is empty!";
+
+ getWriterControlStruct().platformVelocityTargets.x = 0;
+ getWriterControlStruct().platformVelocityTargets.y = 0;
+ getWriterControlStruct().platformVelocityTargets.yaw = 0;
+ writeControlStruct();
+ return;
+ }
+
+ // update controller
+ const armarx::navigation::traj_ctrl::TrajectoryControllerResult result =
+ trajectoryFollowingController->control(
+ configBuffer.getUpToDateReadBuffer().targets.trajectory);
+
+ // store result
+ getWriterControlStruct().platformVelocityTargets.x = result.twist.linear.x();
+ getWriterControlStruct().platformVelocityTargets.y = result.twist.linear.y();
+ getWriterControlStruct().platformVelocityTargets.yaw = result.twist.angular.z();
+
+ writeControlStruct();
}
void
@@ -81,16 +122,44 @@ namespace armarx::navigation::platform_controller::platform_trajectory
const DebugDrawerInterfacePrx& debugDrawer,
const DebugObserverInterfacePrx& debugObservers)
{
+ StringVariantBaseMap datafields;
+
+ datafields["vx"] = new Variant(rtGetControlStruct().platformVelocityTargets.x);
+ datafields["vy"] = new Variant(rtGetControlStruct().platformVelocityTargets.y);
+ datafields["vyaw"] = new Variant(rtGetControlStruct().platformVelocityTargets.yaw);
+
+ debugObservers->setDebugChannel(
+ common::ControllerTypeNames.to_name(common::ControllerType::PlatformTrajectory),
+ 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()
{
+ rtReady.store(true);
}
--
GitLab