diff --git a/source/Navigation/components/ExampleClient/ExampleClient.cpp b/source/Navigation/components/ExampleClient/ExampleClient.cpp
index f12a119de8ccb70b9c43fec739d43b3c87266952..927483ed45b2db477f8d84513dffab1e2199fac9 100644
--- a/source/Navigation/components/ExampleClient/ExampleClient.cpp
+++ b/source/Navigation/components/ExampleClient/ExampleClient.cpp
@@ -27,6 +27,7 @@
// STD/STL
#include <chrono>
#include <thread>
+
#include <Eigen/src/Geometry/Translation.h>
exns::ExampleClient::ExampleClient()
@@ -84,16 +85,12 @@ exns::ExampleClient::exampleNavigation()
// Create an example configuration valid for the following move* calls.
navigator.configure(
NavigationStackConfig()
- .configureGeneral({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
- .configureGlobalPlanner(glob_plan::AStarParams())
- .configureTrajectoryController(traj_ctrl::TrajectoryFollowingControllerParams()));
+ .configureGeneral({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
+ .configureGlobalPlanner(glob_plan::AStarParams())
+ .configureTrajectoryController(traj_ctrl::TrajectoryFollowingControllerParams()));
// Example of registering a lambda as callback.
- navigator.onGoalReached([&]()
- {
- ARMARX_IMPORTANT << "Goal reached! (lambda-style)";
- });
-
+ navigator.onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached! (lambda-style)"; });
// Example of registering a method as callback.
navigator.onGoalReached([this] { goalReached(); });
@@ -109,7 +106,6 @@ exns::ExampleClient::exampleNavigation()
std::this_thread::sleep_for(15s);
-
// Wait until goal is reached
navigator.waitForGoalReached();
ARMARX_INFO << "Goal 1 reached.";
@@ -119,7 +115,6 @@ exns::ExampleClient::exampleNavigation()
std::this_thread::sleep_for(15s);
-
// Wait until goal is reached
navigator.waitForGoalReached();
ARMARX_INFO << "Goal 2 reached.";
@@ -127,7 +122,6 @@ exns::ExampleClient::exampleNavigation()
goal.translation() << 4500, 4500, 0;
navigator.moveTo(goal, NavigationFrame::Absolute);
-
// Wait until goal is reached
navigator.waitForGoalReached();
ARMARX_INFO << "Goal 3 reached.";
diff --git a/source/Navigation/libraries/algorithms/astar/AStarPlanner.cpp b/source/Navigation/libraries/algorithms/astar/AStarPlanner.cpp
index 66bc797782de325c0f4c9a54bf3fd3be16923c0c..de2c76125549a40b86edd45101e798f864814882 100644
--- a/source/Navigation/libraries/algorithms/astar/AStarPlanner.cpp
+++ b/source/Navigation/libraries/algorithms/astar/AStarPlanner.cpp
@@ -31,10 +31,8 @@ namespace armarx::nav::algo::astar
using point_t = bg::model::point<double, 3, bg::cs::cartesian>;
using box_t = bg::model::box<point_t>;
- const auto toPoint = [](const Eigen::Vector3f & pt)
- {
- return point_t(pt.x(), pt.y(), pt.z());
- };
+ const auto toPoint = [](const Eigen::Vector3f& pt)
+ { return point_t(pt.x(), pt.y(), pt.z()); };
box_t box1(toPoint(bb1.getMin()), toPoint(bb1.getMax()));
box_t box2(toPoint(bb2.getMin()), toPoint(bb2.getMax()));
diff --git a/source/Navigation/libraries/core/Trajectory.cpp b/source/Navigation/libraries/core/Trajectory.cpp
index 4de1ff07ee89f46063f58d45e71c6938fad53dca..07db63d68c04ed7957a14d60bb07ea6ee29c97b2 100644
--- a/source/Navigation/libraries/core/Trajectory.cpp
+++ b/source/Navigation/libraries/core/Trajectory.cpp
@@ -37,10 +37,7 @@ namespace armarx::nav::core
std::transform(path.begin(),
path.end(),
std::back_inserter(trajectoryPoints),
- [&](const auto & p)
- {
- return toTrajectoryPoint(p, velocity);
- });
+ [&](const auto& p) { return toTrajectoryPoint(p, velocity); });
return trajectoryPoints;
}
@@ -63,7 +60,7 @@ namespace armarx::nav::core
const auto& wpAfter = points.at(i + 1);
const auto closestPoint = VirtualRobot::MathTools::nearestPointOnSegment<Position>(
- wpBefore.waypoint.pose.translation(), wpAfter.waypoint.pose.translation(), point);
+ wpBefore.waypoint.pose.translation(), wpAfter.waypoint.pose.translation(), point);
const float currentDistance = (closestPoint - point).norm();
if (currentDistance < distance)
@@ -72,7 +69,7 @@ namespace armarx::nav::core
const float d1 = (closestPoint - wpBefore.waypoint.pose.translation()).norm();
const float d =
(wpBefore.waypoint.pose.translation() - wpAfter.waypoint.pose.translation())
- .norm();
+ .norm();
// fraction of distance between segment end points
const float t = d1 / d;
@@ -87,7 +84,8 @@ namespace armarx::nav::core
switch (velocityInterpolation)
{
case VelocityInterpolation::LinearInterpolation:
- bestProj.projection.velocity = wpBefore.velocity + (wpAfter.velocity - wpBefore.velocity) * t;
+ bestProj.projection.velocity =
+ wpBefore.velocity + (wpAfter.velocity - wpBefore.velocity) * t;
break;
case VelocityInterpolation::LastWaypoint:
bestProj.projection.velocity = wpBefore.velocity;
@@ -104,10 +102,8 @@ namespace armarx::nav::core
std::vector<Eigen::Vector3f>
Trajectory::positions() const
{
- const auto toPosition = [](const TrajectoryPoint & wp)
- {
- return wp.waypoint.pose.translation();
- };
+ const auto toPosition = [](const TrajectoryPoint& wp)
+ { return wp.waypoint.pose.translation(); };
std::vector<Eigen::Vector3f> positions;
positions.reserve(points.size());
@@ -124,7 +120,10 @@ namespace armarx::nav::core
}
Trajectory
- Trajectory::FromPath(const Pose& start, const Positions& waypoints, const Pose& goal, const float velocity)
+ Trajectory::FromPath(const Pose& start,
+ const Positions& waypoints,
+ const Pose& goal,
+ const float velocity)
{
// currently, only 2D version
@@ -138,14 +137,15 @@ namespace armarx::nav::core
path.push_back(start);
- const auto directedPose = [](const Position & position, const Position & target) -> Pose
+ const auto directedPose = [](const Position& position, const Position& target) -> Pose
{
const Direction direction = target - position;
const float yaw = math::Helpers::Angle(nav::conv::to2D(direction));
// our robot is moving into y direction ( y is forward ... )
- const Eigen::Rotation2Df pose_R_robot = Eigen::Rotation2Df(yaw) * Eigen::Rotation2Df(-M_PI_2f32);
+ const Eigen::Rotation2Df pose_R_robot =
+ Eigen::Rotation2Df(yaw) * Eigen::Rotation2Df(-M_PI_2f32);
return nav::conv::to3D(Eigen::Translation2f(nav::conv::to2D(position)) * pose_R_robot);
};
diff --git a/source/Navigation/libraries/global_planning/AStar.cpp b/source/Navigation/libraries/global_planning/AStar.cpp
index 5940910b8c37e2f94e4851df5ad58eae227ad55e..29a551406bd767062cfb72852e12550f07616dfb 100644
--- a/source/Navigation/libraries/global_planning/AStar.cpp
+++ b/source/Navigation/libraries/global_planning/AStar.cpp
@@ -118,15 +118,15 @@ namespace armarx::nav::glob_plan
ARMARX_TRACE;
auto trajectory = core::Trajectory::FromPath(core::Pose(scene.robot->getGlobalPose()),
- conv::to3D(smoothPlan),
- goal,
- params.linearVelocity);
+ conv::to3D(smoothPlan),
+ goal,
+ params.linearVelocity);
- // TODO(fabian.reister): resampling of trajectory
+ // TODO(fabian.reister): resampling of trajectory
// TODO(fabian.reister): define optimization problem to optimize orientation
// - start and end pose are fixed
// - orientation diff between neighbor nodes should be minimized (smoothness)
- // - check if position could be optimized gently (this might require resampling during optimization)
+ // - check if position could be optimized gently (this might require resampling during optimization)
ARMARX_TRACE;
return std::make_shared<core::Trajectory>(trajectory);
diff --git a/source/Navigation/libraries/server/Navigator.cpp b/source/Navigation/libraries/server/Navigator.cpp
index 087a5ed99283a24a7b70ed91469beca9450c7fca..d59f6edc1cc4fe0607813706e9b2b8c56533feb5 100644
--- a/source/Navigation/libraries/server/Navigator.cpp
+++ b/source/Navigation/libraries/server/Navigator.cpp
@@ -56,10 +56,8 @@ namespace armarx::nav::server
std::transform(std::begin(waypoints),
std::end(waypoints),
std::back_inserter(globalWaypoints),
- [&](const core::Pose & p)
- {
- return core::Pose(config.scene->robot->getGlobalPose()) * p;
- });
+ [&](const core::Pose& p)
+ { return core::Pose(config.scene->robot->getGlobalPose()) * p; });
break;
}
@@ -127,10 +125,10 @@ namespace armarx::nav::server
// executor
executorTask = new PeriodicTask<Navigator>(this,
- &Navigator::updateExecutor,
- config.general.tasks.executorUpdatePeriod,
- false,
- "ExecutorTask");
+ &Navigator::updateExecutor,
+ config.general.tasks.executorUpdatePeriod,
+ false,
+ "ExecutorTask");
executorTask->start();
// introspector
@@ -146,10 +144,10 @@ namespace armarx::nav::server
goalReachedMonitor =
GoalReachedMonitor(waypoints, *config.scene, GoalReachedMonitorConfig());
monitorTask = new PeriodicTask<Navigator>(this,
- &Navigator::updateMonitor,
- config.general.tasks.monitorUpdatePeriod,
- false,
- "MonitorTask");
+ &Navigator::updateMonitor,
+ config.general.tasks.monitorUpdatePeriod,
+ false,
+ "MonitorTask");
monitorTask->start();
// Could be required if pauseMovement() has been called in the past.
diff --git a/source/Navigation/libraries/trajectory_control/TrajectoryController.h b/source/Navigation/libraries/trajectory_control/TrajectoryController.h
index 9404566bddec5579b2e0278ec9d8f23e44af2211..117194c66f35b5a25c57edfb696984b4185814b1 100644
--- a/source/Navigation/libraries/trajectory_control/TrajectoryController.h
+++ b/source/Navigation/libraries/trajectory_control/TrajectoryController.h
@@ -47,10 +47,9 @@ namespace armarx::nav::traj_ctrl
float angularVelocity{std::numeric_limits<float>::max()};
};
- Limits limits
- {
- .linearVelocity = 400.F, // [mm/s]
- .angularVelocity = 2.F * M_PIf32 / 5.F // [rad/s]
+ Limits limits{
+ .linearVelocity = 400.F, // [mm/s]
+ .angularVelocity = 2.F * M_PIf32 / 5.F // [rad/s]
};
virtual Algorithms algorithm() const = 0;
diff --git a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
index d96b12c5da5544c727e8012349d5169782805da4..4acb64d0b65e595942d6e30af946928650517fd9 100644
--- a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
@@ -53,7 +53,7 @@ namespace armarx::nav::traj_ctrl
// TrajectoryFollowingController
TrajectoryFollowingController::TrajectoryFollowingController(const Params& params,
- const core::Scene& context) :
+ const core::Scene& context) :
TrajectoryController(context),
params(params),
pidPos(params.pidPos.Kp,
@@ -62,17 +62,14 @@ namespace armarx::nav::traj_ctrl
std::numeric_limits<double>::max(),
std::numeric_limits<double>::max(),
false,
- std::vector<bool>
- {
- false, false, false
- }),
- pidOri(params.pidOri.Kp / 2, // FIXME
- params.pidOri.Ki,
- params.pidOri.Kd,
- std::numeric_limits<double>::max(),
- std::numeric_limits<double>::max(),
- false,
- std::vector<bool> {true, true, true})
+ std::vector<bool>{false, false, false}),
+ pidOri(params.pidOri.Kp / 2, // FIXME
+ params.pidOri.Ki,
+ params.pidOri.Kd,
+ std::numeric_limits<double>::max(),
+ std::numeric_limits<double>::max(),
+ false,
+ std::vector<bool>{true, true, true})
{
}
@@ -108,8 +105,8 @@ namespace armarx::nav::traj_ctrl
const core::Pose currentPose(context.robot->getGlobalPose());
- const auto projectedPose = trajectory->getProjection(
- currentPose, core::VelocityInterpolation::LastWaypoint);
+ const auto projectedPose =
+ trajectory->getProjection(currentPose, core::VelocityInterpolation::LastWaypoint);
using simox::math::mat4f_to_pos;
using simox::math::mat4f_to_rpy;
@@ -124,7 +121,7 @@ namespace armarx::nav::traj_ctrl
const Eigen::Vector3f desiredMovementDirection =
(projectedPose.wayPointAfter.waypoint.pose.translation() -
projectedPose.wayPointBefore.waypoint.pose.translation())
- .normalized();
+ .normalized();
const Eigen::Vector3f feedforwardVelocity = desiredMovementDirection * ffVel;
@@ -133,8 +130,7 @@ namespace armarx::nav::traj_ctrl
ARMARX_INFO << deactivateSpam(100) << "Control value " << pidPos.getControlValue();
core::Twist twist = {.linear = pidPos.getControlValue() + feedforwardVelocity,
- .angular = pidOri.getControlValue()
- };
+ .angular = pidOri.getControlValue()};
const auto twistLimited = applyTwistLimits(twist);
ARMARX_INFO << deactivateSpam(100) << "Twist limited " << twistLimited.linear;