From 19ef414f34f44a2cb1bf7a6dfa75eb66052fc2e4 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 25 Jul 2022 20:30:36 +0200
Subject: [PATCH 1/3] renaming trajectory to globaltrajectory; will add
trajectory for local planner soon
---
.../smoothing/CircularPathSmoothing.cpp | 8 +--
.../smoothing/CircularPathSmoothing.h | 2 +-
.../NavigationMemory/NavigationMemory.cpp | 12 ++--
source/armarx/navigation/core/Graph.h | 2 +-
source/armarx/navigation/core/Trajectory.cpp | 68 +++++++++----------
source/armarx/navigation/core/Trajectory.h | 40 +++++++----
.../navigation/core/aron/Trajectory.xml | 25 ++++++-
.../navigation/core/aron_conversions.cpp | 20 +++---
.../armarx/navigation/core/aron_conversions.h | 8 +--
.../navigation/global_planning/AStar.cpp | 4 +-
.../global_planning/GlobalPlanner.h | 2 +-
.../global_planning/Point2Point.cpp | 12 ++--
.../navigation/global_planning/SPFA.cpp | 4 +-
.../optimization/OrientationOptimizer.cpp | 8 +--
.../optimization/OrientationOptimizer.h | 6 +-
.../navigation/local_planning/LocalPlanner.h | 4 +-
.../local_planning/TimedElasticBands.cpp | 2 +-
.../local_planning/TimedElasticBands.h | 4 +-
.../memory/client/stack_result/Writer.cpp | 2 +-
.../PlatformTrajectoryController.h | 2 +-
.../PlatformTrajectoryControllerConfig.xml | 2 +-
source/armarx/navigation/server/Navigator.cpp | 12 ++--
source/armarx/navigation/server/StackResult.h | 2 +-
.../server/execution/ExecutorInterface.h | 4 +-
.../execution/PlatformControllerExecutor.cpp | 2 +-
.../execution/PlatformControllerExecutor.h | 3 +-
.../introspection/ArvizIntrospector.cpp | 4 +-
.../server/introspection/ArvizIntrospector.h | 6 +-
.../trajectory_control/TrajectoryController.h | 4 +-
.../TrajectoryFollowingController.cpp | 2 +-
.../TrajectoryFollowingController.h | 2 +-
.../trajectory_control/WaypointController.cpp | 2 +-
.../trajectory_control/WaypointController.h | 2 +-
.../armarx/navigation/util/Visualization.cpp | 4 +-
source/armarx/navigation/util/Visualization.h | 4 +-
35 files changed, 159 insertions(+), 131 deletions(-)
diff --git a/source/armarx/navigation/algorithms/smoothing/CircularPathSmoothing.cpp b/source/armarx/navigation/algorithms/smoothing/CircularPathSmoothing.cpp
index 083dcfcc..3da33720 100644
--- a/source/armarx/navigation/algorithms/smoothing/CircularPathSmoothing.cpp
+++ b/source/armarx/navigation/algorithms/smoothing/CircularPathSmoothing.cpp
@@ -68,10 +68,10 @@ namespace armarx::navigation::algorithm
return points;
}
- core::Trajectory
- CircularPathSmoothing::smooth(const core::Trajectory& trajectory)
+ core::GlobalTrajectory
+ CircularPathSmoothing::smooth(const core::GlobalTrajectory& trajectory)
{
- core::TrajectoryPoints points;
+ core::GlobalTrajectoryPoints points;
const auto& p = trajectory.points();
points.push_back(p.front());
@@ -107,7 +107,7 @@ namespace armarx::navigation::algorithm
const Eigen::Vector2f prePos = at + Eigen::Vector2f(prev - at).normalized() * l;
const Eigen::Vector2f postPos = at + Eigen::Vector2f(next - at).normalized() * l;
- const auto tpProj = [&](const Eigen::Vector2f& pos) -> core::TrajectoryPoint
+ const auto tpProj = [&](const Eigen::Vector2f& pos) -> core::GlobalTrajectoryPoint
{
const auto proj =
trajectory.getProjection(navigation::conv::to3D(pos),
diff --git a/source/armarx/navigation/algorithms/smoothing/CircularPathSmoothing.h b/source/armarx/navigation/algorithms/smoothing/CircularPathSmoothing.h
index 4e4d45bf..a36cf05d 100644
--- a/source/armarx/navigation/algorithms/smoothing/CircularPathSmoothing.h
+++ b/source/armarx/navigation/algorithms/smoothing/CircularPathSmoothing.h
@@ -41,7 +41,7 @@ namespace armarx::navigation::algorithm
/// circular path smoothing
Points smooth(const Points& p);
- core::Trajectory smooth(const core::Trajectory& trajectory);
+ core::GlobalTrajectory smooth(const core::GlobalTrajectory& trajectory);
};
} // namespace armarx::navigation::algorithm
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
index c3bec2c6..f35c33fa 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
@@ -108,13 +108,13 @@ namespace armarx::navigation
algorithms::arondto::Costmap::ToAronType());
workingMemory().addCoreSegment("Results_GlobalPlanner",
- navigation::core::arondto::Trajectory::ToAronType());
+ navigation::core::arondto::GlobalTrajectory::ToAronType());
workingMemory().addCoreSegment("Results_LocalPlanner",
- navigation::core::arondto::Trajectory::ToAronType());
- workingMemory().addCoreSegment("Results_TrajectoryController",
- navigation::core::arondto::Twist::ToAronType());
- workingMemory().addCoreSegment("Results_SafetyController",
- navigation::core::arondto::Twist::ToAronType());
+ navigation::core::arondto::LocalTrajectory::ToAronType());
+ // workingMemory().addCoreSegment("Results_TrajectoryController",
+ // navigation::core::arondto::Twist::ToAronType());
+ // workingMemory().addCoreSegment("Results_SafetyController",
+ // navigation::core::arondto::Twist::ToAronType());
workingMemory().addCoreSegment("Events"); //, armem::example::ExampleData::ToAronType());
// workingMemory.addCoreSegment("Exceptions"); //, armem::example::ExampleData::ToAronType());
diff --git a/source/armarx/navigation/core/Graph.h b/source/armarx/navigation/core/Graph.h
index 904f51c6..92097dff 100644
--- a/source/armarx/navigation/core/Graph.h
+++ b/source/armarx/navigation/core/Graph.h
@@ -73,7 +73,7 @@ namespace armarx::navigation::core
return m_value;
}
- std::optional<core::Trajectory> trajectory = std::nullopt;
+ std::optional<core::GlobalTrajectory> trajectory = std::nullopt;
float m_value{0.F};
};
diff --git a/source/armarx/navigation/core/Trajectory.cpp b/source/armarx/navigation/core/Trajectory.cpp
index a5bbc1ea..5b200574 100644
--- a/source/armarx/navigation/core/Trajectory.cpp
+++ b/source/armarx/navigation/core/Trajectory.cpp
@@ -136,16 +136,16 @@ namespace armarx::navigation::core
namespace conv
{
- TrajectoryPoint
+ GlobalTrajectoryPoint
toTrajectoryPoint(const Pose& pose, const float velocity)
{
- return TrajectoryPoint{.waypoint = {.pose = pose}, .velocity = velocity};
+ return GlobalTrajectoryPoint{.waypoint = {.pose = pose}, .velocity = velocity};
}
- TrajectoryPoints
+ GlobalTrajectoryPoints
toTrajectoryPoints(const Path& path, const float velocity)
{
- TrajectoryPoints trajectoryPoints;
+ GlobalTrajectoryPoints trajectoryPoints;
trajectoryPoints.reserve(path.size());
std::transform(path.begin(),
@@ -159,7 +159,7 @@ namespace armarx::navigation::core
} // namespace conv
Projection
- Trajectory::getProjection(const Position& point,
+ GlobalTrajectory::getProjection(const Position& point,
const VelocityInterpolation& velocityInterpolation) const
{
float distance = std::numeric_limits<float>::max();
@@ -237,9 +237,9 @@ namespace armarx::navigation::core
}
std::vector<Eigen::Vector3f>
- Trajectory::positions() const noexcept
+ GlobalTrajectory::positions() const noexcept
{
- const auto toPosition = [](const TrajectoryPoint& wp)
+ const auto toPosition = [](const GlobalTrajectoryPoint& wp)
{ return wp.waypoint.pose.translation(); };
std::vector<Eigen::Vector3f> positions;
@@ -251,7 +251,7 @@ namespace armarx::navigation::core
}
std::vector<Pose>
- Trajectory::poses() const noexcept
+ GlobalTrajectory::poses() const noexcept
{
std::vector<Pose> poses;
poses.reserve(pts.size());
@@ -259,20 +259,20 @@ namespace armarx::navigation::core
std::transform(pts.begin(),
pts.end(),
std::back_inserter(poses),
- [](const core::TrajectoryPoint& pt) -> core::Pose
+ [](const core::GlobalTrajectoryPoint& pt) -> core::Pose
{ return pt.waypoint.pose; });
return poses;
}
- Trajectory
- Trajectory::FromPath(const Path& path, const float velocity)
+ GlobalTrajectory
+ GlobalTrajectory::FromPath(const Path& path, const float velocity)
{
return {conv::toTrajectoryPoints(path, velocity)};
}
- Trajectory
- Trajectory::FromPath(const Pose& start,
+ GlobalTrajectory
+ GlobalTrajectory::FromPath(const Pose& start,
const Positions& waypoints,
const Pose& goal,
const float velocity)
@@ -331,7 +331,7 @@ namespace armarx::navigation::core
{
core::Positions resampledPath;
- const auto toPoint = [](const TrajectoryPoint& wp) -> Pose { return wp.waypoint.pose; };
+ const auto toPoint = [](const GlobalTrajectoryPoint& wp) -> Pose { return wp.waypoint.pose; };
const core::Path originalPath = pts | ranges::views::transform(toPoint) | ranges::to_vector;
@@ -424,8 +424,8 @@ namespace armarx::navigation::core
return resampledPath;
}
- Trajectory
- Trajectory::resample(const float eps) const
+ GlobalTrajectory
+ GlobalTrajectory::resample(const float eps) const
{
ARMARX_CHECK_GREATER_EQUAL(pts.size(), 2);
@@ -438,14 +438,14 @@ namespace armarx::navigation::core
ARMARX_DEBUG << "The resampled path contains no points. This means that it is likely "
"very short.";
- const core::Trajectory testTrajectory = core::Trajectory::FromPath(
+ const core::GlobalTrajectory testTrajectory = core::GlobalTrajectory::FromPath(
pts.front().waypoint.pose, resampledPathForward, pts.back().waypoint.pose, 0.F);
ARMARX_CHECK_LESS_EQUAL(testTrajectory.length(), eps)
<< "The resampled trajectory is only allowed to contains no points if it is "
"shorter than eps";
- return Trajectory({pts.front(), pts.back()});
+ return GlobalTrajectory({pts.front(), pts.back()});
}
@@ -488,12 +488,12 @@ namespace armarx::navigation::core
ranges::to_vector;
- auto resampledTrajectory = Trajectory::FromPath(
+ auto resampledTrajectory = GlobalTrajectory::FromPath(
pts.front().waypoint.pose, resampledPath, pts.back().waypoint.pose, 0.F);
// set velocity based on original trajectory
{
- const auto setVelocityInPlace = [&](TrajectoryPoint& pt)
+ const auto setVelocityInPlace = [&](GlobalTrajectoryPoint& pt)
{
const auto projection = getProjection(pt.waypoint.pose.translation(),
VelocityInterpolation::LinearInterpolation);
@@ -520,7 +520,7 @@ namespace armarx::navigation::core
}
float
- Trajectory::length() const
+ GlobalTrajectory::length() const
{
namespace r = ::ranges;
@@ -541,18 +541,18 @@ namespace armarx::navigation::core
}
void
- Trajectory::setMaxVelocity(float maxVelocity)
+ GlobalTrajectory::setMaxVelocity(float maxVelocity)
{
ARMARX_CHECK_GREATER_EQUAL(maxVelocity, 0.F) << "maxVelocity must be positive!";
std::for_each(pts.begin(),
pts.end(),
- [&maxVelocity](TrajectoryPoint& pt)
+ [&maxVelocity](GlobalTrajectoryPoint& pt)
{ pt.velocity = std::clamp(pt.velocity, -maxVelocity, maxVelocity); });
}
bool
- Trajectory::hasMaxDistanceBetweenWaypoints(const float maxDistance) const
+ GlobalTrajectory::hasMaxDistanceBetweenWaypoints(const float maxDistance) const
{
namespace r = ::ranges;
@@ -569,21 +569,21 @@ namespace armarx::navigation::core
rng, [&maxDistance](const auto& dist) -> bool { return dist > maxDistance; });
}
- const std::vector<TrajectoryPoint>&
- Trajectory::points() const
+ const std::vector<GlobalTrajectoryPoint>&
+ GlobalTrajectory::points() const
{
return pts;
}
- std::vector<TrajectoryPoint>&
- Trajectory::mutablePoints()
+ std::vector<GlobalTrajectoryPoint>&
+ GlobalTrajectory::mutablePoints()
{
return pts;
}
float
- Trajectory::duration(const core::VelocityInterpolation interpolation) const
+ GlobalTrajectory::duration(const core::VelocityInterpolation interpolation) const
{
float dur = 0;
@@ -618,16 +618,16 @@ namespace armarx::navigation::core
}
bool
- Trajectory::isValid() const noexcept
+ GlobalTrajectory::isValid() const noexcept
{
- const auto isValid = [](const TrajectoryPoint& pt) -> bool
+ const auto isValid = [](const GlobalTrajectoryPoint& pt) -> bool
{ return std::isfinite(pt.velocity) and pt.velocity < 3000; };
return std::all_of(pts.begin(), pts.end(), isValid);
}
- Trajectory::Indices
- Trajectory::allConnectedPointsInRange(std::size_t idx, float radius) const
+ GlobalTrajectory::Indices
+ GlobalTrajectory::allConnectedPointsInRange(std::size_t idx, float radius) const
{
const core::Position referencePosition = pts.at(idx).waypoint.pose.translation();
@@ -639,7 +639,7 @@ namespace armarx::navigation::core
return posDiff <= radius;
};
- Trajectory::Indices indices;
+ GlobalTrajectory::Indices indices;
// traverse from query point to start
diff --git a/source/armarx/navigation/core/Trajectory.h b/source/armarx/navigation/core/Trajectory.h
index fd5f7e4f..245f5dc9 100644
--- a/source/armarx/navigation/core/Trajectory.h
+++ b/source/armarx/navigation/core/Trajectory.h
@@ -24,26 +24,27 @@
#include <cstddef>
#include <memory>
+#include "ArmarXCore/core/time/DateTime.h"
#include <armarx/navigation/core/basic_types.h>
namespace armarx::navigation::core
{
- struct TrajectoryPoint
+ struct GlobalTrajectoryPoint
{
Waypoint waypoint;
float velocity; // [mm/s]
};
- using TrajectoryPoints = std::vector<TrajectoryPoint>;
+ using GlobalTrajectoryPoints = std::vector<GlobalTrajectoryPoint>;
struct Projection
{
- TrajectoryPoint projection;
+ GlobalTrajectoryPoint projection;
- TrajectoryPoint wayPointBefore;
- TrajectoryPoint wayPointAfter;
+ GlobalTrajectoryPoint wayPointBefore;
+ GlobalTrajectoryPoint wayPointAfter;
enum class Segment
{
@@ -61,14 +62,14 @@ namespace armarx::navigation::core
LastWaypoint
};
- using TrajectoryPtr = std::shared_ptr<class Trajectory>;
+ using GlobalTrajectoryPtr = std::shared_ptr<class GlobalTrajectory>;
- class Trajectory
+ class GlobalTrajectory
{
public:
- Trajectory() = default;
+ GlobalTrajectory() = default;
- Trajectory(const std::vector<TrajectoryPoint>& points) : pts(points)
+ GlobalTrajectory(const std::vector<GlobalTrajectoryPoint>& points) : pts(points)
{
}
@@ -82,15 +83,15 @@ namespace armarx::navigation::core
//! Note: the velocity will not be set!
// currently, only 2D version
// y is pointing forward
- static Trajectory FromPath(const Path& path, float velocity);
+ static GlobalTrajectory FromPath(const Path& path, float velocity);
//! Note: the velocity will not be set!
// currently, only 2D version
// y is pointing forward
- static Trajectory
+ static GlobalTrajectory
FromPath(const Pose& start, const Positions& waypoints, const Pose& goal, float velocity);
- Trajectory resample(float eps) const;
+ GlobalTrajectory resample(float eps) const;
float length() const;
@@ -98,9 +99,9 @@ namespace armarx::navigation::core
bool hasMaxDistanceBetweenWaypoints(float maxDistance) const;
- const std::vector<TrajectoryPoint>& points() const;
+ const std::vector<GlobalTrajectoryPoint>& points() const;
- std::vector<TrajectoryPoint>& mutablePoints();
+ std::vector<GlobalTrajectoryPoint>& mutablePoints();
float duration(core::VelocityInterpolation interpolation) const;
@@ -118,7 +119,16 @@ namespace armarx::navigation::core
private:
- std::vector<TrajectoryPoint> pts;
+ std::vector<GlobalTrajectoryPoint> pts;
};
+ struct LocalTrajectoryPoint
+ {
+ core::Pose pose;
+ DateTime timestamp;
+ };
+
+ using LocalTrajectory = std::vector<LocalTrajectoryPoint>;
+
+
} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/aron/Trajectory.xml b/source/armarx/navigation/core/aron/Trajectory.xml
index 7e3c96c5..b0a19a9c 100644
--- a/source/armarx/navigation/core/aron/Trajectory.xml
+++ b/source/armarx/navigation/core/aron/Trajectory.xml
@@ -8,7 +8,8 @@
</AronIncludes>
<GenerateTypes>
- <Object name='armarx::navigation::core::arondto::TrajectoryPoint'>
+ <!-- Global trajectory -->
+ <Object name='armarx::navigation::core::arondto::GlobalTrajectoryPoint'>
<ObjectChild key='velocity'>
<float />
</ObjectChild>
@@ -17,10 +18,28 @@
</ObjectChild>
</Object>
- <Object name='armarx::navigation::core::arondto::Trajectory'>
+ <Object name='armarx::navigation::core::arondto::GlobalTrajectory'>
<ObjectChild key='points'>
<List>
- <armarx::navigation::core::arondto::TrajectoryPoint />
+ <armarx::navigation::core::arondto::GlobalTrajectoryPoint />
+ </List>
+ </ObjectChild>
+ </Object>
+
+ <!-- Local trajectory -->
+ <Object name='armarx::navigation::core::arondto::LocalTrajectoryPoint'>
+ <ObjectChild key='pose'>
+ <Pose />
+ </ObjectChild>
+ <ObjectChild key='timestamp'>
+ <Time />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::core::arondto::LocalTrajectory'>
+ <ObjectChild key='points'>
+ <List>
+ <armarx::navigation::core::arondto::LocalTrajectoryPoint />
</List>
</ObjectChild>
</Object>
diff --git a/source/armarx/navigation/core/aron_conversions.cpp b/source/armarx/navigation/core/aron_conversions.cpp
index 7b93cdc2..aae32397 100644
--- a/source/armarx/navigation/core/aron_conversions.cpp
+++ b/source/armarx/navigation/core/aron_conversions.cpp
@@ -16,27 +16,27 @@ namespace armarx::navigation::core
void
- toAron(arondto::TrajectoryPoint& dto, const TrajectoryPoint& bo)
+ toAron(arondto::GlobalTrajectoryPoint& dto, const GlobalTrajectoryPoint& bo)
{
dto.pose = bo.waypoint.pose.matrix();
dto.velocity = bo.velocity;
}
void
- fromAron(const arondto::TrajectoryPoint& dto, TrajectoryPoint& bo)
+ fromAron(const arondto::GlobalTrajectoryPoint& dto, GlobalTrajectoryPoint& bo)
{
bo.waypoint.pose = core::Pose(dto.pose);
bo.velocity = dto.velocity;
}
void
- toAron(arondto::Trajectory& dto, const Trajectory& bo)
+ toAron(arondto::GlobalTrajectory& dto, const GlobalTrajectory& bo)
{
dto.points = bo.points() |
ranges::views::transform(
- [](const TrajectoryPoint& boTp) -> arondto::TrajectoryPoint
+ [](const GlobalTrajectoryPoint& boTp) -> arondto::GlobalTrajectoryPoint
{
- arondto::TrajectoryPoint dtoTp;
+ arondto::GlobalTrajectoryPoint dtoTp;
toAron(dtoTp, boTp);
return dtoTp;
}) |
@@ -44,15 +44,15 @@ namespace armarx::navigation::core
}
void
- fromAron(const arondto::Trajectory& dto, Trajectory& bo)
+ fromAron(const arondto::GlobalTrajectory& dto, GlobalTrajectory& bo)
{
- bo = Trajectory(
+ bo = GlobalTrajectory(
dto.points |
ranges::views::
- transform( //static_cast<TrajectoryPoint (*)(const arondto::TrajectoryPoint&)>(&fromAron)
- [](const arondto::TrajectoryPoint& dto) -> TrajectoryPoint
+ transform( //static_cast<TrajectoryPoint (*)(const arondto::GlobalTrajectoryPoint&)>(&fromAron)
+ [](const arondto::GlobalTrajectoryPoint& dto) -> GlobalTrajectoryPoint
{
- TrajectoryPoint bo;
+ GlobalTrajectoryPoint bo;
fromAron(dto, bo);
return bo;
}) |
diff --git a/source/armarx/navigation/core/aron_conversions.h b/source/armarx/navigation/core/aron_conversions.h
index 50f6cb44..0863383f 100644
--- a/source/armarx/navigation/core/aron_conversions.h
+++ b/source/armarx/navigation/core/aron_conversions.h
@@ -41,11 +41,11 @@ namespace armarx::navigation::core
return dto;
}
- void toAron(arondto::TrajectoryPoint& dto, const TrajectoryPoint& bo);
- void fromAron(const arondto::TrajectoryPoint& dto, TrajectoryPoint& bo);
+ void toAron(arondto::GlobalTrajectoryPoint& dto, const GlobalTrajectoryPoint& bo);
+ void fromAron(const arondto::GlobalTrajectoryPoint& dto, GlobalTrajectoryPoint& bo);
- void toAron(arondto::Trajectory& dto, const Trajectory& bo);
- void fromAron(const arondto::Trajectory& dto, Trajectory& bo);
+ void toAron(arondto::GlobalTrajectory& dto, const GlobalTrajectory& bo);
+ void fromAron(const arondto::GlobalTrajectory& dto, GlobalTrajectory& bo);
void toAron(arondto::Twist& dto, const Twist& bo);
void fromAron(const arondto::Twist& dto, Twist& bo);
diff --git a/source/armarx/navigation/global_planning/AStar.cpp b/source/armarx/navigation/global_planning/AStar.cpp
index 3dcd307c..eafc77da 100644
--- a/source/armarx/navigation/global_planning/AStar.cpp
+++ b/source/armarx/navigation/global_planning/AStar.cpp
@@ -236,12 +236,12 @@ namespace armarx::navigation::global_planning
ARMARX_TRACE;
auto trajectory =
- core::Trajectory::FromPath(start, conv::to3D(smoothPlan), goal, params.linearVelocity);
+ core::GlobalTrajectory::FromPath(start, conv::to3D(smoothPlan), goal, params.linearVelocity);
// TODO(fabian.reister): resampling of trajectory
// FIXME param
- std::optional<core::Trajectory> resampledTrajectory;
+ std::optional<core::GlobalTrajectory> resampledTrajectory;
if (params.resampleDistance < 0)
{
diff --git a/source/armarx/navigation/global_planning/GlobalPlanner.h b/source/armarx/navigation/global_planning/GlobalPlanner.h
index 1a028db1..6af2c754 100644
--- a/source/armarx/navigation/global_planning/GlobalPlanner.h
+++ b/source/armarx/navigation/global_planning/GlobalPlanner.h
@@ -37,7 +37,7 @@ namespace armarx::navigation::global_planning
struct GlobalPlannerResult
{
- core::Trajectory trajectory;
+ core::GlobalTrajectory trajectory;
};
diff --git a/source/armarx/navigation/global_planning/Point2Point.cpp b/source/armarx/navigation/global_planning/Point2Point.cpp
index e6714c9a..aa84b2d5 100644
--- a/source/armarx/navigation/global_planning/Point2Point.cpp
+++ b/source/armarx/navigation/global_planning/Point2Point.cpp
@@ -60,16 +60,16 @@ namespace armarx::navigation::global_planning
std::optional<GlobalPlannerResult>
Point2Point::plan(const core::Pose& goal)
{
- std::vector<core::TrajectoryPoint> trajectory;
+ std::vector<core::GlobalTrajectoryPoint> trajectory;
if (params.includeStartPose)
{
- trajectory.push_back(core::TrajectoryPoint{
+ trajectory.push_back(core::GlobalTrajectoryPoint{
.waypoint = core::Waypoint{.pose = core::Pose(scene.robot->getGlobalPose())},
.velocity = params.velocity});
}
- trajectory.push_back(core::TrajectoryPoint{.waypoint = core::Waypoint{.pose = goal},
+ trajectory.push_back(core::GlobalTrajectoryPoint{.waypoint = core::Waypoint{.pose = goal},
.velocity = params.velocity});
return GlobalPlannerResult{.trajectory = trajectory};
@@ -78,15 +78,15 @@ namespace armarx::navigation::global_planning
std::optional<GlobalPlannerResult>
Point2Point::plan(const core::Pose& start, const core::Pose& goal)
{
- std::vector<core::TrajectoryPoint> trajectory;
+ std::vector<core::GlobalTrajectoryPoint> trajectory;
if (params.includeStartPose)
{
- trajectory.push_back(core::TrajectoryPoint{.waypoint = core::Waypoint{.pose = start},
+ trajectory.push_back(core::GlobalTrajectoryPoint{.waypoint = core::Waypoint{.pose = start},
.velocity = params.velocity});
}
- trajectory.push_back(core::TrajectoryPoint{.waypoint = core::Waypoint{.pose = goal},
+ trajectory.push_back(core::GlobalTrajectoryPoint{.waypoint = core::Waypoint{.pose = goal},
.velocity = params.velocity});
return GlobalPlannerResult{.trajectory = trajectory};
diff --git a/source/armarx/navigation/global_planning/SPFA.cpp b/source/armarx/navigation/global_planning/SPFA.cpp
index e5c1c7cd..ed53c513 100644
--- a/source/armarx/navigation/global_planning/SPFA.cpp
+++ b/source/armarx/navigation/global_planning/SPFA.cpp
@@ -155,12 +155,12 @@ namespace armarx::navigation::global_planning
// smoothPlan.pop_back();
ARMARX_TRACE;
- auto trajectory = core::Trajectory::FromPath(start, wpts, goal, params.linearVelocity);
+ auto trajectory = core::GlobalTrajectory::FromPath(start, wpts, goal, params.linearVelocity);
// TODO(fabian.reister): resampling of trajectory
// FIXME param
- std::optional<core::Trajectory> resampledTrajectory;
+ std::optional<core::GlobalTrajectory> resampledTrajectory;
try
{
diff --git a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
index 2a7b5712..4b2182bf 100644
--- a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
+++ b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
@@ -31,7 +31,7 @@
namespace armarx::navigation::global_planning::optimization
{
- OrientationOptimizer::OrientationOptimizer(const core::Trajectory& trajectory,
+ OrientationOptimizer::OrientationOptimizer(const core::GlobalTrajectory& trajectory,
const Params& params) :
trajectory(trajectory), params(params)
{
@@ -45,7 +45,7 @@ namespace armarx::navigation::global_planning::optimization
namespace r = ::ranges;
namespace rv = ::ranges::views;
- const auto toYaw = [](const core::TrajectoryPoint& pt) -> double
+ const auto toYaw = [](const core::GlobalTrajectoryPoint& pt) -> double
{ return simox::math::mat4f_to_xyyaw(pt.waypoint.pose.matrix()).z(); };
std::vector<double> orientations =
@@ -218,9 +218,9 @@ namespace armarx::navigation::global_planning::optimization
// TODO write to file
}
- const auto applyOrientation = [](const auto& p) -> core::TrajectoryPoint
+ const auto applyOrientation = [](const auto& p) -> core::GlobalTrajectoryPoint
{
- core::TrajectoryPoint tp = p.first;
+ core::GlobalTrajectoryPoint tp = p.first;
const float yaw = p.second;
tp.waypoint.pose.linear() = simox::math::rpy_to_mat3f(0.F, 0.F, yaw);
diff --git a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.h b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.h
index 62ef5548..46434a42 100644
--- a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.h
+++ b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.h
@@ -45,12 +45,12 @@ namespace armarx::navigation::global_planning::optimization
public:
using Params = OrientationOptimizerParams;
- OrientationOptimizer(const core::Trajectory& trajectory, const Params& params);
+ OrientationOptimizer(const core::GlobalTrajectory& trajectory, const Params& params);
struct OptimizationResult
{
- std::optional<core::Trajectory> trajectory;
+ std::optional<core::GlobalTrajectory> trajectory;
operator bool() const noexcept
{
@@ -63,7 +63,7 @@ namespace armarx::navigation::global_planning::optimization
protected:
private:
- core::Trajectory trajectory;
+ core::GlobalTrajectory trajectory;
const Params params;
};
diff --git a/source/armarx/navigation/local_planning/LocalPlanner.h b/source/armarx/navigation/local_planning/LocalPlanner.h
index 5e7d06bf..46aa7ede 100644
--- a/source/armarx/navigation/local_planning/LocalPlanner.h
+++ b/source/armarx/navigation/local_planning/LocalPlanner.h
@@ -38,7 +38,7 @@ namespace armarx::navigation::loc_plan
{
struct LocalPlannerResult
{
- core::Trajectory trajectory;
+ core::GlobalTrajectory trajectory;
};
struct LocalPlannerParams
@@ -55,7 +55,7 @@ namespace armarx::navigation::loc_plan
LocalPlanner(const core::Scene& context);
virtual ~LocalPlanner() = default;
- virtual std::optional<LocalPlannerResult> plan(const core::Trajectory& goal) = 0;
+ virtual std::optional<LocalPlannerResult> plan(const core::GlobalTrajectory& goal) = 0;
protected:
private:
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index b069cba6..3965d109 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -34,7 +34,7 @@ namespace armarx::navigation::loc_plan
}
std::optional<LocalPlannerResult>
- TimedElasticBands::plan(const core::Trajectory& goal)
+ TimedElasticBands::plan(const core::GlobalTrajectory& goal)
{
// TODO implement
return {};
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.h b/source/armarx/navigation/local_planning/TimedElasticBands.h
index 8b965d15..6d9a90ae 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.h
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.h
@@ -50,9 +50,9 @@ namespace armarx::navigation::loc_plan
TimedElasticBands(const Params& params, const core::Scene& ctx);
~TimedElasticBands() override = default;
- void init(const core::Trajectory& initialTrajectory);
+ void init(const core::GlobalTrajectory& initialTrajectory);
- std::optional<LocalPlannerResult> plan(const core::Trajectory& goal) override;
+ std::optional<LocalPlannerResult> plan(const core::GlobalTrajectory& goal) override;
protected:
const Params params;
diff --git a/source/armarx/navigation/memory/client/stack_result/Writer.cpp b/source/armarx/navigation/memory/client/stack_result/Writer.cpp
index 759b42f6..e693495e 100644
--- a/source/armarx/navigation/memory/client/stack_result/Writer.cpp
+++ b/source/armarx/navigation/memory/client/stack_result/Writer.cpp
@@ -72,7 +72,7 @@ namespace armarx::navigation::memory::client::stack_result
.withTimestamp(timestamp);
- core::arondto::Trajectory aronDto;
+ core::arondto::GlobalTrajectory aronDto;
// FIXME create Aron type
core::toAron(aronDto, result.trajectory);
diff --git a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h
index 2b47dc98..daee089b 100644
--- a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h
+++ b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h
@@ -44,7 +44,7 @@ namespace armarx::navigation::platform_controller::platform_trajectory
{
struct Targets
{
- core::Trajectory trajectory;
+ core::GlobalTrajectory trajectory;
};
struct Config
diff --git a/source/armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.xml b/source/armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.xml
index a6551b53..3ca40737 100644
--- a/source/armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.xml
+++ b/source/armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.xml
@@ -24,7 +24,7 @@
<Object name='armarx::navigation::platform_controller::platform_trajectory::arondto::Targets'>
<ObjectChild key='trajectory'>
- <armarx::navigation::core::arondto::Trajectory />
+ <armarx::navigation::core::arondto::GlobalTrajectory />
</ObjectChild>
</Object>
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index e43dfa69..79744022 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -284,7 +284,7 @@ namespace armarx::navigation::server
return shortestPath;
}
- core::Trajectory
+ core::GlobalTrajectory
convertToTrajectory(const GraphPath& shortestPath, const core::Graph& graph)
{
ARMARX_TRACE;
@@ -294,7 +294,7 @@ namespace armarx::navigation::server
// ARMARX_CHECK_EQUAL(shortestPath.size() - 1, shortestPath.edges.size());
- std::vector<core::TrajectoryPoint> trajectoryPoints;
+ std::vector<core::GlobalTrajectoryPoint> trajectoryPoints;
// TODO add the start
// trajectoryPoints.push_back(core::TrajectoryPoint{
@@ -337,7 +337,7 @@ namespace armarx::navigation::server
// we have a trajectory
// FIXME trajectory points can be invalid => more points than expected. Why?
ARMARX_TRACE;
- const std::vector<core::TrajectoryPoint> edgeTrajectoryPoints =
+ const std::vector<core::GlobalTrajectoryPoint> edgeTrajectoryPoints =
edge.attrib().trajectory->points();
// if trajectory is being initialized, include the start, otherwise skip it
@@ -376,11 +376,11 @@ namespace armarx::navigation::server
// FIXME variable
const float point2pointVelocity = 400;
- const core::TrajectoryPoint currentTrajPt = {
+ const core::GlobalTrajectoryPoint currentTrajPt = {
.waypoint = {.pose = graph.vertex(shortestPath.at(i)).attrib().getPose()},
.velocity = point2pointVelocity};
- const core::TrajectoryPoint nextTrajPt{
+ const core::GlobalTrajectoryPoint nextTrajPt{
.waypoint = {.pose =
graph.vertex(shortestPath.at(i + 1)).attrib().getPose()},
.velocity = 0};
@@ -591,7 +591,7 @@ namespace armarx::navigation::server
// convert graph / vertex chain to trajectory
ARMARX_TRACE;
- core::Trajectory globalPlanTrajectory = convertToTrajectory(shortestPath, graph);
+ core::GlobalTrajectory globalPlanTrajectory = convertToTrajectory(shortestPath, graph);
// globalPlanTrajectory.setMaxVelocity(1000); // FIXME
diff --git a/source/armarx/navigation/server/StackResult.h b/source/armarx/navigation/server/StackResult.h
index c7f84cd3..f5299e64 100644
--- a/source/armarx/navigation/server/StackResult.h
+++ b/source/armarx/navigation/server/StackResult.h
@@ -35,7 +35,7 @@ namespace armarx::navigation::server
struct StackResult
{
// TODO make struct, add timestamp
- using LocalPlannerResult = core::TrajectoryPtr;
+ using LocalPlannerResult = core::GlobalTrajectoryPtr;
using SafetyControllerResult = std::optional<core::Twist>;
global_planning::GlobalPlannerResult globalPlan;
diff --git a/source/armarx/navigation/server/execution/ExecutorInterface.h b/source/armarx/navigation/server/execution/ExecutorInterface.h
index ea77fd7a..57e35506 100644
--- a/source/armarx/navigation/server/execution/ExecutorInterface.h
+++ b/source/armarx/navigation/server/execution/ExecutorInterface.h
@@ -2,7 +2,7 @@
namespace armarx::navigation::core
{
- class Trajectory;
+ class GlobalTrajectory;
} // namespace armarx::navigation::core
namespace armarx::navigation::server
@@ -17,7 +17,7 @@ namespace armarx::navigation::server
public:
virtual ~ExecutorInterface() = default;
- virtual void execute(const core::Trajectory& trajectory) = 0;
+ virtual void execute(const core::GlobalTrajectory& 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 5f089038..d2bb7a47 100644
--- a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
@@ -70,7 +70,7 @@ namespace armarx::navigation::server
void
- PlatformControllerExecutor::execute(const core::Trajectory& trajectory)
+ PlatformControllerExecutor::execute(const core::GlobalTrajectory& trajectory)
{
ARMARX_INFO << "Received trajectory for execution with " << trajectory.points().size()
<< " points";
diff --git a/source/armarx/navigation/server/execution/PlatformControllerExecutor.h b/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
index d256904c..721d1b62 100644
--- a/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
@@ -5,7 +5,6 @@
#include <armarx/control/client/ComponentPlugin.h>
#include <armarx/navigation/common/controller_types.h>
#include <armarx/navigation/core/types.h>
-#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
#include <armarx/navigation/platform_controller/controller_descriptions.h>
#include <armarx/navigation/server/execution/ExecutorInterface.h>
@@ -32,7 +31,7 @@ namespace armarx::navigation::server
PlatformControllerExecutor(ControllerComponentPlugin& controllerComponentPlugin);
~PlatformControllerExecutor() override;
- void execute(const core::Trajectory& trajectory) override;
+ void execute(const core::GlobalTrajectory& trajectory) override;
void start() override;
void stop() override;
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
index 0f107c99..43f3e5fb 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
@@ -120,7 +120,7 @@ namespace armarx::navigation::server
// private methods
void
- ArvizIntrospector::drawGlobalTrajectory(const core::Trajectory& trajectory)
+ ArvizIntrospector::drawGlobalTrajectory(const core::GlobalTrajectory& trajectory)
{
auto layer = arviz.layer("global_planner");
@@ -151,7 +151,7 @@ namespace armarx::navigation::server
}
void
- ArvizIntrospector::drawLocalTrajectory(const core::Trajectory& trajectory)
+ ArvizIntrospector::drawLocalTrajectory(const core::GlobalTrajectory& trajectory)
{
auto layer = arviz.layer("local_planner");
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.h b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
index a3daddc4..c9a4719a 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.h
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
@@ -40,7 +40,7 @@
// forward declaration
namespace armarx::navigation::core
{
- struct Trajectory;
+ struct GlobalTrajectory;
}
namespace armarx::navigation::server
@@ -70,8 +70,8 @@ namespace armarx::navigation::server
ArvizIntrospector& operator=(ArvizIntrospector&&) noexcept;
private:
- void drawGlobalTrajectory(const core::Trajectory& trajectory);
- void drawLocalTrajectory(const core::Trajectory& trajectory);
+ void drawGlobalTrajectory(const core::GlobalTrajectory& trajectory);
+ void drawLocalTrajectory(const core::GlobalTrajectory& trajectory);
void drawRawVelocity(const core::Twist& twist);
void drawSafeVelocity(const core::Twist& twist);
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryController.h b/source/armarx/navigation/trajectory_control/TrajectoryController.h
index 72d5927d..9ef9dbc2 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryController.h
+++ b/source/armarx/navigation/trajectory_control/TrajectoryController.h
@@ -42,7 +42,7 @@ namespace armarx::navigation::traj_ctrl
struct TrajectoryControllerResult
{
core::Twist twist;
- core::TrajectoryPoint dropPoint;
+ core::GlobalTrajectoryPoint dropPoint;
};
struct TrajectoryControllerParams
@@ -64,7 +64,7 @@ namespace armarx::navigation::traj_ctrl
TrajectoryController() = default;
virtual ~TrajectoryController() = default;
- virtual TrajectoryControllerResult control(const core::Trajectory& goal) = 0;
+ virtual TrajectoryControllerResult control(const core::GlobalTrajectory& goal) = 0;
};
using TrajectoryControllerPtr = std::shared_ptr<TrajectoryController>;
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
index b7fcdb2d..15282bf6 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
@@ -127,7 +127,7 @@ namespace armarx::navigation::traj_ctrl
}
TrajectoryControllerResult
- TrajectoryFollowingController::control(const core::Trajectory& trajectory)
+ TrajectoryFollowingController::control(const core::GlobalTrajectory& trajectory)
{
ARMARX_CHECK_NOT_NULL(robot) << "Robot not available";
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h
index ffb81732..cb7dc596 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h
+++ b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h
@@ -51,7 +51,7 @@ namespace armarx::navigation::traj_ctrl
TrajectoryFollowingController(const VirtualRobot::RobotPtr& robot, const Params& params);
~TrajectoryFollowingController() override = default;
- TrajectoryControllerResult control(const core::Trajectory& trajectory) override;
+ TrajectoryControllerResult control(const core::GlobalTrajectory& trajectory) override;
// protected:
core::Twist applyTwistLimits(core::Twist twist);
diff --git a/source/armarx/navigation/trajectory_control/WaypointController.cpp b/source/armarx/navigation/trajectory_control/WaypointController.cpp
index 527bd4f3..a23f6cd4 100644
--- a/source/armarx/navigation/trajectory_control/WaypointController.cpp
+++ b/source/armarx/navigation/trajectory_control/WaypointController.cpp
@@ -46,7 +46,7 @@ namespace armarx::navigation::traj_ctrl
}
TrajectoryControllerResult
- WaypointController::control(const core::Trajectory& goal)
+ WaypointController::control(const core::GlobalTrajectory& goal)
{
return {}; // TODO implement
}
diff --git a/source/armarx/navigation/trajectory_control/WaypointController.h b/source/armarx/navigation/trajectory_control/WaypointController.h
index ec5d4372..78726abe 100644
--- a/source/armarx/navigation/trajectory_control/WaypointController.h
+++ b/source/armarx/navigation/trajectory_control/WaypointController.h
@@ -42,7 +42,7 @@ namespace armarx::navigation::traj_ctrl
WaypointController(const Params& params);
~WaypointController() override = default;
- TrajectoryControllerResult control(const core::Trajectory& goal) override;
+ TrajectoryControllerResult control(const core::GlobalTrajectory& goal) override;
protected:
private:
diff --git a/source/armarx/navigation/util/Visualization.cpp b/source/armarx/navigation/util/Visualization.cpp
index 31ffe3ec..fb91bf23 100644
--- a/source/armarx/navigation/util/Visualization.cpp
+++ b/source/armarx/navigation/util/Visualization.cpp
@@ -23,7 +23,7 @@ namespace armarx::navigation::util
}
void
- Visualization::visualize(const core::Trajectory& trajectory)
+ Visualization::visualize(const core::GlobalTrajectory& trajectory)
{
// TODO(fabian.reister): more finegrained locking
std::lock_guard g{paramsMutex};
@@ -60,7 +60,7 @@ namespace armarx::navigation::util
void
- Visualization::visualizeTrajectory(const core::Trajectory& trajectory)
+ Visualization::visualizeTrajectory(const core::GlobalTrajectory& trajectory)
{
if (!params.enableTrajectoryVisualization)
{
diff --git a/source/armarx/navigation/util/Visualization.h b/source/armarx/navigation/util/Visualization.h
index 10db349a..6f249d7e 100644
--- a/source/armarx/navigation/util/Visualization.h
+++ b/source/armarx/navigation/util/Visualization.h
@@ -44,7 +44,7 @@ namespace armarx::navigation::util
void failed();
- void visualize(const core::Trajectory& trajectory);
+ void visualize(const core::GlobalTrajectory& trajectory);
void
updateParams(const Params& params)
@@ -54,7 +54,7 @@ namespace armarx::navigation::util
}
private:
- void visualizeTrajectory(const core::Trajectory& trajectory);
+ void visualizeTrajectory(const core::GlobalTrajectory& trajectory);
void asyncHideDelayed();
--
GitLab
From 6b5d9741de4a6146a4c7af497df0f3b7cff5f754 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 25 Jul 2022 21:08:41 +0200
Subject: [PATCH 2/3] distinguishing local and global trajectory type
---
.../navigation/common/controller_types.h | 10 ++--
.../navigation/core/aron_conversions.cpp | 47 +++++++++++++++-
.../armarx/navigation/core/aron_conversions.h | 6 +++
.../platform_controller/CMakeLists.txt | 12 ++---
...=> PlatformGlobalTrajectoryController.cpp} | 14 ++---
...h => PlatformGlobalTrajectoryController.h} | 24 +--------
...tformGlobalTrajectoryControllerConfig.xml} | 8 +--
...latformLocalTrajectoryControllerConfig.xml | 43 +++++++++++++++
.../platform_controller/aron_conversions.cpp | 53 +++++++++++++------
.../platform_controller/aron_conversions.h | 4 +-
.../controller_descriptions.h | 27 +++++++---
.../execution/PlatformControllerExecutor.cpp | 2 +-
.../execution/PlatformControllerExecutor.h | 2 +-
13 files changed, 184 insertions(+), 68 deletions(-)
rename source/armarx/navigation/platform_controller/{PlatformTrajectoryController.cpp => PlatformGlobalTrajectoryController.cpp} (94%)
rename source/armarx/navigation/platform_controller/{PlatformTrajectoryController.h => PlatformGlobalTrajectoryController.h} (82%)
rename source/armarx/navigation/platform_controller/aron/{PlatformTrajectoryControllerConfig.xml => PlatformGlobalTrajectoryControllerConfig.xml} (91%)
create mode 100644 source/armarx/navigation/platform_controller/aron/PlatformLocalTrajectoryControllerConfig.xml
diff --git a/source/armarx/navigation/common/controller_types.h b/source/armarx/navigation/common/controller_types.h
index 905ea6de..444fca1e 100644
--- a/source/armarx/navigation/common/controller_types.h
+++ b/source/armarx/navigation/common/controller_types.h
@@ -7,12 +7,16 @@ namespace armarx::navigation::common
enum class ControllerType
{
- PlatformTrajectory
+ PlatformGlobalTrajectory,
+ PlatformLocalTrajectory
};
- constexpr const char* PlatformTrajectoryControllerName= "PlatformTrajectory";
+ constexpr const char* PlatformGlobalTrajectoryControllerName = "PlatformGlobalTrajectory";
+ constexpr const char* PlatformLocalTrajectoryControllerName = "PlatformLocalTrajectory";
inline const simox::meta::EnumNames<ControllerType> ControllerTypeNames{
- {ControllerType::PlatformTrajectory, PlatformTrajectoryControllerName}};
+ {ControllerType::PlatformGlobalTrajectory, PlatformGlobalTrajectoryControllerName},
+ {ControllerType::PlatformLocalTrajectory, PlatformLocalTrajectoryControllerName}};
+
} // namespace armarx::navigation::common
diff --git a/source/armarx/navigation/core/aron_conversions.cpp b/source/armarx/navigation/core/aron_conversions.cpp
index aae32397..fae8aa97 100644
--- a/source/armarx/navigation/core/aron_conversions.cpp
+++ b/source/armarx/navigation/core/aron_conversions.cpp
@@ -3,8 +3,8 @@
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/transform.hpp>
-#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
#include <RobotAPI/libraries/core/Trajectory.h>
#include <armarx/navigation/core/Trajectory.h>
@@ -59,6 +59,51 @@ namespace armarx::navigation::core
ranges::to_vector);
}
+
+ void
+ toAron(arondto::LocalTrajectoryPoint& dto, const LocalTrajectoryPoint& bo)
+ {
+ dto.pose = bo.pose.matrix();
+ dto.timestamp = bo.timestamp;
+ }
+
+ void
+ fromAron(const arondto::LocalTrajectoryPoint& dto, LocalTrajectoryPoint& bo)
+ {
+ bo.pose = core::Pose(dto.pose);
+ bo.timestamp = dto.timestamp;
+ }
+
+ void
+ toAron(arondto::LocalTrajectory& dto, const LocalTrajectory& bo)
+ {
+ dto.points = bo |
+ ranges::views::transform(
+ [](const LocalTrajectoryPoint& boTp) -> arondto::LocalTrajectoryPoint
+ {
+ arondto::LocalTrajectoryPoint dtoTp;
+ toAron(dtoTp, boTp);
+ return dtoTp;
+ }) |
+ ranges::to_vector;
+ }
+
+ void
+ fromAron(const arondto::LocalTrajectory& dto, LocalTrajectory& bo)
+ {
+ bo =
+ dto.points |
+ ranges::views::
+ transform( //static_cast<TrajectoryPoint (*)(const arondto::GlobalTrajectoryPoint&)>(&fromAron)
+ [](const arondto::LocalTrajectoryPoint& dto) -> LocalTrajectoryPoint
+ {
+ LocalTrajectoryPoint bo;
+ fromAron(dto, bo);
+ return bo;
+ }) |
+ ranges::to_vector;
+ }
+
void
toAron(arondto::Twist& dto, const Twist& bo)
{
diff --git a/source/armarx/navigation/core/aron_conversions.h b/source/armarx/navigation/core/aron_conversions.h
index 0863383f..2fd54753 100644
--- a/source/armarx/navigation/core/aron_conversions.h
+++ b/source/armarx/navigation/core/aron_conversions.h
@@ -47,6 +47,12 @@ namespace armarx::navigation::core
void toAron(arondto::GlobalTrajectory& dto, const GlobalTrajectory& bo);
void fromAron(const arondto::GlobalTrajectory& dto, GlobalTrajectory& bo);
+ void toAron(arondto::LocalTrajectoryPoint& dto, const LocalTrajectoryPoint& bo);
+ void fromAron(const arondto::LocalTrajectoryPoint& dto, LocalTrajectoryPoint& bo);
+
+ void toAron(arondto::LocalTrajectory& dto, const LocalTrajectory& bo);
+ void fromAron(const arondto::LocalTrajectory& dto, LocalTrajectory& bo);
+
void toAron(arondto::Twist& dto, const Twist& bo);
void fromAron(const arondto::Twist& dto, Twist& bo);
diff --git a/source/armarx/navigation/platform_controller/CMakeLists.txt b/source/armarx/navigation/platform_controller/CMakeLists.txt
index 509c8f9a..a5a21b12 100644
--- a/source/armarx/navigation/platform_controller/CMakeLists.txt
+++ b/source/armarx/navigation/platform_controller/CMakeLists.txt
@@ -1,18 +1,19 @@
armarx_add_aron_library(platform_controller_aron
ARON_FILES
- aron/PlatformTrajectoryControllerConfig.xml
+ aron/PlatformLocalTrajectoryControllerConfig.xml
+ aron/PlatformGlobalTrajectoryControllerConfig.xml
DEPENDENCIES
armarx_control::common_aron
)
armarx_add_library(platform_controller
SOURCES
- # WholeBodyImpedanceController.cpp
- PlatformTrajectoryController.cpp
+ # PlatformLocalTrajectoryController.cpp
+ PlatformGlobalTrajectoryController.cpp
aron_conversions.cpp
HEADERS
- # WholeBodyImpedanceController.h
- PlatformTrajectoryController.h
+ # PlatformLocalTrajectoryController.h
+ PlatformGlobalTrajectoryController.h
aron_conversions.h
DEPENDENCIES_PUBLIC
Simox::VirtualRobot
@@ -20,5 +21,4 @@ armarx_add_library(platform_controller
armarx_control::client
armarx_navigation::core
armarx_navigation::trajectory_control
- # armarx_control::njoint_qp_controller_aron
)
diff --git a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp b/source/armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.cpp
similarity index 94%
rename from source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
rename to source/armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.cpp
index b5da2a90..a8758b6c 100644
--- a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
+++ b/source/armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.cpp
@@ -1,4 +1,4 @@
-#include "PlatformTrajectoryController.h"
+#include "PlatformGlobalTrajectoryController.h"
#include "ArmarXCore/core/ArmarXObjectScheduler.h"
#include "ArmarXCore/core/logging/Logging.h"
@@ -11,13 +11,13 @@
#include <armarx/control/common/type.h>
// #include <armarx/control/common/utils.h>
#include <armarx/navigation/common/controller_types.h>
-#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
+#include <armarx/navigation/platform_controller/aron/PlatformGlobalTrajectoryControllerConfig.aron.generated.h>
#include <armarx/navigation/platform_controller/aron_conversions.h>
-namespace armarx::navigation::platform_controller::platform_trajectory
+namespace armarx::navigation::platform_controller::platform_global_trajectory
{
const NJointControllerRegistration<Controller> Registration(
- common::ControllerTypeNames.to_name(common::ControllerType::PlatformTrajectory));
+ common::ControllerTypeNames.to_name(common::ControllerType::PlatformGlobalTrajectory));
Controller::Controller(const RobotUnitPtr& robotUnit,
const NJointControllerConfigPtr& config,
@@ -25,7 +25,7 @@ namespace armarx::navigation::platform_controller::platform_trajectory
{
ARMARX_IMPORTANT << "Creating "
<< common::ControllerTypeNames.to_name(
- common::ControllerType::PlatformTrajectory);
+ common::ControllerType::PlatformGlobalTrajectory);
// config
ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
ARMARX_CHECK_EXPRESSION(cfg);
@@ -57,7 +57,7 @@ namespace armarx::navigation::platform_controller::platform_trajectory
Controller::getClassName(const Ice::Current& iceCurrent) const
{
return armarx::navigation::common::ControllerTypeNames.to_name(
- armarx::navigation::common::ControllerType::PlatformTrajectory);
+ armarx::navigation::common::ControllerType::PlatformGlobalTrajectory);
}
void
@@ -130,7 +130,7 @@ namespace armarx::navigation::platform_controller::platform_trajectory
datafields["trajectory_points"] = new Variant(configBuffer.getUpToDateReadBuffer().targets.trajectory.points().size());
debugObservers->setDebugChannel(
- common::ControllerTypeNames.to_name(common::ControllerType::PlatformTrajectory),
+ common::ControllerTypeNames.to_name(common::ControllerType::PlatformGlobalTrajectory),
datafields);
}
diff --git a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h b/source/armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.h
similarity index 82%
rename from source/armarx/navigation/platform_controller/PlatformTrajectoryController.h
rename to source/armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.h
index daee089b..65d7d79a 100644
--- a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h
+++ b/source/armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.h
@@ -40,7 +40,7 @@ namespace armarx
} // namespace armarx
-namespace armarx::navigation::platform_controller::platform_trajectory
+namespace armarx::navigation::platform_controller::platform_global_trajectory
{
struct Targets
{
@@ -112,15 +112,6 @@ namespace armarx::navigation::platform_controller::platform_trajectory
void rtPreActivateController() override;
private:
- // [[nodiscard]] bool initializeQPIK();
-
- // void updateDebugStatus();
-
- // struct DebugStatus
- // {
- // std::map<std::string, float> data;
- // };
-
TripleBuffer<Config> configBuffer;
// internal
@@ -133,17 +124,6 @@ namespace armarx::navigation::platform_controller::platform_trajectory
Devices getDevices(const VirtualRobot::RobotNodeSet& rns);
-
- // NameValueMap getJointVelocities(const NameValueMap& initialJointValues,
- // const NameValueMap& targetJointValues,
- // const std::set<std::string>& joints);
-
- // Eigen::VectorXd toOptikRobotState(const NameValueMap& jointValues) const;
-
- // NameValueMap getJointValuesWithVirtualPlatformJoints();
-
- // NameValueMap getPlatformJointVelocity(const NameValueMap& initialJointAngles,
- // const NameValueMap& targetJointAngles) const;
};
-} // namespace armarx::navigation::platform_controller::platform_trajectory
+} // namespace armarx::navigation::platform_controller::platform_global_trajectory
diff --git a/source/armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.xml b/source/armarx/navigation/platform_controller/aron/PlatformGlobalTrajectoryControllerConfig.xml
similarity index 91%
rename from source/armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.xml
rename to source/armarx/navigation/platform_controller/aron/PlatformGlobalTrajectoryControllerConfig.xml
index 3ca40737..515b1148 100644
--- a/source/armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.xml
+++ b/source/armarx/navigation/platform_controller/aron/PlatformGlobalTrajectoryControllerConfig.xml
@@ -16,26 +16,26 @@
</CodeIncludes>
<GenerateTypes>
- <Object name='armarx::navigation::platform_controller::platform_trajectory::arondto::Params'>
+ <Object name='armarx::navigation::platform_controller::platform_global_trajectory::arondto::Params'>
<ObjectChild key='twistLimits'>
<armarx::navigation::core::arondto::TwistLimits />
</ObjectChild>
</Object>
- <Object name='armarx::navigation::platform_controller::platform_trajectory::arondto::Targets'>
+ <Object name='armarx::navigation::platform_controller::platform_global_trajectory::arondto::Targets'>
<ObjectChild key='trajectory'>
<armarx::navigation::core::arondto::GlobalTrajectory />
</ObjectChild>
</Object>
- <Object name='armarx::navigation::platform_controller::platform_trajectory::arondto::Config'>
+ <Object name='armarx::navigation::platform_controller::platform_global_trajectory::arondto::Config'>
<ObjectChild key='params'>
<armarx::navigation::traj_ctrl::arondto::TrajectoryFollowingControllerParams />
</ObjectChild>
<ObjectChild key='targets'>
- <armarx::navigation::platform_controller::platform_trajectory::arondto::Targets />
+ <armarx::navigation::platform_controller::platform_global_trajectory::arondto::Targets />
</ObjectChild>
</Object>
diff --git a/source/armarx/navigation/platform_controller/aron/PlatformLocalTrajectoryControllerConfig.xml b/source/armarx/navigation/platform_controller/aron/PlatformLocalTrajectoryControllerConfig.xml
new file mode 100644
index 00000000..a1479eef
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/aron/PlatformLocalTrajectoryControllerConfig.xml
@@ -0,0 +1,43 @@
+<?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>" /> -->
+ </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>" />
+ </CodeIncludes>
+ <GenerateTypes>
+
+ <Object name='armarx::navigation::platform_controller::platform_local_trajectory::arondto::Params'>
+ <ObjectChild key='twistLimits'>
+ <armarx::navigation::core::arondto::TwistLimits />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::platform_controller::platform_local_trajectory::arondto::Targets'>
+ <ObjectChild key='trajectory'>
+ <armarx::navigation::core::arondto::LocalTrajectory />
+ </ObjectChild>
+ </Object>
+
+
+ <Object name='armarx::navigation::platform_controller::platform_local_trajectory::arondto::Config'>
+ <ObjectChild key='params'>
+ <armarx::navigation::traj_ctrl::arondto::TrajectoryFollowingControllerParams />
+ </ObjectChild>
+
+ <ObjectChild key='targets'>
+ <armarx::navigation::platform_controller::platform_local_trajectory::arondto::Targets />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/navigation/platform_controller/aron_conversions.cpp b/source/armarx/navigation/platform_controller/aron_conversions.cpp
index 0f32b69e..8b502245 100644
--- a/source/armarx/navigation/platform_controller/aron_conversions.cpp
+++ b/source/armarx/navigation/platform_controller/aron_conversions.cpp
@@ -24,27 +24,50 @@
#include <RobotAPI/libraries/aron/common/aron_conversions/eigen.h>
#include <armarx/control/common/aron_conversions.h>
-#include <armarx/navigation/platform_controller/PlatformTrajectoryController.h>
#include <armarx/navigation/core/aron_conversions.h>
+#include <armarx/navigation/platform_controller/PlatformGlobalTrajectoryController.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/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
-
-namespace armarx::navigation::platform_controller::platform_trajectory
+namespace armarx::navigation::platform_controller
{
- void
- fromAron(const arondto::Targets& dto, Targets& bo)
+ namespace platform_global_trajectory
{
- 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::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);
+ }
+
+ } // namespace platform_global_trajectory
+
+ // namespace platform_local_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);
+ // }
+
+ // } // namespace platform_local_trajectory
-} // namespace armarx::navigation::platform_controller::platform_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 bd5518b1..1e338a7c 100644
--- a/source/armarx/navigation/platform_controller/aron_conversions.h
+++ b/source/armarx/navigation/platform_controller/aron_conversions.h
@@ -22,7 +22,7 @@
#pragma once
-namespace armarx::navigation::platform_controller::platform_trajectory
+namespace armarx::navigation::platform_controller::platform_global_trajectory
{
namespace arondto
{
@@ -32,4 +32,4 @@ namespace armarx::navigation::platform_controller::platform_trajectory
struct Config;
void fromAron(const arondto::Config& dto, Config& bo);
-} // namespace armarx::navigation::platform_controller::platform_trajectory
+} // namespace armarx::navigation::platform_controller::platform_global_trajectory
diff --git a/source/armarx/navigation/platform_controller/controller_descriptions.h b/source/armarx/navigation/platform_controller/controller_descriptions.h
index d352bed1..cb602115 100644
--- a/source/armarx/navigation/platform_controller/controller_descriptions.h
+++ b/source/armarx/navigation/platform_controller/controller_descriptions.h
@@ -23,22 +23,37 @@
#include <armarx/control/client/ControllerDescription.h>
#include <armarx/navigation/common/controller_types.h>
-#include <armarx/navigation/platform_controller/PlatformTrajectoryController.h>
-#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.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>
namespace armarx::control::client
{
template <>
- struct ControllerDescription<armarx::navigation::common::ControllerType::PlatformTrajectory>
+ struct ControllerDescription<armarx::navigation::common::ControllerType::PlatformGlobalTrajectory>
{
using AronDTO =
- armarx::navigation::platform_controller::platform_trajectory::arondto::Config;
+ armarx::navigation::platform_controller::platform_global_trajectory::arondto::Config;
- using BO = armarx::navigation::platform_controller::platform_trajectory::Config;
+ using BO = armarx::navigation::platform_controller::platform_global_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::PlatformTrajectoryControllerName;
+ armarx::navigation::common::PlatformGlobalTrajectoryControllerName;
};
+ // 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;
+
+ // // 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/execution/PlatformControllerExecutor.cpp b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
index d2bb7a47..9d173d95 100644
--- a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
@@ -45,7 +45,7 @@ namespace armarx::navigation::server
{
ARMARX_TRACE;
auto builder = controllerPlugin.createControllerBuilder<
- armarx::navigation::common::ControllerType::PlatformTrajectory>();
+ armarx::navigation::common::ControllerType::PlatformGlobalTrajectory>();
ARMARX_TRACE;
diff --git a/source/armarx/navigation/server/execution/PlatformControllerExecutor.h b/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
index 721d1b62..f852ea09 100644
--- a/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
@@ -38,7 +38,7 @@ namespace armarx::navigation::server
private:
std::optional<armarx::control::client::ControllerWrapper<
- armarx::navigation::common::ControllerType::PlatformTrajectory>>
+ armarx::navigation::common::ControllerType::PlatformGlobalTrajectory>>
ctrl;
ControllerComponentPlugin& controllerPlugin;
--
GitLab
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 3/3] 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