Environment

class Environment

The environment a robot (or multiple robots) lives in.

The Environment class manages static obstacles, collision checking, waypoints, cameras, and robots within a defined environment. The class provides methods to add, remove, and manage obstacles and waypoints, as well as to retrieve information about these elements. It also includes functionality for collision checking and updating the state of obstacles and robots.

Public Functions

explicit Environment(std::shared_ptr<Robot> robot, float safety_margin = 0.0)

Create an environment with a controllable robot.

Parameters:

• robot – The robot to add to the environment.

• safety_margin – The global safety margin for collision checking [m].

explicit Environment(const std::set<std::shared_ptr<Robot>> &robots, float safety_margin = 0.0)

Create an environment with multiple robots.

Parameters:

• robots – The robots to add to the environment.

• safety_margin – The global safety margin for collision checking [m].

float get_safety_margin() const

Environment’s global safety margin for collision checking [m].

Returns:

The global safety margin for collision checking [m].

std::shared_ptr<Robot> get_robot(const std::string &name = "") const

Get the robot with the given name from the environment.

In case there is only a single robot in the environment, the default empty name argument will return this robot. Otherwise throws an error if no robot with the name exists.

Parameters:

name – The name of the robot to retrieve.

Returns:

The shared pointer to a robot object associated with the given name.

std::vector<std::shared_ptr<Robot>> get_robots() const

Get all robots within the environment.

Returns:

A vector of shared pointers to the robots within the environment.

Point get_waypoint(const std::string &name) const

Get the waypoint with the given name from the environment. Throws an error if no waypoint with the name exists.

Parameters:

name – The name of the waypoint to retrieve.

Returns:

The waypoint associated with the given name.

std::optional<Point> get_waypoint_by_tag(const std::string &tag) const

Get a waypoint within the environment given a tag. If multiple waypoints have the same tag, the first one to be found is returned.

Parameters:

tag – The tag associated with the waypoint to retrieve.

Returns:

An optional containing the waypoint associated with the given tag, or std::nullopt if no waypoint with the tag is found.

std::vector<Point> get_waypoints_by_tag(const std::string &tag) const

Get all waypoints within the environment given a tag.

Parameters:

tag – The tag associated with the waypoints to retrieve.

Returns:

A vector of waypoints that have the specified tag.

std::vector<Point> get_waypoints() const

Get all waypoints within the environment.

This function retrieves a list of all waypoints present in the environment.

Returns:

A vector of waypoints within the environment.

std::shared_ptr<Obstacle> get_obstacle(const std::string &name) const

Get the obstacle with the given name from the environment. Throws an error if no obstacle with the name exists.

Parameters:

name – The name of the obstacle to retrieve.

Returns:

A shared pointer to the obstacle associated with the given name.

std::vector<std::shared_ptr<Obstacle>> get_obstacles_by_tag(const std::string &tag) const

Get all obstacles within the environment that carry the given tag.

Parameters:

tag – The tag associated with the obstacles to retrieve.

Returns:

A vector of shared pointers to the obstacles that have the specified tag.

std::vector<std::shared_ptr<Obstacle>> get_obstacles() const

Get all obstacles within the environment.

This function retrieves a list of all obstacles present in the environment.

Returns:

A vector of shared pointers to the obstacles within the environment.

std::shared_ptr<Camera> get_camera(const std::string &name = "") const

Get a camera from the environment.

Parameters:

name – Optional parameter to specify the name of the camera. If not provided, an empty string defaults to retrieving a camera without a specific name.

Returns:

A shared pointer to the camera associated with the given name, or a default camera if the name is empty.

std::shared_ptr<Obstacle> add_obstacle(std::shared_ptr<Obstacle> obstacle)

Add an obstacle to the environment (and returns the pointer to it)

Parameters:

obstacle – The obstacle to add to the environment.

Returns:

The original shared pointer to the added obstacle.

std::shared_ptr<Obstacle> add_obstacle(const Obstacle &obstacle)

Add an obstacle to the environment (and returns the pointer to it)

Parameters:

obstacle – The obstacle to add to the environment.

Returns:

A shared pointer to the added obstacle.

std::shared_ptr<Obstacle> add_obstacle(const Box &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)

Add a box-shaped obstacle to the environment.

Parameters:

• object – The box-shaped obstacle to add.

• origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).

• color – The color of the obstacle in hexadecimal format (default is “000000”).

• safety_margin – The safety margin around the obstacle (default is 0.0).

Returns:

A shared pointer to the added obstacle.

std::shared_ptr<Obstacle> add_obstacle(const Capsule &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)

Add a capsule-shaped obstacle to the environment.

Parameters:

• object – The capsule-shaped obstacle to add.

• origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).

• color – The color of the obstacle in hexadecimal format (default is “000000”).

• safety_margin – The safety margin around the obstacle (default is 0.0).

Returns:

A shared pointer to the added obstacle.

std::shared_ptr<Obstacle> add_obstacle(const Convex &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)

Add a convex-shaped obstacle to the environment.

Parameters:

• object – The convex-shaped obstacle to add.

• origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).

• color – The color of the obstacle in hexadecimal format (default is “000000”).

• safety_margin – The safety margin around the obstacle (default is 0.0).

Returns:

A shared pointer to the added obstacle.

std::shared_ptr<Obstacle> add_obstacle(const ConvexVector &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)

Add a vector of convex-shaped obstacles to the environment.

Parameters:

• object – The vector of convex-shaped obstacles to add.

• origin – The initial pose of the obstacles in the environment (default is Frame::Identity()).

• color – The color of the obstacles in hexadecimal format (default is “000000”).

• safety_margin – The safety margin around the obstacles (default is 0.0).

Returns:

A shared pointer to the added obstacle.

std::shared_ptr<Obstacle> add_obstacle(const Cylinder &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)

Add a cylinder-shaped obstacle to the environment.

Parameters:

• object – The cylinder-shaped obstacle to add.

• origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).

• color – The color of the obstacle in hexadecimal format (default is “000000”).

• safety_margin – The safety margin around the obstacle (default is 0.0).

Returns:

A shared pointer to the added obstacle.

std::shared_ptr<Obstacle> add_obstacle(const DepthMap &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)

Add a depth map-shaped obstacle to the environment.

Parameters:

• object – The depth map-shaped obstacle to add.

• origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).

• color – The color of the obstacle in hexadecimal format (default is “000000”).

• safety_margin – The safety margin around the obstacle (default is 0.0).

Returns:

A shared pointer to the added obstacle.

std::shared_ptr<Obstacle> add_obstacle(const Sphere &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)

Add a sphere-shaped obstacle to the environment.

Parameters:

• object – The sphere-shaped obstacle to add.

• origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).

• color – The color of the obstacle in hexadecimal format (default is “000000”).

• safety_margin – The safety margin around the obstacle (default is 0.0).

Returns:

A shared pointer to the added obstacle.

std::shared_ptr<Obstacle> add_obstacle(const std::string &name, const Box &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)

Add an obstacle with a name to the environment.

Parameters:

• name – The name to assign to the obstacle.

• object – The obstacle to add (of a specific type, e.g., Box, Capsule, etc.).

• origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).

• color – The color of the obstacle in hexadecimal format (default is “000000”).

• safety_margin – The safety margin around the obstacle (default is 0.0).

Returns:

A shared pointer to the added obstacle.

std::shared_ptr<Obstacle> add_obstacle(const std::string &name, const Capsule &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)

Overload for adding an obstacle with a name and various shapes.

Parameters:

• name – The name to assign to the obstacle.

• object – The obstacle to add (of a specific type, e.g., Capsule, Convex, etc.).

• origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).

• color – The color of the obstacle in hexadecimal format (default is “000000”).

• safety_margin – The safety margin around the obstacle (default is 0.0).

Returns:

A shared pointer to the added obstacle.

std::shared_ptr<Obstacle> add_obstacle(const std::string &name, const Convex &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)

Add a convex-shaped obstacle with a name to the environment.

Parameters:

• name – The name to assign to the obstacle.

• object – The convex-shaped obstacle to add.

• origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).

• color – The color of the obstacle in hexadecimal format (default is “000000”).

• safety_margin – The safety margin around the obstacle (default is 0.0).

Returns:

A shared pointer to the added obstacle.

std::shared_ptr<Obstacle> add_obstacle(const std::string &name, const ConvexVector &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)

Add a vector of convex-shaped obstacles with a name to the environment.

Parameters:

• name – The name to assign to the obstacle.

• object – The vector of convex-shaped obstacles to add.

• origin – The initial pose of the obstacles in the environment (default is Frame::Identity()).

• color – The color of the obstacles in hexadecimal format (default is “000000”).

• safety_margin – The safety margin around the obstacles (default is 0.0).

Returns:

A shared pointer to the added obstacle.

std::shared_ptr<Obstacle> add_obstacle(const std::string &name, const Cylinder &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)

Add a cylinder-shaped obstacle with a name to the environment.

Parameters:

• name – The name to assign to the obstacle.

• object – The cylinder-shaped obstacle to add.

• origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).

• color – The color of the obstacle in hexadecimal format (default is “000000”).

• safety_margin – The safety margin around the obstacle (default is 0.0).

Returns:

A shared pointer to the added obstacle.

std::shared_ptr<Obstacle> add_obstacle(const std::string &name, const DepthMap &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)

Add a depth map-shaped obstacle with a name to the environment.

Parameters:

• name – The name to assign to the obstacle.

• object – The depth map-shaped obstacle to add.

• origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).

• color – The color of the obstacle in hexadecimal format (default is “000000”).

• safety_margin – The safety margin around the obstacle (default is 0.0).

Returns:

A shared pointer to the added obstacle.

std::shared_ptr<Obstacle> add_obstacle(const std::string &name, const Sphere &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)

Add a sphere-shaped obstacle with a name to the environment.

Parameters:

• name – The name to assign to the obstacle.

• object – The sphere-shaped obstacle to add.

• origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).

• color – The color of the obstacle in hexadecimal format (default is “000000”).

• safety_margin – The safety margin around the obstacle (default is 0.0).

Returns:

A shared pointer to the added obstacle.

void remove_obstacle(const std::shared_ptr<Obstacle> &obstacle)

Removes the given obstacles from the environment and from all collision checking.

Parameters:

obstacle – The obstacle to remove from the environment.

void remove_obstacle(const std::string &name)

Removes all obstacles with the given name from the environment and from all collision checking.

Parameters:

name – The name of the obstacle to remove from the environment.

void update_fixed_obstacles()

Updates all fixed obstacles for the internal collision checking. This should be called after changing e.g. the position or size of an obstacle.

void update_depth_map(const std::shared_ptr<Obstacle> &obstacle)

Updates the depths matrix of a given depth map obstacle for the internal collision checking.

Parameters:

obstacle – The obstacle to update the depths map for.

void update_joint_position(const std::shared_ptr<Robot> &robot, const Config &joint_position)

Updates the joint position of the given robot for the internal collision checking.

Parameters:

• robot – The robot to update the joint position for.

• joint_position – The new joint position to set for the robot.

bool check_collision(const std::shared_ptr<Robot> &robot, const Config &joint_position, CollisionDetail &detail)

Check if a joint position is in collision.

Parameters:

• robot – The robot to check the joint position for.

• joint_position – The joint position to check for collision.

• detail – [out] The collision detail to fill in case of a collision.

Returns:

True if the joint position is in collision, false otherwise.

bool check_collision(const std::shared_ptr<Robot> &robot, const Config &joint_position)

Check if a joint position is in collision.

Parameters:

• robot – The robot to check the joint position for.

• joint_position – The joint position to check for collision.

Returns:

True if the joint position is in collision, false otherwise.

bool check_collision(const Config &joint_position)

Check if a joint position is in collision.

Parameters:

joint_position – The joint position to check for collision.

Returns:

True if the joint position is in collision, false otherwise.

bool check_collision(const std::shared_ptr<Robot> &robot, const CartesianWaypoint &waypoint)

Check if there exists a collision-free inverse kinematics for the Cartesian position.

Parameters:

• robot – The robot to check the Cartesian position for.

• waypoint – The Cartesian position to check for collision.

Returns:

True if there exists a collision-free inverse kinematics for the Cartesian position, false otherwise.

bool check_collision(const std::shared_ptr<Robot> &robot, const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)

Check if there exists a collision-free inverse kinematics for the given robot and end-effector pose.

Parameters:

• robot – The robot to check for collision-free inverse kinematics.

• tcp – The end-effector pose (transformation) to check for collision.

• reference_config – Optional reference configuration for inverse kinematics of the end-effector pose.

Returns:

True if there exists a collision-free inverse kinematics for the given end-effector pose, false otherwise.

bool check_collision(const CartesianWaypoint &waypoint)

Check if there exists a collision-free inverse kinematics for the Cartesian position.

Parameters:

waypoint – The Cartesian position to check for collision.

Returns:

True if there exists a collision-free inverse kinematics for the Cartesian position, false otherwise.

bool check_collision(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)

Check if there exists a collision-free inverse kinematics for the given end-effector pose.

Parameters:

• tcp – The end-effector pose to check for collision.

• reference_config – Optional reference configuration for inverse kinematics of the end-effector pose.

Returns:

True if there exists a collision-free inverse kinematics for the given end-effector pose, false otherwise.

std::optional<Config> get_collision_free_joint_position_nearby(const Config &joint_position, const std::shared_ptr<Robot> &robot = nullptr)

Calculate a collision free joint position close to the reference position.

Parameters:

• joint_position – The reference joint position to find a collision free joint position close to.

• robot – The robot to find a collision free joint position for.

Returns:

An optional containing the collision free joint position close to the reference position, or std::nullopt if no such joint position exists.

Public Members

std::map<std::string, std::shared_ptr<Robot>> robots

All robots used for collision checking with default parameters.

std::map<std::string, Point> waypoints

All robots used for collision checking with default parameters.

std::vector<std::shared_ptr<Obstacle>> obstacles

All static obstacles in the environment, owning the data.

std::map<std::string, std::shared_ptr<Camera>> cameras

All cameras in the environment.