armarx_navigation Overview

Table of Contents

Foreword

The goal of this project is to equip the robot with PointGoal navigation capabilities.

The implemented navigation stack consists of three layers:

  1. Global Planning – Computes a collision-free path to the goal. Each waypoint includes velocity constraints that the controller must respect. The global plan is updated whenever the goal changes.
  2. Local Planning (Optional) – Refines the global path using a receding-horizon approach, generating a trajectory over a short time window while considering dynamic obstacles. This layer is executed at a fixed high-frequent rate.
  3. Trajectory Execution – Executes the planned trajectory using a real-time controller. The controller varies depending on whether local planning is active. In all cases, a real-time-capable controller, running within the robot's real-time unit, processes trajectory updates.

Additionally, execution is continuously monitored, and a safety guard ensures collision avoidance by reducing the velocity in close proximity to obstacles.

Publications

This work is described in the following publications:

@ARTICLE {Reister2022,
author = {Fabian Reister and Markus Grotz and Tamim Asfour},
title = {Combining Navigation and Manipulation Costs for Time-Efficient Robot Placement in Mobile Manipulation Tasks},
pages = {9913--9920},
volume ={7},
number ={4},
journal ={IEEE Robotics and Automation Letters (RA-L)},
year = {2022}
}

List of papers that build upon this work:

@INPROCEEDINGS {PohlReister2024,
author = {Christoph Pohl and Fabian Reister and Fabian Peller-Konrad and Tamim Asfour},
title = {MAkEable: Memory-centered and Affordance-based Task Execution Framework for Transferable Mobile Manipulation Skills},
booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
pages = {3674--3681},
year = {2024}
}

Terms and Concepts

Navigation Stack

The framework follows the strategy pattern, allowing a configurable selection of algorithms for each layer of the navigation stack. The available options include:

  • Global Planning
    • A*
    • Shortest Path Faster Algorithm (SPFA)
    • Point2Point (straight line)

The global planners provide a collision free positional path. To obtain suitable base orientations, a subsequent orientation optimization step is performed.

  • Local Planning
    • Timed Elastic Bands (TEB)
  • Trajectory Execution
    • Trajectory-following controller (PID control with a feed-forward term)

Algorithmic Details

Global Planning

A* and SPFA edge weighting:

In addition to the Euclidean distance between nodes, the proximity to the nearest obstacle is also considered. For further details, refer to the paper in the Publications section.

Orientation optimization:

The orientation optimization aims to balance the following objectives:

  • Preserve the initial and final orientations near the start and goal positions.
  • Minimize orientation changes between consecutive nodes to enhance trajectory smoothness.
  • Align the robot's base orientation with the direction of movement.

This optimization is applied to all nodes except the start and goal nodes and is implemented using the Ceres Solver library.

Types

Costmaps

For global planning, we use a costmap-based approach. A costmap is a fixed-resolution grid where each cell is assigned a score, representing its cost, and a validity mask to indicate whether it is traversable.

Also, we use the costmaps for other maps, e.g. distance fields for simplicity.

Locations

Named locations, such as "in front of the table," can be defined in two ways: absolute or relative to objects. For relative locations, a reference frame must be specified. Typically, the root frame is used for rigid objects, while any known frame of the object can be utilized for articulated objects. This flexibility ensures accurate and context-aware location specification.

Those locations should follow a certain naming scheme. That is depending on the location

  • absolute: {location}
  • relative: {object_name}/{location}:{instanceId}. Here, the location should be specified in camel case, e.g., inFrontOf. The instance ID is optional. If it is specified, the location is only valid for a specific object. Usually, the locations are available for all objects of a class.

Rooms

Rooms are represented as polygons with a specified height. This representation enables several functionalities, such as restricting operations to specific rooms or elevating subsymbolic data to a symbolic level—for instance, determining whether the robot is located within a particular room. Additionally, this information can serve as a navigation target, facilitating room-specific navigation tasks.

Instantiation of the navigation stack

Navigator Server

Once a navigation stack is instantiated, a NavigatorServer can be used to coordinate its execution. This class orchestrates the different layers and processes navigation commands. Through well-defined interfaces, dependencies can be injected, enabling modular and flexible integration.

Key Interfaces

  • EventPublisher – Emits events such as movement start, planning failures, or goal completion.
  • Executor – Receives the planned trajectory and forwards it to the platform controller.
  • Introspection – Provides relevant data for visualization (e.g., the planned trajectory).
  • Monitoring – Tracks the progress of navigation requests and verifies goal completion.
  • SceneProvider – Aggregates and supplies scene information.

The NavigatorServer can be instantiated in a custom component or accessed via the navigator, the main component of this package.

Navigator Client

The navigator client is the counterpart of the navigator server. It provides access to a navigator server to which navigation requests can be sent. Also, callbacks can be registered to handle events (e.g., start of movement, planning failure, ...).

Events

During execution, the events described in the following can occur. Each of the events also provides meaningful information (timestamp of the event, poses, etc ...).

We can group them as follows related to the layers of the navigation stack

Global planning:

  • GlobalTrajectoryUpdated
  • GlobalPlanningFailed

Local planning:

  • LocalTrajectoryUpdated
  • LocalPlanningFailed

Safety guard:

  • SafetyThrottlingTriggered
  • SafetyStopTriggered

Also, the progress of the execution is available through the following events

  • MovementStarted
  • GoalReached
  • WaypointReached

In case of general failures, the following events

  • UserAbortTriggered
  • InternalError

For a detailed description, see the events definition.

Navigation memory

Memory structure

The navigation memory provides the following core segments:

Scene information:

  • Costmap:
  • Graph:
  • Location:
  • Human:
  • LaserScannerFeatures:

Navigation stack:

  • Parameterization: Parameterization of the navigation stack for different navigator clients.
  • Events: Events generated by the navigator

Prior knowledge

Prior knowledge is loaded into the memory on startup including locations and rooms.

Locations are provided through a locations.json with the following structure:

{
"locations": {
"center": {
"framedPose": {
"agent": "",
"frame": "Global",
"pose": [
[
1.0,
0.0,
0.0,
3650.0
],
[
0.0,
1.0,
0.0,
7200.0
],
[
0.0,
0.0,
1.0,
0.0
],
[
0.0,
0.0,
0.0,
1.0
]
]
}
},
{
"Kitchen/countertop/inFrontOf": {
"framedPose": {
"agent": "Kitchen/mobile-kitchen-counter/0",
"frame": "root",
"pose": [
[
5.960464477539063e-08,
-0.9999999403953552,
0.0,
1100.0
],
[
0.9999999403953552,
5.960464477539063e-08,
0.0,
-680.0
],
[
0.0,
0.0,
1.0,
0.0
],
[
0.0,
0.0,
0.0,
1.0
]
]
}
}
}
}
}

Rooms are provided through a rooms.json with the following structure:

{
"rooms": {
"Kitchen": {
"polygon": [
[
0,
0
],
[
0,
7000
],
[
6000,
7000
],
[
6000,
0
]
],
"height": 3500
}
}
}

Navigation Goal

To determine a suitable navigation target the following algorithms exist:

  • RoomNavigationTarget: generates a navigation target in front of a room

Simulation

Human Simulation

To test the navigation stack in simulation, humans are modeled using the Reciprocal Velocity Obstacle (RVO) algorithm. Human paths are first generated with a global planning algorithm targeting random goals based on the traversability map. Their velocities are then initialized along these paths and dynamically adjusted to avoid obstacles, including the robot, using RVO.

Structure of this Package

On the code level, this package contains the following libraries and components:

Libraries

The following fundamental libraries are available:

The following libraries provide strategies for the different layers of the navigation stack:

To instantiate different strategies, the factories library provides factory methods that abstract the underlying algorithms. These factories also handle optional algorithms and dependencies, ensuring flexible and modular integration.

Other libraries include:

The client library offers a user-friendly API for interacting with the navigator component. It serves as the primary entry point for users who want to command the robot to navigate to a specific location.

Components

  • navigator
  • navigation_memory

Skill providers:

  • navigation_skill_provider

Distance map providers

  • distance_to_obstacle_costmap_provider: This component extracts scene information from the object memory, specifically the poses of known objects, and generates a corresponding distance map. The distance map is automatically updated whenever a known object's pose changes. Additionally, the generation process can be restricted to specific object datasets.
  • dynamic_distance_to_obstacle_costmap_provider: Enhances a base costmap, such as one generated by the distance_to_obstacle_costmap_provider, by incorporating dynamic scene information from sources like laser scanner data. This component ensures the distance map is continuously updated to reflect real-time changes in the environment.
  • costmap_merger: Combines multiple costmaps into a single unified map. The costmaps are merged in a prioritized order, with the first costmap taking precedence over subsequent ones. This functionality is particularly useful for integrating static scene information with dynamic, up-to-date data near the robot, ensuring a comprehensive and prioritized representation of the environment.

Dynamic scene information

  • dynamic_scene_provider: This component is used to track humans, including an estimate of their velocity.
  • The laser_scanner_feature_extraction component processes raw laser scanner data and clusters it into meaningful features. If distance maps are available, the laser scanner data can be filtered to focus solely on unknown obstacles. The extracted features include:
    • Convex Hull: Represents the outer boundary of the clustered points.
    • Raw Points: Retains the original laser scanner data points.
    • Ellipsoid: Approximates the clustered points as ellipsoidal shapes.
    • Chain/Line Approximation: Simplifies the clustered points into chains or linear segments.

Skill providers

  • navigation_skill_provider

Simulation

  • human_simulator

Skills

navigation_skill_provider

Implemented in the library skills are the following basic skills

Skill name Description
NavigateTo Navigate to a target pose using absolute coordinates.
NavigateToLocation Navigate to a predefined location. The location is provided as an entity in the memory.
NavigateToNamedLocation Navigate to a location. Fuzzy matching is applied to resolve the exact location id.

Skills to give local commands, resulting in point-to-point motion:

Skill name Description
MoveXMeters Move the robot a specified distance into a certain direction.
MoveRelativePlanar Move the robot to a relative position in the planar (x, y) coordinate system.
RotateXDegrees Rotate the robot by a specified angle in degrees around its vertical axis.

More advanced skills:

Skill name Description Status
BringHumanIntoRoom Assist a human in entering a specified room. Planned / Draft
GuideHumanToRoom Guide a human to a designated room. Planned / Draft
NavigateToChargingStation Navigate the robot to a charging station. Planned / Draft

GUI Widgets

LocationGraphEditor

This widget provides an intuitive and user-friendly interface for managing locations. Its key features include:

  • Add and Remove Locations: Easily add new locations or delete existing ones.
  • Update Location Poses:
    • Specify exact coordinates or make adjustments to the current pose.
    • Set the location to match the robot's current pose.
  • Switch Between Absolute and Relative Poses:
    • Convert poses from absolute coordinates to positions relative to an object.