armarx::navigation::algorithms Namespace Reference

This file is part of ArmarX. More...

Namespaces
namespace	arondto
namespace	costmap
	This file is part of ArmarX.
namespace	orientation_aware
	This file is part of ArmarX.
namespace	spfa
	This file is part of ArmarX.
namespace	tsp
	This file is part of ArmarX.

Classes
class	Costmap
class	CostmapBuilder
class	ObstacleAwareVelocityLimit
	The maximum permissible linear velocity as a function of the distance to the closest obstacle. More...
struct	overloaded
struct	Room
struct	SceneBounds
class	Toppra
	Time-optimal reparametrization under per-motor torque and command-ramp limits. More...
class	TrajectoryParametrization
	Assigns the velocities along an already planned path. More...

Typedefs
using	TrajectoryParametrizationPtr = std::shared_ptr<TrajectoryParametrization>

Enumerations
enum class	CostmapMergeMode { MAX , MIN , AVERAGE }
	Defines how the. More...

Functions
void	checkSameSize (const std::vector< Costmap > &costmaps)
SceneBounds	computeSceneBounds (const std::vector< Eigen::Vector2f > &points, const SceneBounds &init, const float margin)
SceneBounds	computeSceneBounds (const VirtualRobot::SceneObjectSetPtr &obstacles, const std::vector< VirtualRobot::RobotPtr > &articulatedObjects, const SceneBounds &init, const std::vector< Room > &rooms, const float margin, const bool restrictToRooms)
orientation_aware::Costmap3D	costmap3dFromAron (const armem::wm::EntityInstance &entityInstance)
Costmap	costmapFromAron (const aron::data::DictPtr &dto)
Costmap::Mask	dilateMask (const Costmap::Mask &mask, int numIterations)
std::filesystem::path	DriveParamsPath (const std::string &robot="Armar7")
	Resolve config/platform/PlatformDynamics<robot>.json inside the armarx_navigation package.
template<typename T, typename Proj>
std::vector< T >	filterByRoom (const armarx::navigation::algorithms::Room &room, const std::vector< T > &objects, Proj proj)
	Filter a collection of objects by whether their projected position lies inside a room.
Costmap	fromAron (const armem::wm::EntityInstance &entityInstance)
void	fromAron (const arondto::Room &dto, Room &bo)
void	fromAron (const arondto::ShortestPathFasterAlgorithmParams &dto, spfa::ShortestPathFasterAlgorithm::Parameters &bo)
void	invalidateOutsideRooms (const std::vector< Room > &rooms, Costmap &costmap, const float footprintRadius)
void	invalidateOutsideRooms (const std::vector< Room > &rooms, orientation_aware::Costmap3D &costmap, const float footprintRadius)
Costmap	load (const std::filesystem::path &directory)
Toppra::DriveParams	LoadDriveParams (const std::filesystem::path &configFile)
	Read Toppra::DriveParams from a PlatformDynamics<Robot>.json.
SceneBounds	merge (const std::vector< SceneBounds > &sceneBounds)
Costmap	mergeAligned (const std::vector< Costmap > &costmaps, const std::vector< float > &weights)
Costmap	mergeAligned (const std::vector< Costmap > &costmaps, CostmapMergeMode mode)
template<class... Ts>
	overloaded (Ts...) -> overloaded< Ts... >
std::optional< core::Pose2D >	sampleValidPositionInMap (const algorithms::Costmap &costmap)
bool	save (const Costmap &costmap, const std::filesystem::path &directory)
Costmap	scaleCostmap (const Costmap &costmap, float cellSize)
void	toAron (arondto::Room &dto, const Room &bo)
void	toAron (arondto::ShortestPathFasterAlgorithmParams &dto, const spfa::ShortestPathFasterAlgorithm::Parameters &bo)
template<class DtoT, class BoT>
DtoT	toAron (const BoT &bo)
armarx::aron::data::DictPtr	toAron (const Costmap &bo)
armarx::aron::data::DictPtr	toAron (const orientation_aware::Costmap3D &bo)
Costmap	toCostmap (const VirtualRobot::WorkspaceGrid &workspaceGrid)
SceneBounds	toSceneBounds (const VirtualRobot::WorkspaceGrid::Extends &extends)
void	visualize (const algorithms::Costmap &costmap, viz::Layer &layer, const std::string &name, const float zOffset)
void	visualize (const algorithms::orientation_aware::Costmap3D &costmap, std::vector< viz::Layer > &layers, const std::string &name, const float zOffset, bool initializeMask)

Detailed Description

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

2021

Copyright

http://www.gnu.org/licenses/gpl-2.0.txt GNU General Public License

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

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

2026

Copyright

http://www.gnu.org/licenses/gpl-2.0.txt GNU General Public License

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

2023

Copyright

http://www.gnu.org/licenses/gpl-2.0.txt GNU General Public License

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

2025

Copyright

http://www.gnu.org/licenses/gpl-2.0.txt GNU General Public License

Typedef Documentation

TrajectoryParametrizationPtr

using TrajectoryParametrizationPtr = std::shared_ptr<TrajectoryParametrization>

Enumeration Type Documentation

CostmapMergeMode

enum class CostmapMergeMode

strong

Defines how the.

Enumerator
MAX
MIN
AVERAGE

Function Documentation

checkSameSize()

void checkSameSize

(

const std::vector< Costmap > &

costmaps

)

computeSceneBounds() [1/2]

SceneBounds computeSceneBounds	(	const std::vector< Eigen::Vector2f > &	points,
		const SceneBounds &	init,
		const float	margin )

computeSceneBounds() [2/2]

SceneBounds computeSceneBounds	(	const VirtualRobot::SceneObjectSetPtr &	obstacles,
		const std::vector< VirtualRobot::RobotPtr > &	articulatedObjects,
		const SceneBounds &	init,
		const std::vector< Room > &	rooms,
		const float	margin,
		const bool	restrictToRooms )

costmap3dFromAron()

orientation_aware::Costmap3D costmap3dFromAron

(

const armem::wm::EntityInstance &

entityInstance

)

costmapFromAron()

Costmap costmapFromAron

(

const aron::data::DictPtr &

dto

)

dilateMask()

Costmap::Mask dilateMask	(	const Costmap::Mask &	mask,
		int	numIterations )

Dilate the costmap mask to bridge over small gaps in the mask

DriveParamsPath()

std::filesystem::path DriveParamsPath

(

const std::string &

robot = "Armar7"

)

Resolve config/platform/PlatformDynamics<robot>.json inside the armarx_navigation package.

robot is the platform name as the rest of the codebase spells it – Armar7, Armar6, ArmarDE – matching the analysis application's --robot.

filterByRoom()

template<typename T, typename Proj>

std::vector< T > filterByRoom	(	const armarx::navigation::algorithms::Room &	room,
		const std::vector< T > &	objects,
		Proj	proj )

Filter a collection of objects by whether their projected position lies inside a room.

This function applies a projection callable to each object in objects and retains those objects for which the room reports the projected value as inside (via Room::isInside). The relative order of the returned elements matches the order in objects.

Template Parameters

T	Type of elements stored in objects. Must be CopyConstructible or MoveConstructible since selected elements are returned by value in a new std::vector<T>.
Proj	Callable type for the projection. Must be Invocable with an argument of type const T& (or T) and return a value accepted by armarx::navigation::algorithms::Room::isInside.

Parameters

room	Reference to an armarx::navigation::algorithms::Room used for the inside-test.
objects	Constant reference to the input vector of objects to be filtered.
proj	Projection callable applied to each object to obtain the value checked by room.isInside.

Returns

A std::vector<T> containing copies of the objects for which room.isInside(proj(obj)) returns true. If no objects satisfy the predicate, an empty vector is returned.

@complexity Linear in objects.size(): each element is projected and tested once.

@exceptionsafety Any exceptions thrown by proj or Room::isInside propagate to the caller.

Note

This implementation relies on C++ ranges utilities (ranges::views::filter and ranges::to_vector).

fromAron() [1/3]

Costmap fromAron

(

const armem::wm::EntityInstance &

entityInstance

)

fromAron() [2/3]

void fromAron	(	const arondto::Room &	dto,
		Room &	bo )

fromAron() [3/3]

void fromAron	(	const arondto::ShortestPathFasterAlgorithmParams &	dto,
		spfa::ShortestPathFasterAlgorithm::Parameters &	bo )

invalidateOutsideRooms() [1/2]

void invalidateOutsideRooms	(	const std::vector< Room > &	rooms,
		Costmap &	costmap,
		const float	footprintRadius )

invalidateOutsideRooms() [2/2]

void invalidateOutsideRooms	(	const std::vector< Room > &	rooms,
		orientation_aware::Costmap3D &	costmap,
		const float	footprintRadius )

load()

Costmap load

(

const std::filesystem::path &

directory

)

LoadDriveParams()

Toppra::DriveParams LoadDriveParams

(

const std::filesystem::path &

configFile

)

Read Toppra::DriveParams from a PlatformDynamics<Robot>.json.

The same files the analysis application and the python benchmark read, so all three agree on a platform by construction rather than by three hand-maintained copies.

Note

The ramp limits come from that file's commandRateLimit block, which mirrors the robot's HardwareConfig/.../Platform.xml. The two are not linked, so a change to the hardware config has to be reflected here by hand.

Exceptions

std::runtime_error

if the file is missing, unreadable or lacks a required key.

merge()

SceneBounds merge

(

const std::vector< SceneBounds > &

sceneBounds

)

mergeAligned() [1/2]

Costmap mergeAligned	(	const std::vector< Costmap > &	costmaps,
		const std::vector< float > &	weights )

mergeAligned() [2/2]

Costmap mergeAligned	(	const std::vector< Costmap > &	costmaps,
		CostmapMergeMode	mode )

overloaded()

template<class... Ts>

overloaded

(

Ts...

)

-> overloaded< Ts... >

sampleValidPositionInMap()

std::optional< core::Pose2D > sampleValidPositionInMap

(

const algorithms::Costmap &

costmap

)

save()

bool save	(	const Costmap &	costmap,
		const std::filesystem::path &	directory )

scaleCostmap()

Costmap scaleCostmap	(	const Costmap &	costmap,
		float	cellSize )

toAron() [1/5]

void toAron	(	arondto::Room &	dto,
		const Room &	bo )

toAron() [2/5]

void toAron	(	arondto::ShortestPathFasterAlgorithmParams &	dto,
		const spfa::ShortestPathFasterAlgorithm::Parameters &	bo )

toAron() [3/5]

template<class DtoT, class BoT>

DtoT toAron

(

const BoT &

bo

)

toAron() [4/5]

armarx::aron::data::DictPtr toAron

(

const Costmap &

bo

)

toAron() [5/5]

armarx::aron::data::DictPtr toAron

(

const orientation_aware::Costmap3D &

bo

)

toCostmap()

Costmap toCostmap

(

const VirtualRobot::WorkspaceGrid &

workspaceGrid

)

toSceneBounds()

SceneBounds toSceneBounds

(

const VirtualRobot::WorkspaceGrid::Extends &

extends

)

visualize() [1/2]

void visualize	(	const algorithms::Costmap &	costmap,
		viz::Layer &	layer,
		const std::string &	name,
		const float	zOffset )

visualize() [2/2]

void visualize	(	const algorithms::orientation_aware::Costmap3D &	costmap,
		std::vector< viz::Layer > &	layers,
		const std::string &	name,
		const float	zOffset,
		bool	initializeMask )