Base class

Base class#

#include "navground/core/behavior.h"

class Behavior : public virtual navground::core::HasRegister<Behavior>, protected navground::core::TrackChanges #

This class describes a generic behavior to reach a target position avoiding collision.

Users should not instantiate this class, which will not make the agent move, but one of its concrete sub-classes equipped with the desired navigation algorithms, which are implemented by overriding any of the (virtual) methods listed in compute_cmd_internal.

The following describes the typical usage of a behavior.

At initialization

select the concrete behavior, the agent’s size (agents are shaped like discs), and Kinematics;
configure the generic parameters: set_optimal_speed, set_horizon, set_safety_margin set_rotation_tau, and set_heading_behavior;
configure the specific parameters of the concrete behavior.

At regular time intervals

update the agent’s state with set_pose and set_twist (or other convenience methods)
update the target with set_target
update the environment state get_environment_state
ask for a control commands by calling compute_cmd
actuate the control commands through user code

Subclassed by navground::core::DummyBehavior, navground::core::HLBehavior, navground::core::HRVOBehavior, navground::core::ORCABehavior

Public Types

enum class Heading#

Different behavior variants for the angular motion when it is no constrained by the kinematics or already specified by the current target.

They ability to apply them depends on the kinematics and on the current target.

Values:

enumerator idle#: keep the same orientation

enumerator target_point#: turn towards the target position (if any)

enumerator target_angle#: turn towards the target orientation (if any)

enumerator target_angular_speed#: follow a target angular speed (if any)

enumerator velocity#: turn towards the velocity direction. This is the only behavior available to constrained kinematics.

Public Functions

inline Behavior(std::shared_ptr<Kinematics> kinematics = nullptr, ng_float_t radius = 0)#

Constructs a new instance.

Parameters:

kinematics – The kinematics of the agent.
radius – The radius of the agent.

inline std::shared_ptr<Kinematics> get_kinematics() const#

Gets the kinematics.

Returns:: The agent’s kinematics.

inline void set_kinematics(std::shared_ptr<Kinematics> value)#

Sets the kinematics.

Parameters:: value – [in] The desired kinematics.

inline ng_float_t get_radius() const#

Gets the radius of the agent.

Returns:: The agent’s radius.

inline void set_radius(ng_float_t value)#

Sets the radius of the agent.

Parameters:: value – A positive value.

inline ng_float_t get_max_speed() const#

Gets the maximal speed.

Returns:: The maximal speed returned from Kinematics::get_max_speed

inline void set_max_speed(ng_float_t value)#

Sets the maximal speed.

Parameters:: value – [in] The value to pass to Kinematics::set_max_speed

inline Radians get_max_angular_speed() const#

Gets the maximal angular speed speed.

Returns:: The maximal angular speed from Kinematics::get_max_angular_speed

inline void set_max_angular_speed(Radians value)#

Sets the maximal angular speed speed.

Parameters:: value – [in] The value to pass to Kinematics::set_max_angular_speed.

inline ng_float_t get_optimal_speed() const#

Gets the desired optimal speed.

If not configured through set_optimal_speed, it return get_max_speed.

Returns:: The desired optimal speed.

inline void set_optimal_speed(ng_float_t value)#

Sets the desired optimal speed.

Parameters:: value – [in] A positive linear speed.

inline ng_float_t feasible_speed(ng_float_t value) const#

Clamp a speed in the range of feasible values given by the kinematics.

Parameters:: value – [in] The desired value
Returns:: the nearest feasible value

inline ng_float_t feasible_angular_speed(ng_float_t value) const#

Clamp an angular speed in the range of feasible values given by the kinematics.

Parameters:: value – [in] The desired value
Returns:: the nearest feasible value

Twist2 feasible_twist(const Twist2 &value) const#

Clamp an twist in the range of feasible values given by the kinematics.

Parameters:: value – [in] The desired value
Returns:: the nearest feasible value

Twist2 feasible_twist_from_current(const Twist2 &value, ng_float_t time_step) const#

Computes the nearest feasible twist given the kinematics and the current twist over a time step.

Parameters:

value – [in] The desired value
time_step – [in] The time step

Returns:

the nearest feasible value

inline Radians get_optimal_angular_speed() const#

Gets the desired optimal angular speed.

If not configured through set_optimal_angular_speed, it will return set to get_max_angular_speed.

Returns:: The desired optimal angular speed.

inline void set_optimal_angular_speed(Radians value)#

Sets the optimal angular speed.

Parameters:: value – [in] A positive angular speed in radians/time unit.

inline ng_float_t get_rotation_tau() const#

Gets the relaxation time to rotate towards a desired orientation.

The default behaviors applies a P control to rotations, e.g., \(\omega = \frac{\delta \theta}{\tau_\textrm{rot}}\)

Returns:: The rotation relaxation time.

inline void set_rotation_tau(ng_float_t value)#

Sets the relaxation time to rotate towards a desired orientation. See get_rotation_tau.

Parameters:: value – [in] The rotation relaxation time. Set it to a zero or negative to rotate as fast as possible.

inline ng_float_t get_safety_margin() const#

Gets the minimal safety margin to keep away from obstacles.

Returns:: The safety margin.

inline void set_safety_margin(ng_float_t value)#

Sets the safety margin to keep away from obstacles.

Parameters:: value – [in] A positive value.

inline ng_float_t get_horizon() const#

Gets the horizon: the size of the portion of environment around the agent considered when computing possible collisions. Larger values generally lead to a more computationally expensive compute_cmd but fewer deadlocks and less jamming.

Returns:: The horizon.

inline void set_horizon(ng_float_t value)#

Sets the horizon, see get_horizon.

Parameters:: value – [in] A positive value.

inline bool get_assume_cmd_is_actuated() const#

Gets whether to assume that the compute command will be actuated as it is.

If set, Behavior will assume that the control command computed by compute_cmd be actuated, therefore setting set_actuated_twist to that value. If not set, the user should set Behavior::set_actuated_twist before querying for a new control commands: some behavior use the old actuated control command to compute smoother controls.

Returns:: True if it assumes that the command will be actuated as it is.

inline void set_assume_cmd_is_actuated(bool value)#

Sets whether to assume that the compute command will be actuated as it is.

See also

get_assume_cmd_is_actuated.

Parameters:: value – [in] The desired value.

inline ng_float_t get_path_tau() const#

Gets the path relaxation time.

Returns:: The time.

inline void set_path_tau(ng_float_t value)#

Sets the path relaxation time, see get_path_tau.

Parameters:: value – [in] A positive value.

inline ng_float_t get_path_look_ahead() const#

Gets the path look-ahead distance.

Returns:: The distance.

inline void set_path_look_ahead(ng_float_t value)#

Sets the path look-ahead distance, see get_path_look_ahead.

Parameters:: value – [in] A positive value.

inline Pose2 get_pose() const#

Gets the current pose in the world-fixed frame.

Returns:: The current pose.

inline void set_pose(const Pose2 &value)#

Sets the current pose in the world-fixed frame.

Parameters:: value – [in] The desired value

inline Vector2 get_position() const#

Convenience method to get the current position in the world-fixed frame. See get_pose.

Returns:: The position.

inline void set_position(const Vector2 &value)#

Convenience method to set the current position in the world-fixed frame. See set_pose.

Parameters:: value – [in] The desired value

inline Radians get_orientation() const#

Convenience method to get the current orientation. See get_pose.

Returns:: The current orientation.

inline void set_orientation(Radians value)#

Convenience method to set the current orientation. See set_pose.

Parameters:: value – [in] The desired value

inline Twist2 get_twist(Frame frame = Frame::absolute) const#

Gets the current twist.

Parameters:: frame – [in] The desired frame of reference.
Returns:: The current twist.

inline void set_twist(const Twist2 &value)#

Sets the current twist.

Parameters:: value – [in] The desired value

inline Vector2 get_velocity(Frame frame = Frame::absolute) const#

Convenience method to get the current velocity. See get_twist.

Parameters:: frame – [in] The desired frame of reference.
Returns:: The velocity.

inline void set_velocity(const Vector2 &value, Frame frame = Frame::absolute)#

Convenience method to set the current velocity. See set_twist.

Parameters:

value – [in] The velocity
frame – [in] The desired frame of reference.

inline ng_float_t get_speed() const#

Convenience method to get the current speed. See get_twist.

Returns:: The current speed.

inline Radians get_angular_speed() const#

Convenience method to get the current the angular speed.

Returns:: The current angular speed.

inline WheelSpeeds get_wheel_speeds() const#

Convenience method to get the current wheel speeds. See get_twist.

Returns:: The wheel speeds.

inline void set_wheel_speeds(const WheelSpeeds &value)#

Convenience method to set the current wheel speeds. See set_twist.

Parameters:: value – [in] The wheel speeds

inline Twist2 get_actuated_twist(Frame frame = Frame::absolute) const#

Gets the last actuated twist.

Parameters:: frame – [in] The desired frame of reference.
Returns:: The actuated twist.

inline void set_actuated_twist(const Twist2 &value)#

Sets the last actuated twist.

Parameters:: value – [in] The desired value

inline WheelSpeeds get_actuated_wheel_speeds() const#

Convenience method to get the last actuated wheel speeds from actuated_twist.

If the agent is not wheeled (Kinematics::is_wheeled), an empty vector is returned.

Returns:: The actuated wheel speeds.

void actuate(const Twist2 &twist_cmd, ng_float_t time_step, bool enforce_feasibility = false)#

Actuate a twist command, integrating using Pose2::integrate.

Parameters:

twist_cmd – [in] The twist
time_step – [in] The time step
enforce_feasibility – [in] Whether to enforce that the command is kinematically feasible

inline void actuate(ng_float_t time_step)#

Convenience method to actuate the stored actuated twist command,.

Parameters:: time_step – [in] The time step

void set_state_from(const Behavior &other)#

Clone the state of this behavior from another behavior.

Parameters:: other – [in] The other behavior

inline Heading get_heading_behavior() const#

Gets the heading behavior:

Returns:: The heading behavior.

inline void set_heading_behavior(Heading value)#

Sets the heading behavior, see get_heading_behavior. When the kinematics has only 2 degrees of freedom (see Kinematics::dof()), the only available behavior is Heading::velocity.

Parameters:: value – [in] The desired value

inline Target get_target() const#

Gets the target.

Returns:: The target.

inline Target &get_target_ref()#

Gets a reference to the target.

Returns:: The target.

inline void set_target(const Target &value)#

Sets the target.

Parameters:: value – [in] The desired value

Twist2 compute_cmd(ng_float_t time_step, std::optional<Frame> frame = std::nullopt, bool enforce_feasibility = false)#

Query the behavior to get a control command.

Before calling this method, update the state using methods such as set_pose, and set_twist and set the target set_target.

Behaviors may use caching to speed up the next queries if the state does not change.

Modulations are applied as wrappers/context modifiers: right before evaluating the behavior in compute_cmd_internal, it will call BehaviorModulation::pre for any modulation in get_modulations, and right after BehaviorModulation::post but in reverse order.

Parameters:

time_step – [in] The control time step. Not all behavior use it but some may use it, for example, to limit accelerations.
frame – [in] An optional desired frame of reference for the command.
enforce_feasibility – [in] Whether to enforce that the command is kinematically feasible

Returns:

The control command as a twist in the specified frame.

inline Vector2 get_desired_velocity() const#

Gets the last computed desired velocity.

Returns:: The desired velocity (only valid if computed) in Frame::absolute

inline Twist2 to_absolute(const Twist2 &value) const#

Transform a twist to Frame::absolute.

Parameters:: value – [in] The original twist
Returns:: The same twist in Frame::absolute (unchanged if already in Frame::absolute)

inline Twist2 to_relative(const Twist2 &value) const#

Transform a twist to Frame::relative.

Parameters:: value – [in] The original twist
Returns:: The same twist in Frame::relative (unchanged if already in Frame::relative)

inline Vector2 to_relative(const Vector2 &value) const#

Transform a vector (e.g., a velocity) from Frame::absolute to Frame::relative.

Parameters:: value – [in] The vector in Frame::absolute
Returns:: The vector in Frame::relative

inline Twist2 to_frame(const Twist2 &value, Frame frame) const#

Convert a twist to a reference frame.

Parameters:

value – [in] The original twist.
frame – [in] The desired frame of reference

Returns:

The twist in the desired frame of reference.

inline WheelSpeeds wheel_speeds_from_twist(const Twist2 &value) const#

Convenience method to transform from twist to wheel speeds.

If the agent is not wheeled (Kinematics::is_wheeled), an empty vector is returned.

Parameters:: value – [in] The twist
Returns:: The corresponding wheel speeds.

inline Twist2 twist_from_wheel_speeds(const WheelSpeeds &value) const#

Convenience method to transform from wheel speeds to twist.

If the agent is not wheeled (Kinematics::is_wheeled), an zero twist is returned.

Parameters:: value – [in] The wheel speeds
Returns:: The corresponding twist.

inline virtual EnvironmentState *get_environment_state()#

Gets the environment state.

Returns:: The environment state.

bool check_if_target_satisfied() const#

Check if the current target has been satisfied.

Returns:: True if the current target has been satisfied.

ng_float_t estimate_time_until_target_satisfied() const#

Estimate how much time before the target is satisfied.

Returns:: A positive value if the target is not yet satisfied, else 0.

bool is_stuck() const#

Determines if the agent is stuck: if should move but it is still.

Returns:: True if stuck, False otherwise.

ng_float_t get_efficacy() const#

Gets the efficacy: the projection of the current velocity on the ideal velocity (ignoring obstacles) towards the target.

A value of 1.0 denotes ideal efficacy, value of 0.0 that the agent is stuck.

Returns:: The efficacy.

inline std::vector<std::shared_ptr<BehaviorModulation>> &get_modulations()#

Gets the modulations applied to this behavior.

Returns:: The modulations.

inline const std::vector<std::shared_ptr<BehaviorModulation>> &get_modulations() const#

Gets the modulations applied to this behavior.

Returns:: The modulations.

inline void add_modulation(const std::shared_ptr<BehaviorModulation> &value)#

Adds a modulation.

Parameters:: value – [in] The modulation to add

inline void remove_modulation(const std::shared_ptr<BehaviorModulation> &value)#

Removes a modulation.

Parameters:: value – [in] The modulation to remove

inline void clear_modulations()#: Removes all modulations.

std::optional<Vector2> get_target_position(Frame frame) const#

Returns the target’s position if not satisfied, else null.

Parameters:: frame – [in] The desired frame
Returns:: The position in the desired frame

std::optional<ng_float_t> get_target_orientation(Frame frame) const#

Returns the target’s orientation if not satisfied, else null.

Parameters:: frame – [in] The desired frame
Returns:: The orientation in the desired frame

std::optional<Vector2> get_target_direction(Frame frame) const#

Returns the direction towards the target’s position if not satisfied, or the target’s direction if defined, else null.

Parameters:: frame – [in] The desired frame
Returns:: The normalized direction in the desired frame

std::optional<ng_float_t> get_target_distance(bool ignore_tolerance = false) const#

Returns the distance to the target point, if valid, else null.

Parameters:: ignore_tolerance – [in] Whether to ignore the target tolerance
Returns:: The distance.

Vector2 get_target_velocity(Frame frame) const#

Gets the current target velocity.

        Multiplication of \ref get_target_direction
        by \ref get_target_speed.
        Returns a zero vector if the target direction is undefined.

Parameters:: frame – [in] The desired frame
Returns:: The velocity in the desired frame.

ng_float_t get_target_speed() const#

Returns the nearest feasible speed to target’s speed, if defined, else to the behavior own optimal speed.

Returns:: The speed

ng_float_t get_target_angular_speed() const#

Returns the nearest feasible angular speed to target’s angular speed, if defined, else to the behavior own optimal angular speed.

Returns:: The angular speed.

Public Members

SocialMargin social_margin#: The behavior social margin modulation

Public Static Functions

static inline Heading heading_from_string(const std::string &value)#

Get a named Heading mode.

Parameters:: value – The name of the Heading mode
Returns:: The corresponding Heading

static inline std::string heading_to_string(const Heading &value)#

Get the name of an Heading mode.

Parameters:: value – [in] The Heading mode
Returns:: The name of the Heading mode.

Public Static Attributes

static ng_float_t default_rotation_tau = 0.5#: Default rotation tau

static ng_float_t default_horizon = 5#: Default horizon

static ng_float_t default_safety_margin = 0#: Default safety margin

static ng_float_t default_path_tau = 0.5#: Default safety margin

static ng_float_t default_path_look_ahead = 1.0#: Default safety margin

Protected Functions

virtual Twist2 compute_cmd_internal(ng_float_t time_step)#

Computes the control command.

This is a virtual protected that sub-classes may override to define a behavior and is not part of the public API. Users should call compute_cmd instead.

The base implementation checks for a valid target:

position along a path => calls cmd_twist_along_path
pose => calls cmd_twist_towards_pose
position => calls cmd_twist_towards_point
orientation => calls cmd_twist_towards_orientation
velocity => calls cmd_twist_towards_velocity
angular speed => calls cmd_twist_towards_angular_speed
else => calls cmd_twist_towards_stopping.

To specialize a Behavior, users may override this or any of the methods listed above. They may also override the following internal methods:

desired_velocity_towards_point
desired_velocity_towards_velocity
twist_towards_velocity

The command should be kinematically feasible. The base implementation does not guaranteed it, delegating the check to the methods listed above.

Parameters:: time_step – [in] The time step
Returns:: The command in the desired frame.

virtual Twist2 cmd_twist_along_path(Path &path, ng_float_t speed, ng_float_t time_step)#

Compute a command to follow a path.

Users may override it to specialize a Behavior.

The base implementation uses a carrot planner to computes a target velocity, and then calls cmd_twist_towards_velocity.

Parameters:

path – The desired path in world-fixed frame
speed – [in] The desired speed
time_step – [in] The time step

Returns:

The command twist.

virtual Twist2 cmd_twist_towards_pose(const Pose2 &pose, ng_float_t speed, Radians angular_speed, ng_float_t time_step)#

Computes a command to move towards a desired pose.

Users may override it to specialize a Behavior.

The base implementation ignores the orientation, calling call cmd_twist_towards_point, to first reach the desired position and then rotate in place to reach the desired orientation.

Parameters:

pose – [in] The desired pose in world-fixed frame
speed – [in] The desired speed
angular_speed – [in] The desired angular speed
time_step – [in] The time step

Returns:

The command twist.

virtual Twist2 cmd_twist_towards_point(const Vector2 &point, ng_float_t speed, ng_float_t time_step)#

Computes a command to move towards a desired position.

Users may override it to specialize a Behavior.

The base implementation:

calls desired_velocity_towards_point to compute a desired velocity
calls twist_towards_velocity to compute a desired twist
returns the nearest feasible twist.

Parameters:

point – [in] The desired position in world-fixed frame
speed – [in] The desired speed
time_step – [in] The time step

Returns:

The command twist.

virtual Twist2 cmd_twist_towards_velocity(const Vector2 &velocity, ng_float_t time_step)#

Computes a command to turn towards a desired orientation.

Users may override it to specialize a Behavior.

The base implementation:

calls desired_velocity_towards_velocity to compute a desired velocity
calls twist_towards_velocity to compute a desired twist
returns the nearest feasible twist.

Parameters:

velocity – [in] The velocity in world-fixed frame
time_step – [in] The time step

Returns:

The command twist.

virtual Twist2 cmd_twist_towards_orientation(Radians orientation, ng_float_t angular_speed, ng_float_t time_step)#

Computes a command to turn towards a desired orientation.

Users may override it to specialize a Behavior.

The base implementation:

computes a desired angular speed to rotate in get_rotation_tau time towards the desired orientation
calls cmd_twist_towards_angular_speed

Parameters:

orientation – [in] The desired orientation in world-fixed frame
angular_speed – [in] The desired angular speed
time_step – [in] The time step

Returns:

The command twist.

virtual Twist2 cmd_twist_towards_angular_speed(ng_float_t angular_speed, ng_float_t time_step)#

Computes a command to turn at a desired angular speed.

Users may override it to specialize a Behavior.

The base implementation returns the nearest feasible twist to a twist with null velocity and desired angular speed.

Parameters:

angular_speed – [in] The desired angular speed
time_step – [in] The time step

Returns:

The command twist.

virtual Twist2 cmd_twist_towards_stopping(ng_float_t time_step)#

Computes a command to stop.

Users may override it to specialize a Behavior.

The base implementation returns the null twist.

Parameters:: time_step – [in] The time step
Returns:: The command twist.

virtual Vector2 desired_velocity_towards_point(const Vector2 &point, ng_float_t speed, ng_float_t time_step)#

Computes a control velocity towards a desired position.

Users may override it to specialize a Behavior.

The base implementation returns a null vector.

Parameters:

point – [in] The desired point in world-fixed frame
speed – [in] The desired speed
time_step – [in] The time step

Returns:

A velocity in world-fixed frame

virtual Vector2 desired_velocity_towards_velocity(const Vector2 &velocity, ng_float_t time_step)#

Computes a control velocity towards a desired velocity.

Users may override it to specialize a Behavior.

The base implementation returns a null vector.

Parameters:

velocity – [in] The desired velocity in world-fixed frame
time_step – [in] The time step

Returns:

A velocity in world-fixed frame

virtual Twist2 twist_towards_velocity(const Vector2 &velocity)#

Computes a twist to reach a control velocity.

This method assumes that the control velocity is safe. Use it to convert a velocity to a twist once you have computed (using, e.g., desired_velocity_towards_point or desired_velocity_towards_velocity) a safe control velocity.

Parameters:: velocity – [in] The control velocity in world-fixed frame
Returns:: A command twist.

Base class

Contents

Base class#