From 4754489619945c83b36cefc9c2ed144df6624220 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 25 Jul 2022 23:23:14 +0200
Subject: [PATCH] restructured trajectory following
---
.../components/example_client/Component.cpp | 8 +-
.../client/NavigationStackConfig.cpp | 4 +-
.../navigation/client/NavigationStackConfig.h | 5 +-
.../services/EventSubscriptionInterface.h | 8 +-
.../client/services/SimpleEventHandler.cpp | 21 +-
.../client/services/SimpleEventHandler.h | 7 +-
.../components/Navigator/RemoteGui.cpp | 4 +-
source/armarx/navigation/core/Trajectory.h | 2 +-
.../factories/TrajectoryControllerFactory.cpp | 21 +-
.../factories/TrajectoryControllerFactory.h | 15 +-
.../navigation/local_planning/LocalPlanner.h | 4 +-
.../memory/client/stack_result/Writer.cpp | 22 +-
.../memory/client/stack_result/Writer.h | 16 +-
.../platform_controller/CMakeLists.txt | 4 +-
.../PlatformGlobalTrajectoryController.cpp | 11 +-
.../PlatformGlobalTrajectoryController.h | 7 +-
.../PlatformLocalTrajectoryController.cpp | 168 +++++++++++++++
.../PlatformLocalTrajectoryController.h | 129 ++++++++++++
...atformGlobalTrajectoryControllerConfig.xml | 13 +-
...latformLocalTrajectoryControllerConfig.xml | 13 +-
.../platform_controller/aron_conversions.cpp | 32 +--
.../platform_controller/aron_conversions.h | 32 ++-
.../controller_descriptions.h | 22 +-
.../navigation/server/NavigationStack.h | 3 +-
source/armarx/navigation/server/Navigator.cpp | 4 +-
source/armarx/navigation/server/Navigator.h | 2 +-
source/armarx/navigation/server/StackResult.h | 11 +-
.../EventPublishingInterface.h | 6 +-
.../event_publishing/MemoryPublisher.cpp | 11 +-
.../server/event_publishing/MemoryPublisher.h | 4 +-
.../server/execution/ExecutorInterface.h | 9 +-
.../execution/PlatformControllerExecutor.cpp | 19 +-
.../execution/PlatformControllerExecutor.h | 4 +-
.../introspection/ArvizIntrospector.cpp | 21 +-
.../server/introspection/ArvizIntrospector.h | 10 +-
.../introspection/IntrospectorInterface.h | 2 +-
.../NavigationGroup/NavigateToLocation.cpp | 6 +-
.../trajectory_control/CMakeLists.txt | 70 +++++--
.../TrajectoryController.cpp | 9 -
.../global/TrajectoryController.h | 77 +++++++
.../TrajectoryFollowingController.cpp | 16 +-
.../TrajectoryFollowingController.h | 6 +-
.../{ => global}/WaypointController.cpp | 14 +-
.../{ => global}/WaypointController.h | 6 +-
.../aron/TrajectoryControllerParams.xml | 19 ++
.../TrajectoryFollowingControllerParams.xml | 8 +-
.../aron/WaypointControllerParams.xml | 4 +-
.../{ => global}/aron_conversions.cpp | 16 +-
.../{ => global}/aron_conversions.h | 6 +-
.../trajectory_control/{ => global}/core.h | 6 +-
.../{ => local}/TrajectoryController.h | 8 +-
.../local/TrajectoryFollowingController.cpp | 197 ++++++++++++++++++
.../local/TrajectoryFollowingController.h | 71 +++++++
.../aron/TrajectoryControllerParams.xml | 4 +-
.../TrajectoryFollowingControllerParams.xml | 31 +++
.../local/aron_conversions.cpp | 35 ++++
.../local/aron_conversions.h | 45 ++++
.../trajectory_control/local/core.h | 36 ++++
58 files changed, 1105 insertions(+), 259 deletions(-)
create mode 100644 source/armarx/navigation/platform_controller/PlatformLocalTrajectoryController.cpp
create mode 100644 source/armarx/navigation/platform_controller/PlatformLocalTrajectoryController.h
delete mode 100644 source/armarx/navigation/trajectory_control/TrajectoryController.cpp
create mode 100644 source/armarx/navigation/trajectory_control/global/TrajectoryController.h
rename source/armarx/navigation/trajectory_control/{ => global}/TrajectoryFollowingController.cpp (93%)
rename source/armarx/navigation/trajectory_control/{ => global}/TrajectoryFollowingController.h (90%)
rename source/armarx/navigation/trajectory_control/{ => global}/WaypointController.cpp (66%)
rename source/armarx/navigation/trajectory_control/{ => global}/WaypointController.h (90%)
create mode 100644 source/armarx/navigation/trajectory_control/global/aron/TrajectoryControllerParams.xml
rename source/armarx/navigation/trajectory_control/{ => global}/aron/TrajectoryFollowingControllerParams.xml (77%)
rename source/armarx/navigation/trajectory_control/{ => global}/aron/WaypointControllerParams.xml (81%)
rename source/armarx/navigation/trajectory_control/{ => global}/aron_conversions.cpp (74%)
rename source/armarx/navigation/trajectory_control/{ => global}/aron_conversions.h (93%)
rename source/armarx/navigation/trajectory_control/{ => global}/core.h (89%)
rename source/armarx/navigation/trajectory_control/{ => local}/TrajectoryController.h (89%)
create mode 100644 source/armarx/navigation/trajectory_control/local/TrajectoryFollowingController.cpp
create mode 100644 source/armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h
rename source/armarx/navigation/trajectory_control/{ => local}/aron/TrajectoryControllerParams.xml (72%)
create mode 100644 source/armarx/navigation/trajectory_control/local/aron/TrajectoryFollowingControllerParams.xml
create mode 100644 source/armarx/navigation/trajectory_control/local/aron_conversions.cpp
create mode 100644 source/armarx/navigation/trajectory_control/local/aron_conversions.h
create mode 100644 source/armarx/navigation/trajectory_control/local/core.h
diff --git a/examples/components/example_client/Component.cpp b/examples/components/example_client/Component.cpp
index 29001741..afbf7503 100644
--- a/examples/components/example_client/Component.cpp
+++ b/examples/components/example_client/Component.cpp
@@ -40,7 +40,7 @@
#include <armarx/navigation/global_planning/Point2Point.h>
#include <armarx/navigation/client/PathBuilder.h>
#include <armarx/navigation/global_planning/AStar.h>
-#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
namespace armarx::navigation::components::example_client
@@ -108,7 +108,7 @@ namespace armarx::navigation::components::example_client
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
.globalPlanner(global_planning::AStarParams())
- .trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams()));
+ .trajectoryController(traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
// Example of registering a lambda as callback.
getNavigator().onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached! (lambda-style)"; });
@@ -207,7 +207,7 @@ namespace armarx::navigation::components::example_client
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
.globalPlanner(global_planning::AStarParams())
- .trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams()));
+ .trajectoryController(traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
// Example of registering a lambda as callback.
getNavigator().onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached! (lambda-style)"; });
@@ -283,7 +283,7 @@ namespace armarx::navigation::components::example_client
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
.globalPlanner(global_planning::Point2PointParams())
- .trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams()));
+ .trajectoryController(traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
Clock::WaitFor(Duration::Seconds(1));
diff --git a/source/armarx/navigation/client/NavigationStackConfig.cpp b/source/armarx/navigation/client/NavigationStackConfig.cpp
index 5e23bb7f..314a8e29 100644
--- a/source/armarx/navigation/client/NavigationStackConfig.cpp
+++ b/source/armarx/navigation/client/NavigationStackConfig.cpp
@@ -88,12 +88,12 @@ namespace armarx::navigation::client
}
NavigationStackConfig&
- NavigationStackConfig::trajectoryController(const traj_ctrl::TrajectoryControllerParams& params)
+ NavigationStackConfig::trajectoryController(const traj_ctrl::local::TrajectoryControllerParams& params)
{
aron::data::DictPtr element(new aron::data::Dict);
element->addElement(core::NAME_KEY,
std::make_shared<aron::data::String>(
- traj_ctrl::AlgorithmNames.to_name(params.algorithm())));
+ traj_ctrl::local::AlgorithmNames.to_name(params.algorithm())));
element->addElement(core::PARAMS_KEY, params.toAron());
dict.addElement(core::StackLayerNames.to_name(core::StackLayer::TrajectoryController),
diff --git a/source/armarx/navigation/client/NavigationStackConfig.h b/source/armarx/navigation/client/NavigationStackConfig.h
index 2e3d6901..e37dfa4a 100644
--- a/source/armarx/navigation/client/NavigationStackConfig.h
+++ b/source/armarx/navigation/client/NavigationStackConfig.h
@@ -33,7 +33,8 @@
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/safety_control/SafetyController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/global/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
namespace armarx::navigation::client
@@ -64,7 +65,7 @@ namespace armarx::navigation::client
NavigationStackConfig& localPlanner(const loc_plan::LocalPlannerParams& params);
NavigationStackConfig&
- trajectoryController(const traj_ctrl::TrajectoryControllerParams& params);
+ trajectoryController(const traj_ctrl::local::TrajectoryControllerParams& params);
NavigationStackConfig& safetyController(const safe_ctrl::SafetyControllerParams& params);
diff --git a/source/armarx/navigation/client/services/EventSubscriptionInterface.h b/source/armarx/navigation/client/services/EventSubscriptionInterface.h
index 91fe8722..82bd26cf 100644
--- a/source/armarx/navigation/client/services/EventSubscriptionInterface.h
+++ b/source/armarx/navigation/client/services/EventSubscriptionInterface.h
@@ -1,14 +1,11 @@
#pragma once
-
-// STD/STL
#include <functional>
-// Navigation
#include <armarx/navigation/core/events.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
namespace armarx::navigation::client
@@ -18,7 +15,7 @@ namespace armarx::navigation::client
std::function<void(const global_planning::GlobalPlannerResult&)>;
using LocalTrajectoryUpdatedCallback = std::function<void(const loc_plan::LocalPlannerResult&)>;
using TrajectoryControllerUpdatedCallback =
- std::function<void(const traj_ctrl::TrajectoryControllerResult&)>;
+ std::function<void(const traj_ctrl::local::TrajectoryControllerResult&)>;
using GlobalPlanningFailedCallback =
std::function<void(const core::GlobalPlanningFailedEvent&)>;
@@ -54,7 +51,6 @@ namespace armarx::navigation::client
virtual void onGlobalPlanningFailed(const GlobalPlanningFailedCallback& callback) = 0;
// Non-API.
- public:
virtual ~EventSubscriptionInterface() = default;
};
diff --git a/source/armarx/navigation/client/services/SimpleEventHandler.cpp b/source/armarx/navigation/client/services/SimpleEventHandler.cpp
index cda19d47..0c2b8055 100644
--- a/source/armarx/navigation/client/services/SimpleEventHandler.cpp
+++ b/source/armarx/navigation/client/services/SimpleEventHandler.cpp
@@ -55,7 +55,8 @@ armarx::navigation::client::SimpleEventHandler::SimpleEventHandler::onMovementSt
}
void
-armarx::navigation::client::SimpleEventHandler::onGlobalPlanningFailed(const GlobalPlanningFailedCallback& callback)
+armarx::navigation::client::SimpleEventHandler::onGlobalPlanningFailed(
+ const GlobalPlanningFailedCallback& callback)
{
subscriptions.globalPlanningFailedCallbacks.push_back(callback);
}
@@ -143,15 +144,15 @@ namespace armarx::navigation::client
}
}
- void
- SimpleEventHandler::trajectoryControllerUpdated(
- const traj_ctrl::TrajectoryControllerResult& event)
- {
- for (const auto& callback : subscriptions.trajectoryControllerUpdatedCallbacks)
- {
- callback(event);
- }
- }
+ // void
+ // SimpleEventHandler::trajectoryControllerUpdated(
+ // const traj_ctrl::local::TrajectoryControllerResult& event)
+ // {
+ // for (const auto& callback : subscriptions.trajectoryControllerUpdatedCallbacks)
+ // {
+ // callback(event);
+ // }
+ // }
void
SimpleEventHandler::globalPlanningFailed(const core::GlobalPlanningFailedEvent& event)
diff --git a/source/armarx/navigation/client/services/SimpleEventHandler.h b/source/armarx/navigation/client/services/SimpleEventHandler.h
index 3f5fea9a..faa56a89 100644
--- a/source/armarx/navigation/client/services/SimpleEventHandler.h
+++ b/source/armarx/navigation/client/services/SimpleEventHandler.h
@@ -14,8 +14,9 @@ namespace armarx::navigation::client
virtual public server::EventPublishingInterface
{
- // EventSubscriptionInterface
public:
+
+ // EventSubscriptionInterface
void onGoalReached(const OnGoalReachedCallback& callback) override;
void onWaypointReached(const OnWaypointReachedCallback& callback) override;
void
@@ -28,11 +29,9 @@ namespace armarx::navigation::client
void onGlobalPlanningFailed(const GlobalPlanningFailedCallback& callback) override;
// EventPublishingInterface
- public:
void globalTrajectoryUpdated(const global_planning::GlobalPlannerResult& event) override;
void localTrajectoryUpdated(const loc_plan::LocalPlannerResult& event) override;
- void
- trajectoryControllerUpdated(const traj_ctrl::TrajectoryControllerResult& event) override;
+ // void trajectoryControllerUpdated(const traj_ctrl::local::TrajectoryControllerResult& event) override;
void globalPlanningFailed(const core::GlobalPlanningFailedEvent& event) override;
void movementStarted(const core::MovementStartedEvent& event) override;
diff --git a/source/armarx/navigation/components/Navigator/RemoteGui.cpp b/source/armarx/navigation/components/Navigator/RemoteGui.cpp
index 9ab923ba..83e93879 100644
--- a/source/armarx/navigation/components/Navigator/RemoteGui.cpp
+++ b/source/armarx/navigation/components/Navigator/RemoteGui.cpp
@@ -23,7 +23,7 @@
#include <armarx/navigation/global_planning/AStar.h>
#include <armarx/navigation/global_planning/Point2Point.h>
#include <armarx/navigation/server/Navigator.h>
-#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
#include <armarx/navigation/util/util.h>
namespace armarx::navigation::components
@@ -254,7 +254,7 @@ namespace armarx::navigation::components
client::NavigationStackConfig cfg;
cfg.globalPlanner(global_planning::SPFAParams());
- cfg.trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams());
+ cfg.trajectoryController(traj_ctrl::local::TrajectoryFollowingControllerParams());
const auto stackConfig = cfg.toAron();
diff --git a/source/armarx/navigation/core/Trajectory.h b/source/armarx/navigation/core/Trajectory.h
index 245f5dc9..747c1cc3 100644
--- a/source/armarx/navigation/core/Trajectory.h
+++ b/source/armarx/navigation/core/Trajectory.h
@@ -124,8 +124,8 @@ namespace armarx::navigation::core
struct LocalTrajectoryPoint
{
- core::Pose pose;
DateTime timestamp;
+ core::Pose pose;
};
using LocalTrajectory = std::vector<LocalTrajectoryPoint>;
diff --git a/source/armarx/navigation/factories/TrajectoryControllerFactory.cpp b/source/armarx/navigation/factories/TrajectoryControllerFactory.cpp
index 3b8f6a39..0e628b27 100644
--- a/source/armarx/navigation/factories/TrajectoryControllerFactory.cpp
+++ b/source/armarx/navigation/factories/TrajectoryControllerFactory.cpp
@@ -5,17 +5,16 @@
#include <RobotAPI/libraries/aron/core/data/variant/primitive/String.h>
#include <armarx/navigation/core/constants.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
-#include <armarx/navigation/trajectory_control/WaypointController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
namespace armarx::navigation::fac
{
- traj_ctrl::TrajectoryControllerPtr
+ traj_ctrl::local::TrajectoryControllerPtr
TrajectoryControllerFactory::create(const VirtualRobot::RobotPtr& robot,
const aron::data::DictPtr& params)
{
- namespace layer = traj_ctrl;
+ namespace layer = traj_ctrl::local;
if (not params)
{
@@ -31,16 +30,12 @@ namespace armarx::navigation::fac
const auto algoParams = aron::data::Dict::DynamicCast(params->getElement(core::PARAMS_KEY));
ARMARX_CHECK_NOT_NULL(algoParams);
- traj_ctrl::TrajectoryControllerPtr controller;
+ traj_ctrl::local::TrajectoryControllerPtr controller;
switch (algo)
{
- case traj_ctrl::Algorithms::WaypointController:
- controller = std::make_shared<traj_ctrl::WaypointController>(
- traj_ctrl::WaypointControllerParams::FromAron(algoParams));
- break;
- case traj_ctrl::Algorithms::TrajectoryFollowingController:
- controller = std::make_shared<traj_ctrl::TrajectoryFollowingController>(
- robot, traj_ctrl::TrajectoryFollowingControllerParams::FromAron(algoParams));
+ case traj_ctrl::local::Algorithms::TrajectoryFollowingController:
+ controller = std::make_shared<traj_ctrl::local::TrajectoryFollowingController>(
+ robot, traj_ctrl::local::TrajectoryFollowingControllerParams::FromAron(algoParams));
break;
}
diff --git a/source/armarx/navigation/factories/TrajectoryControllerFactory.h b/source/armarx/navigation/factories/TrajectoryControllerFactory.h
index cd35d860..e2528c95 100644
--- a/source/armarx/navigation/factories/TrajectoryControllerFactory.h
+++ b/source/armarx/navigation/factories/TrajectoryControllerFactory.h
@@ -22,27 +22,26 @@
#pragma once
-// #include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <VirtualRobot/VirtualRobot.h>
+
#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
#include <armarx/navigation/core/fwd.h>
#include <armarx/navigation/factories/fwd.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
namespace armarx::navigation::fac
{
/**
- * @brief Factory to create a traj_ctrl::TrajectoryController
- *
- */
+ * @brief Factory to create a traj_ctrl::TrajectoryController
+ *
+ */
class TrajectoryControllerFactory
{
public:
- static traj_ctrl::TrajectoryControllerPtr create(const VirtualRobot::RobotPtr& robot,
- const aron::data::DictPtr& params);
+ static traj_ctrl::local::TrajectoryControllerPtr create(const VirtualRobot::RobotPtr& robot,
+ const aron::data::DictPtr& params);
- protected:
- private:
};
} // namespace armarx::navigation::fac
diff --git a/source/armarx/navigation/local_planning/LocalPlanner.h b/source/armarx/navigation/local_planning/LocalPlanner.h
index 46aa7ede..6ae1c69c 100644
--- a/source/armarx/navigation/local_planning/LocalPlanner.h
+++ b/source/armarx/navigation/local_planning/LocalPlanner.h
@@ -28,7 +28,7 @@
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-#include "core.h"
+#include <armarx/navigation/local_planning/core.h>
#include <armarx/navigation/core/DynamicScene.h>
#include <armarx/navigation/core/StaticScene.h>
#include <armarx/navigation/core/Trajectory.h>
@@ -38,7 +38,7 @@ namespace armarx::navigation::loc_plan
{
struct LocalPlannerResult
{
- core::GlobalTrajectory trajectory;
+ core::LocalTrajectory trajectory;
};
struct LocalPlannerParams
diff --git a/source/armarx/navigation/memory/client/stack_result/Writer.cpp b/source/armarx/navigation/memory/client/stack_result/Writer.cpp
index e693495e..4a3a3a75 100644
--- a/source/armarx/navigation/memory/client/stack_result/Writer.cpp
+++ b/source/armarx/navigation/memory/client/stack_result/Writer.cpp
@@ -4,6 +4,7 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
+#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/core/aron/Trajectory.aron.generated.h>
#include <armarx/navigation/core/aron/Twist.aron.generated.h>
#include <armarx/navigation/core/aron_conversions.h>
@@ -17,8 +18,8 @@ namespace armarx::navigation::memory::client::stack_result
armem::Commit commit;
storePrepare(result.globalPlan, clientID, commit);
- // FIXME storePrepare(result.localTrajectory, clientID, commit);
- storePrepare(result.controlVelocity, clientID, commit);
+ storePrepare(result.localTrajectory, clientID, commit);
+ // storePrepare(result.controlVelocity, clientID, commit);
// FIXME storePrepare(result.safeVelocity, clientID, commit);
std::lock_guard g{memoryWriterMutex()};
@@ -45,7 +46,7 @@ namespace armarx::navigation::memory::client::stack_result
}
bool
- Writer::store(const armarx::navigation::traj_ctrl::TrajectoryControllerResult& result,
+ Writer::store(const armarx::navigation::loc_plan::LocalPlannerResult& result,
const std::string& clientID)
{
armem::Commit commit;
@@ -55,6 +56,7 @@ namespace armarx::navigation::memory::client::stack_result
return updateResult.allSuccess();
}
+
bool
Writer::storePrepare(const armarx::navigation::global_planning::GlobalPlannerResult& result,
const std::string& clientID,
@@ -84,8 +86,9 @@ namespace armarx::navigation::memory::client::stack_result
return true;
}
+
bool
- Writer::storePrepare(const armarx::navigation::traj_ctrl::TrajectoryControllerResult& result,
+ Writer::storePrepare(const loc_plan::LocalPlannerResult& result,
const std::string& clientID,
armem::Commit& commit)
{
@@ -95,15 +98,15 @@ namespace armarx::navigation::memory::client::stack_result
armem::EntityUpdate update;
update.entityID = armem::MemoryID()
.withMemoryName(properties().memoryName)
- .withCoreSegmentName("Results_TrajectoryController")
+ .withCoreSegmentName("Results_LocalPlanner")
.withProviderSegmentName(clientID)
- .withEntityName("velocity")
+ .withEntityName("trajectory")
.withTimestamp(timestamp);
- core::arondto::Twist aronDto;
- // FIXME own type
- core::toAron(aronDto, result.twist);
+ core::arondto::LocalTrajectory aronDto;
+ // FIXME create Aron type
+ toAron(aronDto, result.trajectory);
update.timeCreated = timestamp;
update.instancesData = {aronDto.toAron()};
@@ -113,6 +116,7 @@ namespace armarx::navigation::memory::client::stack_result
return true;
}
+
std::string
Writer::propertyPrefix() const
{
diff --git a/source/armarx/navigation/memory/client/stack_result/Writer.h b/source/armarx/navigation/memory/client/stack_result/Writer.h
index 1ece5594..2e29333f 100644
--- a/source/armarx/navigation/memory/client/stack_result/Writer.h
+++ b/source/armarx/navigation/memory/client/stack_result/Writer.h
@@ -24,9 +24,10 @@
#include <RobotAPI/libraries/armem/client/util/SimpleWriterBase.h>
#include <RobotAPI/libraries/armem/core/Commit.h>
+#include "armarx/navigation/local_planning/LocalPlanner.h"
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/server/StackResult.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
namespace armarx::navigation::memory::client::stack_result
{
@@ -40,7 +41,8 @@ namespace armarx::navigation::memory::client::stack_result
bool store(const armarx::navigation::global_planning::GlobalPlannerResult& result,
const std::string& clientID);
- bool store(const armarx::navigation::traj_ctrl::TrajectoryControllerResult& result,
+
+ bool store(const armarx::navigation::loc_plan::LocalPlannerResult& result,
const std::string& clientID);
@@ -49,18 +51,10 @@ namespace armarx::navigation::memory::client::stack_result
const std::string& clientID,
armem::Commit& commit);
- // bool storePrepare(const server::StackResult::LocalPlannerResult& result,
- // const std::string& clientID,
- // armem::Commit& commit);
-
- bool storePrepare(const armarx::navigation::traj_ctrl::TrajectoryControllerResult& result,
+ bool storePrepare(const loc_plan::LocalPlannerResult& result,
const std::string& clientID,
armem::Commit& commit);
- // bool storePrepare(const server::StackResult::SafetyControllerResult& result,
- // const std::string& clientID,
- // armem::Commit& commit);
-
protected:
std::string propertyPrefix() const override;
Properties defaultProperties() const override;
diff --git a/source/armarx/navigation/platform_controller/CMakeLists.txt b/source/armarx/navigation/platform_controller/CMakeLists.txt
index a5a21b12..dd919b00 100644
--- a/source/armarx/navigation/platform_controller/CMakeLists.txt
+++ b/source/armarx/navigation/platform_controller/CMakeLists.txt
@@ -8,11 +8,11 @@ armarx_add_aron_library(platform_controller_aron
armarx_add_library(platform_controller
SOURCES
- # PlatformLocalTrajectoryController.cpp
+ PlatformLocalTrajectoryController.cpp
PlatformGlobalTrajectoryController.cpp
aron_conversions.cpp
HEADERS
- # PlatformLocalTrajectoryController.h
+ PlatformLocalTrajectoryController.h
PlatformGlobalTrajectoryController.h
aron_conversions.h
DEPENDENCIES_PUBLIC
diff --git a/source/armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.cpp b/source/armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.cpp
index a8758b6c..3e46aa55 100644
--- a/source/armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.cpp
+++ b/source/armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.cpp
@@ -105,7 +105,7 @@ namespace armarx::navigation::platform_controller::platform_global_trajectory
}
// update controller
- const armarx::navigation::traj_ctrl::TrajectoryControllerResult result =
+ const armarx::navigation::traj_ctrl::global::TrajectoryControllerResult result =
trajectoryFollowingController->control(
configBuffer.getUpToDateReadBuffer().targets.trajectory);
@@ -118,8 +118,8 @@ namespace armarx::navigation::platform_controller::platform_global_trajectory
}
void
- Controller::onPublish(const SensorAndControl& sac,
- const DebugDrawerInterfacePrx& debugDrawer,
+ Controller::onPublish(const SensorAndControl& /*sac*/,
+ const DebugDrawerInterfacePrx& /*debugDrawer*/,
const DebugObserverInterfacePrx& debugObservers)
{
StringVariantBaseMap datafields;
@@ -127,7 +127,8 @@ namespace armarx::navigation::platform_controller::platform_global_trajectory
datafields["vx"] = new Variant(rtGetControlStruct().platformVelocityTargets.x);
datafields["vy"] = new Variant(rtGetControlStruct().platformVelocityTargets.y);
datafields["vyaw"] = new Variant(rtGetControlStruct().platformVelocityTargets.yaw);
- datafields["trajectory_points"] = new Variant(configBuffer.getUpToDateReadBuffer().targets.trajectory.points().size());
+ datafields["trajectory_points"] =
+ new Variant(configBuffer.getUpToDateReadBuffer().targets.trajectory.points().size());
debugObservers->setDebugChannel(
common::ControllerTypeNames.to_name(common::ControllerType::PlatformGlobalTrajectory),
@@ -166,4 +167,4 @@ namespace armarx::navigation::platform_controller::platform_global_trajectory
Controller::~Controller() = default;
-} // namespace armarx::navigation::platform_controller::platform_trajectory
+} // namespace armarx::navigation::platform_controller::platform_global_trajectory
diff --git a/source/armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.h b/source/armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.h
index 65d7d79a..e9fee601 100644
--- a/source/armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.h
+++ b/source/armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.h
@@ -29,9 +29,10 @@
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h>
+#include "armarx/navigation/trajectory_control/global/TrajectoryFollowingController.h"
#include <armarx/control/interface/ConfigurableNJointControllerInterface.h>
#include <armarx/navigation/core/types.h>
-#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
namespace armarx
{
@@ -49,7 +50,7 @@ namespace armarx::navigation::platform_controller::platform_global_trajectory
struct Config
{
- using Params = traj_ctrl::TrajectoryFollowingControllerParams;
+ using Params = traj_ctrl::global::TrajectoryFollowingControllerParams;
Params params;
Targets targets;
@@ -120,7 +121,7 @@ namespace armarx::navigation::platform_controller::platform_global_trajectory
ControlTargetHolonomicPlatformVelocity* platformTarget;
- std::optional<traj_ctrl::TrajectoryFollowingController> trajectoryFollowingController;
+ std::optional<traj_ctrl::global::TrajectoryFollowingController> trajectoryFollowingController;
Devices getDevices(const VirtualRobot::RobotNodeSet& rns);
diff --git a/source/armarx/navigation/platform_controller/PlatformLocalTrajectoryController.cpp b/source/armarx/navigation/platform_controller/PlatformLocalTrajectoryController.cpp
new file mode 100644
index 00000000..eff20f7b
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/PlatformLocalTrajectoryController.cpp
@@ -0,0 +1,168 @@
+#include "PlatformLocalTrajectoryController.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/control/common/aron_conversions.h>
+#include <armarx/control/common/type.h>
+#include <armarx/navigation/common/controller_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.reinitAllBuffers(configData);
+
+ trajectoryFollowingController.emplace(robot, 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();
+
+ 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);
+
+ ARMARX_INFO << "Trajectory with " << updateConfig.targets.trajectory.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.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 auto 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
+ Controller::onPublish(const SensorAndControl& sac,
+ 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);
+ datafields["trajectory_points"] =
+ new Variant(configBuffer.getUpToDateReadBuffer().targets.trajectory.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()
+ {
+ rtReady.store(true);
+ }
+
+
+ Controller::~Controller() = default;
+
+} // namespace armarx::navigation::platform_controller::platform_local_trajectory
diff --git a/source/armarx/navigation/platform_controller/PlatformLocalTrajectoryController.h b/source/armarx/navigation/platform_controller/PlatformLocalTrajectoryController.h
new file mode 100644
index 00000000..8aa75fb7
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/PlatformLocalTrajectoryController.h
@@ -0,0 +1,129 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h>
+
+#include <armarx/control/interface/ConfigurableNJointControllerInterface.h>
+#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
+
+namespace armarx
+{
+ class ControlTargetHolonomicPlatformVelocity;
+ class SensorValueHolonomicPlatformWithAbsolutePosition;
+} // namespace armarx
+
+
+namespace armarx::navigation::platform_controller::platform_local_trajectory
+{
+ struct Targets
+ {
+ core::LocalTrajectory trajectory;
+ };
+
+ struct Config
+ {
+ using Params = traj_ctrl::local::TrajectoryFollowingControllerParams;
+
+ Params params;
+ Targets targets;
+ };
+
+ struct Target
+ {
+ struct
+ {
+ double x = 0;
+ double y = 0;
+ double yaw = 0;
+ } platformVelocityTargets;
+
+ void
+ reset()
+ {
+ platformVelocityTargets.x = 0;
+ platformVelocityTargets.y = 0;
+ platformVelocityTargets.yaw = 0;
+ }
+ };
+
+ struct Devices
+ {
+ ControlTargetHolonomicPlatformVelocity* platformTarget;
+ };
+
+ using NameValueMap = std::map<std::string, float>;
+
+ class Controller :
+ virtual public NJointControllerWithTripleBuffer<Target>,
+ virtual public armarx::control::ConfigurableNJointControllerInterface
+ {
+ public:
+ using ConfigPtrT = control::ConfigurableNJointControllerConfigPtr;
+
+ Controller(const RobotUnitPtr& robotUnit,
+ const NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&);
+
+ ~Controller() override;
+
+ std::string getClassName(const Ice::Current& iceCurrent = Ice::emptyCurrent) const override;
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+
+ void updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
+ const Ice::Current& iceCurrent = Ice::emptyCurrent) override;
+
+ protected:
+ void additionalTask();
+ void onPublish(const SensorAndControl& sac,
+ const DebugDrawerInterfacePrx& debugDrawer,
+ const DebugObserverInterfacePrx& debugObservers) override;
+
+
+ void onInitNJointController() override;
+ void rtPreActivateController() override;
+
+ private:
+ TripleBuffer<Config> configBuffer;
+
+ // internal
+ std::atomic_bool rtFirstRun = true;
+ std::atomic_bool rtReady = false;
+
+ ControlTargetHolonomicPlatformVelocity* platformTarget;
+
+ std::optional<traj_ctrl::local::TrajectoryFollowingController> trajectoryFollowingController;
+
+
+ Devices getDevices(const VirtualRobot::RobotNodeSet& rns);
+ };
+
+} // namespace armarx::navigation::platform_controller::platform_local_trajectory
diff --git a/source/armarx/navigation/platform_controller/aron/PlatformGlobalTrajectoryControllerConfig.xml b/source/armarx/navigation/platform_controller/aron/PlatformGlobalTrajectoryControllerConfig.xml
index 515b1148..455dfa22 100644
--- a/source/armarx/navigation/platform_controller/aron/PlatformGlobalTrajectoryControllerConfig.xml
+++ b/source/armarx/navigation/platform_controller/aron/PlatformGlobalTrajectoryControllerConfig.xml
@@ -1,18 +1,17 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<AronIncludes>
- <Include include="../../core/aron/TwistLimits.xml" />
- <Include include="../../core/aron/PIDParams.xml" />
- <Include include="../../core/aron/Trajectory.xml" />
- <Include include="../../trajectory_control/aron/TrajectoryFollowingControllerParams.xml" />
- <!-- <Include include="<armarx/control/common/control_law/aron/TaskspaceImpedanceControllerConfig.xml>" /> -->
+ <Include include="armarx/navigation/core/aron/TwistLimits.xml" />
+ <Include include="armarx/navigation/core/aron/PIDParams.xml" />
+ <Include include="armarx/navigation/core/aron/Trajectory.xml" />
+ <Include include="armarx/navigation/trajectory_control/global/aron/TrajectoryFollowingControllerParams.xml" />
</AronIncludes>
<CodeIncludes>
<Include include="<Eigen/Core>" />
<Include include="<armarx/navigation/core/aron/TwistLimits.aron.generated.h>" />
<Include include="<armarx/navigation/core/aron/PIDParams.aron.generated.h>" />
<Include include="<armarx/navigation/core/aron/Trajectory.aron.generated.h>" />
- <Include include="<armarx/navigation/trajectory_control/aron/TrajectoryFollowingControllerParams.aron.generated.h>" />
+ <Include include="<armarx/navigation/trajectory_control/global/aron/TrajectoryFollowingControllerParams.aron.generated.h>" />
</CodeIncludes>
<GenerateTypes>
@@ -31,7 +30,7 @@
<Object name='armarx::navigation::platform_controller::platform_global_trajectory::arondto::Config'>
<ObjectChild key='params'>
- <armarx::navigation::traj_ctrl::arondto::TrajectoryFollowingControllerParams />
+ <armarx::navigation::traj_ctrl::global::arondto::TrajectoryFollowingControllerParams />
</ObjectChild>
<ObjectChild key='targets'>
diff --git a/source/armarx/navigation/platform_controller/aron/PlatformLocalTrajectoryControllerConfig.xml b/source/armarx/navigation/platform_controller/aron/PlatformLocalTrajectoryControllerConfig.xml
index a1479eef..01899f98 100644
--- a/source/armarx/navigation/platform_controller/aron/PlatformLocalTrajectoryControllerConfig.xml
+++ b/source/armarx/navigation/platform_controller/aron/PlatformLocalTrajectoryControllerConfig.xml
@@ -1,18 +1,17 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<AronIncludes>
- <Include include="../../core/aron/TwistLimits.xml" />
- <Include include="../../core/aron/PIDParams.xml" />
- <Include include="../../core/aron/Trajectory.xml" />
- <Include include="../../trajectory_control/aron/TrajectoryFollowingControllerParams.xml" />
- <!-- <Include include="<armarx/control/common/control_law/aron/TaskspaceImpedanceControllerConfig.xml>" /> -->
+ <Include include="armarx/navigation/core/aron/TwistLimits.xml" />
+ <Include include="armarx/navigation/core/aron/PIDParams.xml" />
+ <Include include="armarx/navigation/core/aron/Trajectory.xml" />
+ <Include include="armarx/navigation/trajectory_control/local/aron/TrajectoryFollowingControllerParams.xml" />
</AronIncludes>
<CodeIncludes>
<Include include="<Eigen/Core>" />
<Include include="<armarx/navigation/core/aron/TwistLimits.aron.generated.h>" />
<Include include="<armarx/navigation/core/aron/PIDParams.aron.generated.h>" />
<Include include="<armarx/navigation/core/aron/Trajectory.aron.generated.h>" />
- <Include include="<armarx/navigation/trajectory_control/aron/TrajectoryFollowingControllerParams.aron.generated.h>" />
+ <Include include="<armarx/navigation/trajectory_control/local/aron/TrajectoryFollowingControllerParams.aron.generated.h>" />
</CodeIncludes>
<GenerateTypes>
@@ -31,7 +30,7 @@
<Object name='armarx::navigation::platform_controller::platform_local_trajectory::arondto::Config'>
<ObjectChild key='params'>
- <armarx::navigation::traj_ctrl::arondto::TrajectoryFollowingControllerParams />
+ <armarx::navigation::traj_ctrl::local::arondto::TrajectoryFollowingControllerParams />
</ObjectChild>
<ObjectChild key='targets'>
diff --git a/source/armarx/navigation/platform_controller/aron_conversions.cpp b/source/armarx/navigation/platform_controller/aron_conversions.cpp
index 8b502245..f3245428 100644
--- a/source/armarx/navigation/platform_controller/aron_conversions.cpp
+++ b/source/armarx/navigation/platform_controller/aron_conversions.cpp
@@ -26,9 +26,11 @@
#include <armarx/control/common/aron_conversions.h>
#include <armarx/navigation/core/aron_conversions.h>
#include <armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.h>
+#include <armarx/navigation/platform_controller/PlatformLocalTrajectoryController.h>
#include <armarx/navigation/platform_controller/aron/PlatformGlobalTrajectoryControllerConfig.aron.generated.h>
#include <armarx/navigation/platform_controller/aron/PlatformLocalTrajectoryControllerConfig.aron.generated.h>
-#include <armarx/navigation/trajectory_control/aron_conversions.h>
+#include <armarx/navigation/trajectory_control/local/aron_conversions.h>
+#include <armarx/navigation/trajectory_control/global/aron_conversions.h>
namespace armarx::navigation::platform_controller
@@ -52,22 +54,22 @@ namespace armarx::navigation::platform_controller
} // namespace platform_global_trajectory
- // namespace platform_local_trajectory
- // {
+ namespace platform_local_trajectory
+ {
- // void
- // fromAron(const arondto::Targets& dto, Targets& bo)
- // {
- // fromAron(dto.trajectory, bo.trajectory);
- // }
+ void
+ fromAron(const arondto::Targets& dto, Targets& bo)
+ {
+ fromAron(dto.trajectory, bo.trajectory);
+ }
- // void
- // fromAron(const arondto::Config& dto, Config& bo)
- // {
- // fromAron(dto.params, bo.params);
- // fromAron(dto.targets, bo.targets);
- // }
+ void
+ fromAron(const arondto::Config& dto, Config& bo)
+ {
+ fromAron(dto.params, bo.params);
+ fromAron(dto.targets, bo.targets);
+ }
- // } // namespace platform_local_trajectory
+ } // namespace platform_local_trajectory
} // namespace armarx::navigation::platform_controller
diff --git a/source/armarx/navigation/platform_controller/aron_conversions.h b/source/armarx/navigation/platform_controller/aron_conversions.h
index 1e338a7c..4e158dc1 100644
--- a/source/armarx/navigation/platform_controller/aron_conversions.h
+++ b/source/armarx/navigation/platform_controller/aron_conversions.h
@@ -22,14 +22,32 @@
#pragma once
-namespace armarx::navigation::platform_controller::platform_global_trajectory
+namespace armarx::navigation::platform_controller
{
- namespace arondto
+ namespace platform_global_trajectory
{
- class Config;
- }
+ namespace arondto
+ {
+ class Config;
+ }
- struct Config;
+ struct Config;
- void fromAron(const arondto::Config& dto, Config& bo);
-} // namespace armarx::navigation::platform_controller::platform_global_trajectory
+ void fromAron(const arondto::Config& dto, Config& bo);
+
+ } // namespace platform_global_trajectory
+
+ namespace platform_local_trajectory
+ {
+ namespace arondto
+ {
+ class Config;
+ }
+
+ struct Config;
+
+ void fromAron(const arondto::Config& dto, Config& bo);
+
+ } // namespace platform_local_trajectory
+
+} // namespace armarx::navigation::platform_controller
diff --git a/source/armarx/navigation/platform_controller/controller_descriptions.h b/source/armarx/navigation/platform_controller/controller_descriptions.h
index cb602115..30ce76d9 100644
--- a/source/armarx/navigation/platform_controller/controller_descriptions.h
+++ b/source/armarx/navigation/platform_controller/controller_descriptions.h
@@ -24,7 +24,7 @@
#include <armarx/control/client/ControllerDescription.h>
#include <armarx/navigation/common/controller_types.h>
#include <armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.h>
-// #include <armarx/navigation/platform_controller/PlatformLocalTrajectoryController.h>
+#include <armarx/navigation/platform_controller/PlatformLocalTrajectoryController.h>
#include <armarx/navigation/platform_controller/aron/PlatformGlobalTrajectoryControllerConfig.aron.generated.h>
#include <armarx/navigation/platform_controller/aron/PlatformLocalTrajectoryControllerConfig.aron.generated.h>
@@ -43,17 +43,17 @@ namespace armarx::control::client
armarx::navigation::common::PlatformGlobalTrajectoryControllerName;
};
- // template <>
- // struct ControllerDescription<armarx::navigation::common::ControllerType::PlatformLocalTrajectory>
- // {
- // using AronDTO =
- // armarx::navigation::platform_controller::platform_local_trajectory::arondto::Config;
+ template <>
+ struct ControllerDescription<armarx::navigation::common::ControllerType::PlatformLocalTrajectory>
+ {
+ using AronDTO =
+ armarx::navigation::platform_controller::platform_local_trajectory::arondto::Config;
- // using BO = armarx::navigation::platform_controller::platform_local_trajectory::Config;
+ using BO = armarx::navigation::platform_controller::platform_local_trajectory::Config;
- // // constexpr static const char* name = armarx::navigation::common::ControllerTypeNames.to_name(armarx::navigation::common::ControllerType::PlatformTrajectory);
- // constexpr static const char* name =
- // armarx::navigation::common::PlatformLocalTrajectoryControllerName;
- // };
+ // constexpr static const char* name = armarx::navigation::common::ControllerTypeNames.to_name(armarx::navigation::common::ControllerType::PlatformTrajectory);
+ constexpr static const char* name =
+ armarx::navigation::common::PlatformLocalTrajectoryControllerName;
+ };
} // namespace armarx::control::client
diff --git a/source/armarx/navigation/server/NavigationStack.h b/source/armarx/navigation/server/NavigationStack.h
index c2a76f9a..2bed9fdf 100644
--- a/source/armarx/navigation/server/NavigationStack.h
+++ b/source/armarx/navigation/server/NavigationStack.h
@@ -25,7 +25,8 @@
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/safety_control/SafetyController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/global/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
namespace armarx::navigation::server
{
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index 79744022..4a25256e 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -605,7 +605,7 @@ namespace armarx::navigation::server
// the following is similar to moveToAbsolute
// TODO(fabian.reister): remove code duplication
- srv.executor->execute(globalPlan->trajectory);
+ // srv.executor->execute(globalPlan->trajectory);
ARMARX_TRACE;
srv.publisher->globalTrajectoryUpdated(globalPlan.value());
@@ -700,7 +700,7 @@ namespace armarx::navigation::server
ARMARX_TRACE;
srv.publisher->globalTrajectoryUpdated(globalPlan.value());
srv.introspector->onGlobalPlannerResult(globalPlan.value());
- srv.executor->execute(globalPlan->trajectory);
+ // srv.executor->execute(globalPlan->trajectory);
ARMARX_INFO << "Global planning completed. Will now start all required threads";
ARMARX_TRACE;
diff --git a/source/armarx/navigation/server/Navigator.h b/source/armarx/navigation/server/Navigator.h
index a0affd5d..f7ef3948 100644
--- a/source/armarx/navigation/server/Navigator.h
+++ b/source/armarx/navigation/server/Navigator.h
@@ -47,7 +47,7 @@
#include <armarx/navigation/server/execution/ExecutorInterface.h>
#include <armarx/navigation/server/introspection/IntrospectorInterface.h>
#include <armarx/navigation/server/monitoring/GoalReachedMonitor.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
namespace armarx::navigation::server
{
diff --git a/source/armarx/navigation/server/StackResult.h b/source/armarx/navigation/server/StackResult.h
index f5299e64..64abee03 100644
--- a/source/armarx/navigation/server/StackResult.h
+++ b/source/armarx/navigation/server/StackResult.h
@@ -24,9 +24,11 @@
#include <optional>
+#include "armarx/navigation/local_planning/LocalPlanner.h"
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/global/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
namespace armarx::navigation::server
@@ -35,13 +37,12 @@ namespace armarx::navigation::server
struct StackResult
{
// TODO make struct, add timestamp
- using LocalPlannerResult = core::GlobalTrajectoryPtr;
using SafetyControllerResult = std::optional<core::Twist>;
global_planning::GlobalPlannerResult globalPlan;
- LocalPlannerResult localTrajectory;
- traj_ctrl::TrajectoryControllerResult controlVelocity;
- SafetyControllerResult safeVelocity;
+ loc_plan::LocalPlannerResult localTrajectory;
+ // traj_ctrl::global::TrajectoryControllerResult controlVelocity;
+ // SafetyControllerResult safeVelocity;
// core::TrajectoryPtr trajectory() const;
// core::Twist velocity() const;
diff --git a/source/armarx/navigation/server/event_publishing/EventPublishingInterface.h b/source/armarx/navigation/server/event_publishing/EventPublishingInterface.h
index 416d8cc6..775d77aa 100644
--- a/source/armarx/navigation/server/event_publishing/EventPublishingInterface.h
+++ b/source/armarx/navigation/server/event_publishing/EventPublishingInterface.h
@@ -4,7 +4,7 @@
#include <armarx/navigation/core/events.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
namespace armarx::navigation::server
@@ -20,8 +20,8 @@ namespace armarx::navigation::server
// TODO(fabian.reister): fwd
virtual void globalTrajectoryUpdated(const global_planning::GlobalPlannerResult& res) = 0;
virtual void localTrajectoryUpdated(const loc_plan::LocalPlannerResult& res) = 0;
- virtual void
- trajectoryControllerUpdated(const traj_ctrl::TrajectoryControllerResult& res) = 0;
+ // virtual void
+ // trajectoryControllerUpdated(const traj_ctrl::local::TrajectoryControllerResult& res) = 0;
virtual void globalPlanningFailed(const core::GlobalPlanningFailedEvent& event) = 0;
diff --git a/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp b/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp
index 6226169b..30bc7e6b 100644
--- a/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp
+++ b/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp
@@ -57,11 +57,12 @@ namespace armarx::navigation::server
// resultWriter->store(res, clientId);
}
- void
- MemoryPublisher::trajectoryControllerUpdated(const traj_ctrl::TrajectoryControllerResult& res)
- {
- resultWriter->store(res, clientId);
- }
+ // void
+ // MemoryPublisher::trajectoryControllerUpdated(
+ // const traj_ctrl::local::TrajectoryControllerResult& res)
+ // {
+ // resultWriter->store(res, clientId);
+ // }
// TODO(fabian.reister): event with message or reason (enum)
void
diff --git a/source/armarx/navigation/server/event_publishing/MemoryPublisher.h b/source/armarx/navigation/server/event_publishing/MemoryPublisher.h
index 57204aba..61db181e 100644
--- a/source/armarx/navigation/server/event_publishing/MemoryPublisher.h
+++ b/source/armarx/navigation/server/event_publishing/MemoryPublisher.h
@@ -18,7 +18,8 @@ namespace armarx::navigation::server
void globalTrajectoryUpdated(const global_planning::GlobalPlannerResult& res) override;
void localTrajectoryUpdated(const loc_plan::LocalPlannerResult& res) override;
- void trajectoryControllerUpdated(const traj_ctrl::TrajectoryControllerResult& res) override;
+ // void trajectoryControllerUpdated(
+ // const traj_ctrl::local::TrajectoryControllerResult& res) override;
void globalPlanningFailed(const core::GlobalPlanningFailedEvent& event) override;
@@ -36,7 +37,6 @@ namespace armarx::navigation::server
void internalError(const core::InternalErrorEvent& event) override;
// Non-API.
- public:
~MemoryPublisher() override = default;
private:
diff --git a/source/armarx/navigation/server/execution/ExecutorInterface.h b/source/armarx/navigation/server/execution/ExecutorInterface.h
index 57e35506..65426bf5 100644
--- a/source/armarx/navigation/server/execution/ExecutorInterface.h
+++ b/source/armarx/navigation/server/execution/ExecutorInterface.h
@@ -1,9 +1,6 @@
#pragma once
-namespace armarx::navigation::core
-{
- class GlobalTrajectory;
-} // namespace armarx::navigation::core
+#include "armarx/navigation/core/Trajectory.h"
namespace armarx::navigation::server
{
@@ -17,8 +14,8 @@ namespace armarx::navigation::server
public:
virtual ~ExecutorInterface() = default;
- virtual void execute(const core::GlobalTrajectory& trajectory) = 0;
-
+ virtual void execute(const core::LocalTrajectory& trajectory) = 0;
+
virtual void start() = 0;
virtual void stop() = 0;
};
diff --git a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
index 9d173d95..9ba41405 100644
--- a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
@@ -8,10 +8,11 @@
#include <armarx/control/memory/config/util.h>
#include <armarx/navigation/common/controller_types.h>
#include <armarx/navigation/core/aron_conversions.h>
-#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
+#include <armarx/navigation/platform_controller/aron/PlatformLocalTrajectoryControllerConfig.aron.generated.h>
#include <armarx/navigation/platform_controller/aron_conversions.h>
#include <armarx/navigation/platform_controller/controller_descriptions.h>
#include <armarx/navigation/platform_controller/json_conversions.h>
+#include <armarx/navigation/trajectory_control/local/aron_conversions.h>
namespace armarx::navigation::server
@@ -45,16 +46,16 @@ namespace armarx::navigation::server
{
ARMARX_TRACE;
auto builder = controllerPlugin.createControllerBuilder<
- armarx::navigation::common::ControllerType::PlatformGlobalTrajectory>();
+ armarx::navigation::common::ControllerType::PlatformLocalTrajectory>();
ARMARX_TRACE;
- const armarx::PackagePath configPath("armarx_navigation", "controller_config/PlatformTrajectory/default.json");
+ const armarx::PackagePath configPath(
+ "armarx_navigation", "controller_config/PlatformTrajectory/default.json");
- auto ctrlWrapper =
- builder.withNodeSet("PlatformPlanning")
- .withConfig(configPath.toSystemPath())
- .create();
+ auto ctrlWrapper = builder.withNodeSet("PlatformPlanning")
+ .withConfig(configPath.toSystemPath())
+ .create();
ARMARX_TRACE;
ARMARX_CHECK(ctrlWrapper.has_value());
@@ -70,9 +71,9 @@ namespace armarx::navigation::server
void
- PlatformControllerExecutor::execute(const core::GlobalTrajectory& trajectory)
+ PlatformControllerExecutor::execute(const core::LocalTrajectory& trajectory)
{
- ARMARX_INFO << "Received trajectory for execution with " << trajectory.points().size()
+ ARMARX_INFO << "Received trajectory for execution with " << trajectory.size()
<< " points";
toAron(ctrl->config.targets.trajectory, trajectory);
diff --git a/source/armarx/navigation/server/execution/PlatformControllerExecutor.h b/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
index f852ea09..2e5fe975 100644
--- a/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
@@ -31,14 +31,14 @@ namespace armarx::navigation::server
PlatformControllerExecutor(ControllerComponentPlugin& controllerComponentPlugin);
~PlatformControllerExecutor() override;
- void execute(const core::GlobalTrajectory& trajectory) override;
+ void execute(const core::LocalTrajectory& trajectory) override;
void start() override;
void stop() override;
private:
std::optional<armarx::control::client::ControllerWrapper<
- armarx::navigation::common::ControllerType::PlatformGlobalTrajectory>>
+ armarx::navigation::common::ControllerType::PlatformLocalTrajectory>>
ctrl;
ControllerComponentPlugin& controllerPlugin;
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
index 43f3e5fb..a57ba285 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
@@ -7,16 +7,19 @@
#include <Eigen/Geometry>
+#include <SimoxUtility/algorithm/apply.hpp>
#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/color/ColorMap.h>
#include <SimoxUtility/color/cmaps/colormaps.h>
#include <VirtualRobot/Robot.h>
+
#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/components/ArViz/Client/elements/Color.h>
#include <RobotAPI/components/ArViz/Client/elements/Path.h>
+#include "armarx/navigation/core/Trajectory.h"
#include "range/v3/algorithm/max.hpp"
#include "range/v3/algorithm/max_element.hpp"
#include <armarx/navigation/core/basic_types.h>
@@ -128,10 +131,11 @@ namespace armarx::navigation::server
viz::Path("path").points(trajectory.positions()).color(simox::color::Color::blue()));
const auto cmap = simox::color::cmaps::viridis();
-
- const float maxVelocity = ranges::max_element(trajectory.points(), [](const auto& a, const auto& b){
- return a.velocity < b.velocity;
- })->velocity;
+
+ const float maxVelocity = ranges::max_element(trajectory.points(),
+ [](const auto& a, const auto& b)
+ { return a.velocity < b.velocity; })
+ ->velocity;
for (const auto& [idx, tp] : trajectory.points() | ranges::views::enumerate)
@@ -151,11 +155,16 @@ namespace armarx::navigation::server
}
void
- ArvizIntrospector::drawLocalTrajectory(const core::GlobalTrajectory& trajectory)
+ ArvizIntrospector::drawLocalTrajectory(const core::LocalTrajectory& trajectory)
{
auto layer = arviz.layer("local_planner");
- layer.add(viz::Path("path").points(trajectory.positions()).color(simox::Color::blue()));
+ const std::vector<Eigen::Vector3f> points =
+ simox::alg::apply(trajectory,
+ [](const core::LocalTrajectoryPoint& pt) -> Eigen::Vector3f
+ { return pt.pose.translation(); });
+
+ layer.add(viz::Path("path").points(points).color(simox::Color::blue()));
layers.emplace_back(std::move(layer));
}
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.h b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
index c9a4719a..f27ea0be 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.h
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
@@ -34,13 +34,13 @@
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/safety_control/SafetyController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
#include <armarx/navigation/util/Visualization.h>
// forward declaration
namespace armarx::navigation::core
{
- struct GlobalTrajectory;
+ class GlobalTrajectory;
}
namespace armarx::navigation::server
@@ -57,8 +57,8 @@ namespace armarx::navigation::server
void onGoal(const core::Pose& goal) override;
- void success();
- void failure();
+ void success() override;
+ void failure() override;
void onGlobalShortestPath(const std::vector<core::Pose>& path) override;
@@ -71,7 +71,7 @@ namespace armarx::navigation::server
private:
void drawGlobalTrajectory(const core::GlobalTrajectory& trajectory);
- void drawLocalTrajectory(const core::GlobalTrajectory& trajectory);
+ void drawLocalTrajectory(const core::LocalTrajectory& trajectory);
void drawRawVelocity(const core::Twist& twist);
void drawSafeVelocity(const core::Twist& twist);
diff --git a/source/armarx/navigation/server/introspection/IntrospectorInterface.h b/source/armarx/navigation/server/introspection/IntrospectorInterface.h
index 18eaa36b..f92ed9e8 100644
--- a/source/armarx/navigation/server/introspection/IntrospectorInterface.h
+++ b/source/armarx/navigation/server/introspection/IntrospectorInterface.h
@@ -25,7 +25,7 @@
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/safety_control/SafetyController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
namespace armarx::navigation::server
{
diff --git a/source/armarx/navigation/statecharts/NavigationGroup/NavigateToLocation.cpp b/source/armarx/navigation/statecharts/NavigationGroup/NavigateToLocation.cpp
index 3cf8a00a..32448b96 100644
--- a/source/armarx/navigation/statecharts/NavigationGroup/NavigateToLocation.cpp
+++ b/source/armarx/navigation/statecharts/NavigationGroup/NavigateToLocation.cpp
@@ -36,8 +36,8 @@
#include <armarx/navigation/client/services/SimpleEventHandler.h>
#include <armarx/navigation/client/types.h>
#include <armarx/navigation/global_planning/AStar.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
//#include <ArmarXCore/core/time/TimeUtil.h>
//#include <ArmarXCore/observers/variant/DatafieldRef.h>
@@ -67,7 +67,7 @@ namespace armarx::navigation::statecharts::navigation_group
cfg.general(client::GeneralConfig{});
cfg.globalPlanner(armarx::navigation::global_planning::AStarParams{});
cfg.trajectoryController(
- armarx::navigation::traj_ctrl::TrajectoryFollowingControllerParams{});
+ armarx::navigation::traj_ctrl::local::TrajectoryFollowingControllerParams{});
// configure the `navigator` which provides a simplified and typed interface to the navigation server
client::IceNavigator iceNavigator(getNavigator());
diff --git a/source/armarx/navigation/trajectory_control/CMakeLists.txt b/source/armarx/navigation/trajectory_control/CMakeLists.txt
index 07c8a589..8057dd3a 100644
--- a/source/armarx/navigation/trajectory_control/CMakeLists.txt
+++ b/source/armarx/navigation/trajectory_control/CMakeLists.txt
@@ -1,34 +1,66 @@
-armarx_add_aron_library(trajectory_control_aron
+armarx_add_aron_library(trajectory_control_local_aron
ARON_FILES
- aron/TrajectoryControllerParams.xml
- aron/TrajectoryFollowingControllerParams.xml
- aron/WaypointControllerParams.xml
+ local/aron/TrajectoryControllerParams.xml
+ local/aron/TrajectoryFollowingControllerParams.xml
)
-armarx_add_library(trajectory_control
+armarx_add_aron_library(trajectory_control_global_aron
+ ARON_FILES
+ global/aron/TrajectoryControllerParams.xml
+ global/aron/TrajectoryFollowingControllerParams.xml
+ global/aron/WaypointControllerParams.xml
+)
+
+
+armarx_add_library(trajectory_control_local
SOURCES
- ./TrajectoryController.cpp
- ./TrajectoryFollowingController.cpp
- ./WaypointController.cpp
- ./aron_conversions.cpp
+ local/TrajectoryFollowingController.cpp
+ local/aron_conversions.cpp
HEADERS
- ./TrajectoryController.h
- ./TrajectoryFollowingController.h
- ./WaypointController.h
- ./aron_conversions.h
- ./core.h
+ local/TrajectoryController.h
+ local/TrajectoryFollowingController.h
+ local/aron_conversions.h
+ local/core.h
DEPENDENCIES
ArmarXCoreInterfaces
ArmarXCore
RobotAPICore
armarx_navigation::core
- armarx_navigation::trajectory_control_aron
+ armarx_navigation::trajectory_control_local_aron
+ OBJECT
)
-armarx_add_test(trajectory_control_test
- TEST_FILES
- test/trajectory_controlTest.cpp
+armarx_add_library(trajectory_control_global
+ SOURCES
+ global/aron_conversions.cpp
+ global/TrajectoryFollowingController.cpp
+ global/WaypointController.cpp
+ HEADERS
+ global/TrajectoryController.h
+ global/aron_conversions.h
+ global/TrajectoryFollowingController.h
+ global/WaypointController.h
+ global/core.h
DEPENDENCIES
+ ArmarXCoreInterfaces
ArmarXCore
- armarx_navigation::trajectory_control
+ RobotAPICore
+ armarx_navigation::core
+ armarx_navigation::trajectory_control_global_aron
+ OBJECT
+)
+
+armarx_add_library(trajectory_control
+ DEPENDENCIES
+ armarx_navigation::trajectory_control_local
+ armarx_navigation::trajectory_control_global
)
+
+
+# armarx_add_test(trajectory_control_test
+# TEST_FILES
+# test/trajectory_controlTest.cpp
+# DEPENDENCIES
+# ArmarXCore
+# armarx_navigation::trajectory_control
+# )
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryController.cpp
deleted file mode 100644
index be79e84e..00000000
--- a/source/armarx/navigation/trajectory_control/TrajectoryController.cpp
+++ /dev/null
@@ -1,9 +0,0 @@
-#include "TrajectoryController.h"
-
-namespace armarx::navigation::traj_ctrl
-{
- // TrajectoryController::TrajectoryController(const core::Scene& context) : context(context)
- // {
- // }
-
-} // namespace armarx::navigation::traj_ctrl
diff --git a/source/armarx/navigation/trajectory_control/global/TrajectoryController.h b/source/armarx/navigation/trajectory_control/global/TrajectoryController.h
new file mode 100644
index 00000000..ddd84342
--- /dev/null
+++ b/source/armarx/navigation/trajectory_control/global/TrajectoryController.h
@@ -0,0 +1,77 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author Christian R. G. Dreher ( c dot dreher at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <cmath>
+#include <limits>
+#include <memory>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+#include <armarx/navigation/core/DynamicScene.h>
+#include <armarx/navigation/core/StaticScene.h>
+#include <armarx/navigation/core/Trajectory.h>
+#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/trajectory_control/global/core.h>
+
+namespace armarx::navigation::traj_ctrl
+{
+ namespace global
+ {
+
+ struct TrajectoryControllerResult
+ {
+ core::Twist twist;
+ core::GlobalTrajectoryPoint dropPoint;
+ };
+
+ struct TrajectoryControllerParams
+ {
+ core::TwistLimits limits{
+ .linear = 500.F, // [mm/s]
+ .angular = 2.F * M_PIf32 / 10.F // [rad/s]
+ };
+
+ virtual ~TrajectoryControllerParams() = default;
+
+ virtual Algorithms algorithm() const = 0;
+ virtual aron::data::DictPtr toAron() const = 0;
+ };
+
+ class TrajectoryController
+ {
+ public:
+ TrajectoryController() = default;
+ virtual ~TrajectoryController() = default;
+
+ virtual TrajectoryControllerResult control(const core::GlobalTrajectory& goal) = 0;
+ };
+ using TrajectoryControllerPtr = std::shared_ptr<TrajectoryController>;
+
+ } // namespace global
+
+
+
+} // namespace armarx::navigation::traj_ctrl
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp b/source/armarx/navigation/trajectory_control/global/TrajectoryFollowingController.cpp
similarity index 93%
rename from source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
rename to source/armarx/navigation/trajectory_control/global/TrajectoryFollowingController.cpp
index 15282bf6..8a6db3f4 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/armarx/navigation/trajectory_control/global/TrajectoryFollowingController.cpp
@@ -14,12 +14,12 @@
#include "armarx/navigation/core/basic_types.h"
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/types.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
-#include <armarx/navigation/trajectory_control/aron/TrajectoryFollowingControllerParams.aron.generated.h>
-#include <armarx/navigation/trajectory_control/aron_conversions.h>
-#include <armarx/navigation/trajectory_control/core.h>
+#include <armarx/navigation/trajectory_control/global/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/global/aron/TrajectoryFollowingControllerParams.aron.generated.h>
+#include <armarx/navigation/trajectory_control/global/aron_conversions.h>
+#include <armarx/navigation/trajectory_control/global/core.h>
-namespace armarx::navigation::traj_ctrl
+namespace armarx::navigation::traj_ctrl::global
{
// TrajectoryFollowingControllerParams
@@ -35,7 +35,7 @@ namespace armarx::navigation::traj_ctrl
arondto::TrajectoryFollowingControllerParams dto;
TrajectoryFollowingControllerParams bo;
- armarx::navigation::traj_ctrl::toAron(dto, bo);
+ armarx::navigation::traj_ctrl::global::toAron(dto, bo);
return dto.toAron();
}
@@ -47,7 +47,7 @@ namespace armarx::navigation::traj_ctrl
dto.fromAron(dict);
TrajectoryFollowingControllerParams bo;
- armarx::navigation::traj_ctrl::fromAron(dto, bo);
+ armarx::navigation::traj_ctrl::global::fromAron(dto, bo);
return bo;
}
@@ -217,4 +217,4 @@ namespace armarx::navigation::traj_ctrl
.dropPoint = projectedPose.projection};
}
-} // namespace armarx::navigation::traj_ctrl
+} // namespace armarx::navigation::traj_ctrl::global
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h b/source/armarx/navigation/trajectory_control/global/TrajectoryFollowingController.h
similarity index 90%
rename from source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h
rename to source/armarx/navigation/trajectory_control/global/TrajectoryFollowingController.h
index cb7dc596..a8d416f6 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h
+++ b/source/armarx/navigation/trajectory_control/global/TrajectoryFollowingController.h
@@ -24,10 +24,10 @@
#include <RobotAPI/libraries/core/MultiDimPIDController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
-#include <armarx/navigation/trajectory_control/aron/TrajectoryFollowingControllerParams.aron.generated.h>
+#include <armarx/navigation/trajectory_control/global/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/global/aron/TrajectoryFollowingControllerParams.aron.generated.h>
-namespace armarx::navigation::traj_ctrl
+namespace armarx::navigation::traj_ctrl::global
{
// using arondto::TrajectoryFollowingControllerParams;
diff --git a/source/armarx/navigation/trajectory_control/WaypointController.cpp b/source/armarx/navigation/trajectory_control/global/WaypointController.cpp
similarity index 66%
rename from source/armarx/navigation/trajectory_control/WaypointController.cpp
rename to source/armarx/navigation/trajectory_control/global/WaypointController.cpp
index a23f6cd4..ae8f0884 100644
--- a/source/armarx/navigation/trajectory_control/WaypointController.cpp
+++ b/source/armarx/navigation/trajectory_control/global/WaypointController.cpp
@@ -1,11 +1,11 @@
#include "WaypointController.h"
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
-#include <armarx/navigation/trajectory_control/aron/WaypointControllerParams.aron.generated.h>
-#include <armarx/navigation/trajectory_control/aron_conversions.h>
-#include <armarx/navigation/trajectory_control/core.h>
+#include <armarx/navigation/trajectory_control/global/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/global/aron/WaypointControllerParams.aron.generated.h>
+#include <armarx/navigation/trajectory_control/global/aron_conversions.h>
+#include <armarx/navigation/trajectory_control/global/core.h>
-namespace armarx::navigation::traj_ctrl
+namespace armarx::navigation::traj_ctrl::global
{
// WaypointControllerParams
@@ -21,7 +21,7 @@ namespace armarx::navigation::traj_ctrl
arondto::WaypointControllerParams dto;
WaypointControllerParams bo;
- armarx::navigation::traj_ctrl::toAron(dto, bo);
+ armarx::navigation::traj_ctrl::global::toAron(dto, bo);
return dto.toAron();
}
@@ -33,7 +33,7 @@ namespace armarx::navigation::traj_ctrl
dto.fromAron(dict);
WaypointControllerParams bo;
- armarx::navigation::traj_ctrl::fromAron(dto, bo);
+ armarx::navigation::traj_ctrl::global::fromAron(dto, bo);
return bo;
}
diff --git a/source/armarx/navigation/trajectory_control/WaypointController.h b/source/armarx/navigation/trajectory_control/global/WaypointController.h
similarity index 90%
rename from source/armarx/navigation/trajectory_control/WaypointController.h
rename to source/armarx/navigation/trajectory_control/global/WaypointController.h
index 78726abe..c87fe9ad 100644
--- a/source/armarx/navigation/trajectory_control/WaypointController.h
+++ b/source/armarx/navigation/trajectory_control/global/WaypointController.h
@@ -22,9 +22,9 @@
#pragma once
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/global/TrajectoryController.h>
-namespace armarx::navigation::traj_ctrl
+namespace armarx::navigation::traj_ctrl::global
{
struct WaypointControllerParams : public TrajectoryControllerParams
@@ -50,4 +50,4 @@ namespace armarx::navigation::traj_ctrl
};
using WaypointControllerPtr = std::shared_ptr<WaypointController>;
-} // namespace armarx::navigation::traj_ctrl
+} // namespace armarx::navigation::traj_ctrl::global
diff --git a/source/armarx/navigation/trajectory_control/global/aron/TrajectoryControllerParams.xml b/source/armarx/navigation/trajectory_control/global/aron/TrajectoryControllerParams.xml
new file mode 100644
index 00000000..b8400d15
--- /dev/null
+++ b/source/armarx/navigation/trajectory_control/global/aron/TrajectoryControllerParams.xml
@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<armarx/navigation/core/aron/TwistLimits.aron.generated.h>" />
+ </CodeIncludes>
+ <AronIncludes>
+ <Include include="armarx/navigation/core/aron/TwistLimits.xml" />
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <Object name='armarx::navigation::traj_ctrl::global::arondto::TrajectoryControllerParams'>
+ <!-- <ObjectChild key='limits'>
+ <armarx::navigation::core::arondto::TwistLimits />
+ </ObjectChild> -->
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/navigation/trajectory_control/aron/TrajectoryFollowingControllerParams.xml b/source/armarx/navigation/trajectory_control/global/aron/TrajectoryFollowingControllerParams.xml
similarity index 77%
rename from source/armarx/navigation/trajectory_control/aron/TrajectoryFollowingControllerParams.xml
rename to source/armarx/navigation/trajectory_control/global/aron/TrajectoryFollowingControllerParams.xml
index 357cfb65..263c5b43 100644
--- a/source/armarx/navigation/trajectory_control/aron/TrajectoryFollowingControllerParams.xml
+++ b/source/armarx/navigation/trajectory_control/global/aron/TrajectoryFollowingControllerParams.xml
@@ -1,20 +1,20 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<CodeIncludes>
- <Include include="<armarx/navigation/trajectory_control/aron/TrajectoryControllerParams.aron.generated.h>" />
+ <Include include="<armarx/navigation/trajectory_control/global/aron/TrajectoryControllerParams.aron.generated.h>" />
<Include include="<armarx/navigation/core/aron/PIDParams.aron.generated.h>" />
<Include include="<armarx/navigation/core/aron/TwistLimits.aron.generated.h>" />
</CodeIncludes>
<AronIncludes>
<Include include="TrajectoryControllerParams.xml" />
- <Include include="../../core/aron/PIDParams.xml" />
- <Include include="../../core/aron/TwistLimits.xml" />
+ <Include include="armarx/navigation/core/aron/PIDParams.xml" />
+ <Include include="armarx/navigation/core/aron/TwistLimits.xml" />
</AronIncludes>
<GenerateTypes>
<!-- <Object name='armarx::navigation::traj_ctrl::arondto::TrajectoryFollowingControllerParams' extends="armarx::navigation::traj_ctrl::arondto::TrajectoryControllerParams"> -->
- <Object name='armarx::navigation::traj_ctrl::arondto::TrajectoryFollowingControllerParams'>
+ <Object name='armarx::navigation::traj_ctrl::global::arondto::TrajectoryFollowingControllerParams'>
<ObjectChild key='pidPos'>
<armarx::navigation::core::arondto::PIDParams />
</ObjectChild>
diff --git a/source/armarx/navigation/trajectory_control/aron/WaypointControllerParams.xml b/source/armarx/navigation/trajectory_control/global/aron/WaypointControllerParams.xml
similarity index 81%
rename from source/armarx/navigation/trajectory_control/aron/WaypointControllerParams.xml
rename to source/armarx/navigation/trajectory_control/global/aron/WaypointControllerParams.xml
index 9f1e482d..8fe8b856 100644
--- a/source/armarx/navigation/trajectory_control/aron/WaypointControllerParams.xml
+++ b/source/armarx/navigation/trajectory_control/global/aron/WaypointControllerParams.xml
@@ -1,7 +1,7 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<CodeIncludes>
- <Include include="<armarx/navigation/trajectory_control/aron/TrajectoryControllerParams.aron.generated.h>" />
+ <Include include="<armarx/navigation/trajectory_control/global/aron/TrajectoryControllerParams.aron.generated.h>" />
</CodeIncludes>
<AronIncludes>
<Include include="TrajectoryControllerParams.xml" />
@@ -10,7 +10,7 @@
<GenerateTypes>
<!-- <Object name='armarx::navigation::traj_ctrl::arondto::WaypointControllerParams' extends="armarx::navigation::traj_ctrl::arondto::TrajectoryControllerParams"> -->
- <Object name='armarx::navigation::traj_ctrl::arondto::WaypointControllerParams'>
+ <Object name='armarx::navigation::traj_ctrl::global::arondto::WaypointControllerParams'>
<ObjectChild key='includeStartPose'>
<bool />
</ObjectChild>
diff --git a/source/armarx/navigation/trajectory_control/aron_conversions.cpp b/source/armarx/navigation/trajectory_control/global/aron_conversions.cpp
similarity index 74%
rename from source/armarx/navigation/trajectory_control/aron_conversions.cpp
rename to source/armarx/navigation/trajectory_control/global/aron_conversions.cpp
index 2cc2d0b4..d32a6132 100644
--- a/source/armarx/navigation/trajectory_control/aron_conversions.cpp
+++ b/source/armarx/navigation/trajectory_control/global/aron_conversions.cpp
@@ -5,15 +5,15 @@
#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
#include <armarx/navigation/core/aron_conversions.h>
-#include <armarx/navigation/trajectory_control/TrajectoryController.h>
-#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
-#include <armarx/navigation/trajectory_control/WaypointController.h>
-#include <armarx/navigation/trajectory_control/aron/TrajectoryControllerParams.aron.generated.h>
-#include <armarx/navigation/trajectory_control/aron/TrajectoryFollowingControllerParams.aron.generated.h>
-#include <armarx/navigation/trajectory_control/aron/WaypointControllerParams.aron.generated.h>
+#include <armarx/navigation/trajectory_control/global/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/global/TrajectoryFollowingController.h>
+#include <armarx/navigation/trajectory_control/global/WaypointController.h>
+#include <armarx/navigation/trajectory_control/global/aron/TrajectoryControllerParams.aron.generated.h>
+#include <armarx/navigation/trajectory_control/global/aron/TrajectoryFollowingControllerParams.aron.generated.h>
+#include <armarx/navigation/trajectory_control/global/aron/WaypointControllerParams.aron.generated.h>
-namespace armarx::navigation::traj_ctrl
+namespace armarx::navigation::traj_ctrl::global
{
void
@@ -67,4 +67,4 @@ namespace armarx::navigation::traj_ctrl
}
-} // namespace armarx::navigation::traj_ctrl
+} // namespace armarx::navigation::traj_ctrl::global
diff --git a/source/armarx/navigation/trajectory_control/aron_conversions.h b/source/armarx/navigation/trajectory_control/global/aron_conversions.h
similarity index 93%
rename from source/armarx/navigation/trajectory_control/aron_conversions.h
rename to source/armarx/navigation/trajectory_control/global/aron_conversions.h
index f8496c98..3f0dbd61 100644
--- a/source/armarx/navigation/trajectory_control/aron_conversions.h
+++ b/source/armarx/navigation/trajectory_control/global/aron_conversions.h
@@ -22,9 +22,8 @@
#pragma once
-namespace armarx::navigation::traj_ctrl
+namespace armarx::navigation::traj_ctrl::global
{
- struct PIDParams;
struct TrajectoryControllerParams;
struct TrajectoryFollowingControllerParams;
@@ -32,7 +31,6 @@ namespace armarx::navigation::traj_ctrl
namespace arondto
{
- class PIDParams;
class TrajectoryControllerParams;
class TrajectoryFollowingControllerParams;
@@ -51,4 +49,4 @@ namespace armarx::navigation::traj_ctrl
void toAron(arondto::WaypointControllerParams& dto, const WaypointControllerParams& bo);
void fromAron(const arondto::WaypointControllerParams& dto, WaypointControllerParams& bo);
-} // namespace armarx::navigation::traj_ctrl
+} // namespace armarx::navigation::traj_ctrl::global
diff --git a/source/armarx/navigation/trajectory_control/core.h b/source/armarx/navigation/trajectory_control/global/core.h
similarity index 89%
rename from source/armarx/navigation/trajectory_control/core.h
rename to source/armarx/navigation/trajectory_control/global/core.h
index 0cb07330..2c2d4674 100644
--- a/source/armarx/navigation/trajectory_control/core.h
+++ b/source/armarx/navigation/trajectory_control/global/core.h
@@ -24,17 +24,17 @@
#include <SimoxUtility/meta/enum/EnumNames.hpp>
-namespace armarx::navigation::traj_ctrl
+namespace armarx::navigation::traj_ctrl::global
{
enum class Algorithms
{
WaypointController,
- TrajectoryFollowingController
+ TrajectoryFollowingController,
};
const inline simox::meta::EnumNames<Algorithms> AlgorithmNames{
{Algorithms::WaypointController, "WaypointController"},
{Algorithms::TrajectoryFollowingController, "TrajectoryFollowingController"}};
-} // namespace armarx::navigation::traj_ctrl
+} // namespace armarx::navigation::traj_ctrl::global
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryController.h b/source/armarx/navigation/trajectory_control/local/TrajectoryController.h
similarity index 89%
rename from source/armarx/navigation/trajectory_control/TrajectoryController.h
rename to source/armarx/navigation/trajectory_control/local/TrajectoryController.h
index 9ef9dbc2..4a5c3bbe 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryController.h
+++ b/source/armarx/navigation/trajectory_control/local/TrajectoryController.h
@@ -36,13 +36,12 @@
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/types.h>
-namespace armarx::navigation::traj_ctrl
+namespace armarx::navigation::traj_ctrl::local
{
struct TrajectoryControllerResult
{
core::Twist twist;
- core::GlobalTrajectoryPoint dropPoint;
};
struct TrajectoryControllerParams
@@ -64,9 +63,8 @@ namespace armarx::navigation::traj_ctrl
TrajectoryController() = default;
virtual ~TrajectoryController() = default;
- virtual TrajectoryControllerResult control(const core::GlobalTrajectory& goal) = 0;
-
+ virtual TrajectoryControllerResult control(const core::LocalTrajectory& goal) = 0;
};
using TrajectoryControllerPtr = std::shared_ptr<TrajectoryController>;
-} // namespace armarx::navigation::traj_ctrl
+} // namespace armarx::navigation::traj_ctrl::local
diff --git a/source/armarx/navigation/trajectory_control/local/TrajectoryFollowingController.cpp b/source/armarx/navigation/trajectory_control/local/TrajectoryFollowingController.cpp
new file mode 100644
index 00000000..df64d939
--- /dev/null
+++ b/source/armarx/navigation/trajectory_control/local/TrajectoryFollowingController.cpp
@@ -0,0 +1,197 @@
+#include "TrajectoryFollowingController.h"
+
+#include <algorithm>
+
+#include <SimoxUtility/math/convert/mat4f_to_pos.h>
+#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
+#include <SimoxUtility/math/convert/mat4f_to_xyyaw.h>
+#include <VirtualRobot/Robot.h>
+
+#include "ArmarXCore/core/time/Clock.h"
+#include "ArmarXCore/core/time/DateTime.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/interface/serialization/Eigen/Eigen_fdi.h>
+
+#include <RobotAPI/libraries/core/MultiDimPIDController.h>
+
+#include "armarx/navigation/core/basic_types.h"
+#include <armarx/navigation/core/Trajectory.h>
+#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/aron/TrajectoryFollowingControllerParams.aron.generated.h>
+#include <armarx/navigation/trajectory_control/local/aron_conversions.h>
+#include <armarx/navigation/trajectory_control/local/core.h>
+
+namespace armarx::navigation::traj_ctrl::local
+{
+ // TrajectoryFollowingControllerParams
+
+ Algorithms
+ TrajectoryFollowingControllerParams::algorithm() const
+ {
+ return Algorithms::TrajectoryFollowingController;
+ }
+
+ aron::data::DictPtr
+ TrajectoryFollowingControllerParams::toAron() const
+ {
+ arondto::TrajectoryFollowingControllerParams dto;
+
+ TrajectoryFollowingControllerParams bo;
+ armarx::navigation::traj_ctrl::local::toAron(dto, bo);
+
+ return dto.toAron();
+ }
+
+ TrajectoryFollowingControllerParams
+ TrajectoryFollowingControllerParams::FromAron(const aron::data::DictPtr& dict)
+ {
+ arondto::TrajectoryFollowingControllerParams dto;
+ dto.fromAron(dict);
+
+ TrajectoryFollowingControllerParams bo;
+ armarx::navigation::traj_ctrl::local::fromAron(dto, bo);
+
+ return bo;
+ }
+
+ // TrajectoryFollowingController
+
+ TrajectoryFollowingController::TrajectoryFollowingController(
+ const VirtualRobot::RobotPtr& robot,
+ const Params& params) :
+ robot(robot),
+ params(params),
+ pidPos(params.pidPos.Kp,
+ params.pidPos.Ki,
+ params.pidPos.Kd,
+ std::numeric_limits<double>::max(),
+ std::numeric_limits<double>::max(),
+ false,
+ std::vector<bool>{false, false, false}),
+ pidPosTarget(params.pidPos.Kp / 2,
+ params.pidPos.Ki,
+ params.pidPos.Kd,
+ std::numeric_limits<double>::max(),
+ std::numeric_limits<double>::max(),
+ false,
+ std::vector<bool>{false, false, false}),
+ pidOri(params.pidOri.Kp,
+ params.pidOri.Ki,
+ params.pidOri.Kd,
+ std::numeric_limits<double>::max(),
+ std::numeric_limits<double>::max(),
+ false,
+ std::vector<bool>{true, true, true}),
+ pidOriTarget(params.pidOri.Kp,
+ params.pidOri.Ki,
+ params.pidOri.Kd,
+ std::numeric_limits<double>::max(),
+ std::numeric_limits<double>::max(),
+ false,
+ std::vector<bool>{true, true, true})
+ {
+ ARMARX_IMPORTANT << "Trajectory following controller params: "
+ << VAROUT(params.limits.linear) << ", " << VAROUT(params.limits.angular);
+ }
+
+ core::Twist
+ TrajectoryFollowingController::applyTwistLimits(core::Twist twist)
+ {
+ const core::Twist limits{.linear = Eigen::Vector3f::Ones() * params.limits.linear,
+ .angular = Eigen::Vector3f::Ones() * params.limits.angular};
+
+ // for all entries, scale should be less than 1
+ const auto scalePos = twist.linear.cwiseAbs().cwiseQuotient(limits.linear.cwiseAbs());
+ const auto scaleOri = twist.angular.cwiseAbs().cwiseQuotient(limits.angular.cwiseAbs());
+
+ const float scaleMax = std::max(scalePos.maxCoeff(), scaleOri.maxCoeff());
+
+ if (scaleMax < 1.0F) // both linear and angular velocity in bounds?
+ {
+ return twist;
+ }
+
+ // scale such that no limit is violated
+ twist.linear /= scaleMax;
+ twist.angular /= scaleMax;
+
+ constexpr float eps = 0.001;
+
+ // pedantic checks
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.x()), params.limits.linear + eps);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.y()), params.limits.linear + eps);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.z()), params.limits.linear + eps);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.x()), params.limits.angular + eps);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.y()), params.limits.angular + eps);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.z()), params.limits.angular + eps);
+
+ return twist;
+ }
+
+ core::Pose
+ evaluateTrajectoryAt(const core::LocalTrajectory& trajectory, const DateTime& timestamp)
+ {
+ const auto cmp = [](const core::LocalTrajectoryPoint& lhs,
+ const DateTime& timestamp) -> bool
+ { return lhs.timestamp < timestamp; };
+
+ const auto lower = std::lower_bound(trajectory.begin(), trajectory.end(), timestamp, cmp);
+ // const auto upper = std::upper_bound(trajectory.begin(), trajectory.end(), timestamp, cmp);
+
+ const std::size_t pos = std::distance(trajectory.begin(), lower);
+
+ if (pos == 0)
+ {
+ return trajectory.front().pose;
+ }
+
+ return trajectory.at(pos - 1).pose;
+ }
+
+ TrajectoryControllerResult
+ TrajectoryFollowingController::control(const core::LocalTrajectory& trajectory)
+ {
+ ARMARX_CHECK_NOT_NULL(robot) << "Robot not available";
+
+ if (trajectory.empty())
+ {
+ ARMARX_INFO << "Trajectory is empty.";
+ return TrajectoryControllerResult{.twist = core::Twist::Zero()};
+ }
+
+ const core::Pose currentPose(robot->getGlobalPose());
+
+ // TOOD maybe set via arg?
+ const DateTime timestamp = Clock::Now();
+
+ const core::Pose targetPose = evaluateTrajectoryAt(trajectory, timestamp);
+
+ using simox::math::mat4f_to_pos;
+ using simox::math::mat4f_to_rpy;
+
+ pidPos.update(mat4f_to_pos(currentPose.matrix()), mat4f_to_pos(targetPose.matrix()));
+ pidOri.update(mat4f_to_rpy(currentPose.matrix()), mat4f_to_rpy(targetPose.matrix()));
+
+ const core::Twist twist{.linear = pidPos.getControlValue(),
+ .angular = pidOri.getControlValue()};
+
+ const auto twistLimited = applyTwistLimits(twist);
+ ARMARX_VERBOSE << deactivateSpam(1) << "Twist limited " << twistLimited.linear.transpose();
+ ARMARX_VERBOSE << deactivateSpam(1) << "Twist angular " << twistLimited.angular.transpose();
+
+ // convert to the robot's base frame
+ const core::Pose global_T_robot(robot->getGlobalPose());
+
+ const auto& twistGlobal = twistLimited;
+
+ core::Twist twistLocal;
+ twistLocal.linear = global_T_robot.linear().inverse() * twistGlobal.linear;
+ // TODO if not in 2D, then this must be changed!
+ twistLocal.angular = twistGlobal.angular;
+
+ return TrajectoryControllerResult{.twist = twistLocal};
+ }
+
+} // namespace armarx::navigation::traj_ctrl::local
diff --git a/source/armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h b/source/armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h
new file mode 100644
index 00000000..6484ebfe
--- /dev/null
+++ b/source/armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h
@@ -0,0 +1,71 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author Christian R. G. Dreher ( c dot dreher at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/core/MultiDimPIDController.h>
+
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/aron/TrajectoryFollowingControllerParams.aron.generated.h>
+
+namespace armarx::navigation::traj_ctrl::local
+{
+ // using arondto::LocalTrajectoryFollowingControllerParams;
+
+ struct TrajectoryFollowingControllerParams : public TrajectoryControllerParams
+ {
+
+ core::PIDParams pidPos;
+ core::PIDParams pidOri;
+
+ Algorithms algorithm() const override;
+ aron::data::DictPtr toAron() const override;
+
+ static TrajectoryFollowingControllerParams FromAron(const aron::data::DictPtr& dict);
+ };
+
+ class TrajectoryFollowingController : virtual public TrajectoryController
+ {
+ public:
+ using Params = TrajectoryFollowingControllerParams;
+
+ TrajectoryFollowingController(const VirtualRobot::RobotPtr& robot, const Params& params);
+ ~TrajectoryFollowingController() override = default;
+
+ TrajectoryControllerResult control(const core::LocalTrajectory& trajectory) override;
+
+ // protected:
+ core::Twist applyTwistLimits(core::Twist twist);
+
+ private:
+ const VirtualRobot::RobotPtr robot;
+ const Params params;
+
+
+ MultiDimPIDControllerTemplate<3> pidPos;
+ MultiDimPIDControllerTemplate<3> pidPosTarget;
+ MultiDimPIDControllerTemplate<3> pidOri;
+ MultiDimPIDControllerTemplate<3> pidOriTarget;
+ };
+ using LocalTrajectoryFollowingControllerPtr = std::shared_ptr<TrajectoryFollowingController>;
+
+} // namespace armarx::navigation::traj_ctrl::local
diff --git a/source/armarx/navigation/trajectory_control/aron/TrajectoryControllerParams.xml b/source/armarx/navigation/trajectory_control/local/aron/TrajectoryControllerParams.xml
similarity index 72%
rename from source/armarx/navigation/trajectory_control/aron/TrajectoryControllerParams.xml
rename to source/armarx/navigation/trajectory_control/local/aron/TrajectoryControllerParams.xml
index b40e68ba..85f4487f 100644
--- a/source/armarx/navigation/trajectory_control/aron/TrajectoryControllerParams.xml
+++ b/source/armarx/navigation/trajectory_control/local/aron/TrajectoryControllerParams.xml
@@ -4,12 +4,12 @@
<Include include="<armarx/navigation/core/aron/TwistLimits.aron.generated.h>" />
</CodeIncludes>
<AronIncludes>
- <Include include="../../core/aron/TwistLimits.xml" />
+ <Include include="armarx/navigation/core/aron/TwistLimits.xml" />
</AronIncludes>
<GenerateTypes>
- <Object name='armarx::navigation::traj_ctrl::arondto::TrajectoryControllerParams'>
+ <Object name='armarx::navigation::traj_ctrl::local::arondto::TrajectoryControllerParams'>
<!-- <ObjectChild key='limits'>
<armarx::navigation::core::arondto::TwistLimits />
</ObjectChild> -->
diff --git a/source/armarx/navigation/trajectory_control/local/aron/TrajectoryFollowingControllerParams.xml b/source/armarx/navigation/trajectory_control/local/aron/TrajectoryFollowingControllerParams.xml
new file mode 100644
index 00000000..db25d84b
--- /dev/null
+++ b/source/armarx/navigation/trajectory_control/local/aron/TrajectoryFollowingControllerParams.xml
@@ -0,0 +1,31 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<armarx/navigation/trajectory_control/local/aron/TrajectoryControllerParams.aron.generated.h>" />
+ <Include include="<armarx/navigation/core/aron/PIDParams.aron.generated.h>" />
+ <Include include="<armarx/navigation/core/aron/TwistLimits.aron.generated.h>" />
+ </CodeIncludes>
+ <AronIncludes>
+ <Include include="TrajectoryControllerParams.xml" />
+ <Include include="armarx/navigation/core/aron/PIDParams.xml" />
+ <Include include="armarx/navigation/core/aron/TwistLimits.xml" />
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <!-- <Object name='armarx::navigation::traj_ctrl::arondto::LocalTrajectoryFollowingControllerParams' extends="armarx::navigation::traj_ctrl::arondto::TrajectoryControllerParams"> -->
+ <Object name='armarx::navigation::traj_ctrl::local::arondto::TrajectoryFollowingControllerParams'>
+ <ObjectChild key='pidPos'>
+ <armarx::navigation::core::arondto::PIDParams />
+ </ObjectChild>
+ <ObjectChild key='pidOri'>
+ <armarx::navigation::core::arondto::PIDParams />
+ </ObjectChild>
+ <ObjectChild key='limits'>
+ <armarx::navigation::core::arondto::TwistLimits />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/navigation/trajectory_control/local/aron_conversions.cpp b/source/armarx/navigation/trajectory_control/local/aron_conversions.cpp
new file mode 100644
index 00000000..8cfc96a7
--- /dev/null
+++ b/source/armarx/navigation/trajectory_control/local/aron_conversions.cpp
@@ -0,0 +1,35 @@
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
+
+#include <armarx/navigation/core/aron_conversions.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
+#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
+
+
+namespace armarx::navigation::traj_ctrl::local
+{
+
+
+ void
+ toAron(arondto::TrajectoryFollowingControllerParams& dto,
+ const TrajectoryFollowingControllerParams& bo)
+ {
+ core::toAron(dto.pidOri, bo.pidOri);
+ core::toAron(dto.pidPos, bo.pidPos);
+ core::toAron(dto.limits, bo.limits);
+ }
+
+ void
+ fromAron(const arondto::TrajectoryFollowingControllerParams& dto,
+ TrajectoryFollowingControllerParams& bo)
+ {
+ core::fromAron(dto.pidOri, bo.pidOri);
+ core::fromAron(dto.pidPos, bo.pidPos);
+ core::fromAron(dto.limits, bo.limits);
+ }
+
+
+} // namespace armarx::navigation::traj_ctrl::local
diff --git a/source/armarx/navigation/trajectory_control/local/aron_conversions.h b/source/armarx/navigation/trajectory_control/local/aron_conversions.h
new file mode 100644
index 00000000..91b8f9bb
--- /dev/null
+++ b/source/armarx/navigation/trajectory_control/local/aron_conversions.h
@@ -0,0 +1,45 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author Christian R. G. Dreher ( c dot dreher at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+namespace armarx::navigation::traj_ctrl::local
+{
+ struct TrajectoryControllerParams;
+ struct TrajectoryFollowingControllerParams;
+
+ namespace arondto
+ {
+ class TrajectoryControllerParams;
+ class TrajectoryFollowingControllerParams;
+ } // namespace arondto
+
+
+ void toAron(arondto::TrajectoryControllerParams& dto, const TrajectoryControllerParams& bo);
+ void fromAron(const arondto::TrajectoryControllerParams& dto, TrajectoryControllerParams& bo);
+
+ void toAron(arondto::TrajectoryFollowingControllerParams& dto,
+ const TrajectoryFollowingControllerParams& bo);
+ void fromAron(const arondto::TrajectoryFollowingControllerParams& dto,
+ TrajectoryFollowingControllerParams& bo);
+
+} // namespace armarx::navigation::traj_ctrl::local
diff --git a/source/armarx/navigation/trajectory_control/local/core.h b/source/armarx/navigation/trajectory_control/local/core.h
new file mode 100644
index 00000000..3a3aa676
--- /dev/null
+++ b/source/armarx/navigation/trajectory_control/local/core.h
@@ -0,0 +1,36 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author Christian R. G. Dreher ( c dot dreher at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <SimoxUtility/meta/enum/EnumNames.hpp>
+
+namespace armarx::navigation::traj_ctrl::local
+{
+ enum class Algorithms
+ {
+ TrajectoryFollowingController
+ };
+
+ const inline simox::meta::EnumNames<Algorithms> AlgorithmNames{
+ {Algorithms::TrajectoryFollowingController, "TrajectoryFollowingController"}};
+} // namespace armarx::navigation::traj_ctrl::local
--
GitLab