class mrpt::nav::CParameterizedTrajectoryGenerator

Python API: mrpt.nav.CParameterizedTrajectoryGenerator

Overview

Base class for all Parameterized Trajectory Generators (PTGs).

A PTG defines a family of feasible trajectories, all starting at the robot pose (0,0,0), parameterized by a single real number \(\alpha \in (-\pi,\pi]\) discretized into getPathCount() paths indexed by k. Each path k is sampled at discrete step s, which map to a pose (getPathPose()) and a traversed arc length (getPathDist()).

This is what turns obstacle avoidance for a kinematically-constrained robot into a holonomic problem: a Workspace obstacle point is mapped by updateTPObstacle() into the collision-free length of every path, giving a “polar plot” (the TP-Space, or Trajectory Parameter Space) in which the robot can be treated as a point that moves freely in any direction \(\alpha\). The holonomic method (see Holonomic navigation methods) picks a direction there, and directionToMotionCommand() maps it back to a velocity command.

Distances are in “pseudometers”: real path lengths in meters, which callers normalize to [0,1] by getRefDistance().

Instances are normally built by class name via CreatePTG(), then configured with loadFromConfigFile() and finally initialize() d (which is where collision look-up tables get built or loaded from cache, see derived classes). Use the ptg-configurator app to tune parameters interactively.

Reference: J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, “Extending Obstacle Avoidance Methods through Multiple Parameter-Space Transformations”, Autonomous Robots, vol. 24, no. 1, 2008. http://ingmec.ual.es/~jlblanco/papers/blanco2008eoa_DRAFT.pdf

#include <mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h>

class CParameterizedTrajectoryGenerator:
    public mrpt::serialization::CSerializable,
    public mrpt::config::CLoadableOptions
{
public:
    // typedefs

    typedef std::shared_ptr<CParameterizedTrajectoryGenerator> Ptr;
    typedef std::shared_ptr<const CParameterizedTrajectoryGenerator> ConstPtr;

    // structs

    struct TNavDynamicState;

    // construction

    CParameterizedTrajectoryGenerator();

    // methods

    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    virtual std::string getDescription() const = 0;

    virtual std::optional<std::pair<int, double>> inverseMap_WS2TP(
        double x,
        double y,
        double tolerance_dist = 0.10
        ) const = 0;

    virtual bool PTG_IsIntoDomain(double x, double y) const;
    virtual bool isBijectiveAt(] uint16_t k, ] uint32_t step) const;
    virtual mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(uint16_t k) const = 0;
    virtual mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const = 0;
    virtual void setRefDistance(const double refDist);
    virtual size_t getPathStepCount(uint16_t k) const = 0;
    virtual mrpt::math::TPose2D getPathPose(uint16_t k, uint32_t step) const = 0;
    virtual mrpt::math::TTwist2D getPathTwist(uint16_t k, uint32_t step) const;
    virtual double getPathDist(uint16_t k, uint32_t step) const = 0;
    virtual double getPathStepDuration() const = 0;
    virtual double getMaxLinVel() const = 0;
    virtual double getMaxAngVel() const = 0;
    virtual std::optional<uint32_t> getPathStepForDist(uint16_t k, double dist) const = 0;
    bool getPathStepForDist(uint16_t k, double dist, uint32_t& out_step) const;
    uint32_t getPathStepForDistClamped(uint16_t k, double dist) const;
    virtual void updateTPObstacle(double ox, double oy, std::vector<double>& tp_obstacles) const = 0;
    virtual void updateTPObstacleSingle(double ox, double oy, uint16_t k, double& tp_obstacle_k) const = 0;

    virtual void updateTPObstacles(
        const float* xs,
        const float* ys,
        std::size_t n,
        std::vector<double>& tp_obstacles
        ) const;

    virtual void loadDefaultParams();
    virtual bool supportVelCmdNOP() const;
    virtual bool supportSpeedAtTarget() const;
    virtual double maxTimeInVelCmdNOP(int path_k) const;
    virtual double getActualUnloopedPathLength(] uint16_t k) const;
    virtual double evalPathRelativePriority(] uint16_t k, ] double target_distance) const;
    virtual double getMaxRobotRadius() const = 0;
    virtual bool isPointInsideRobotShape(const double x, const double y) const = 0;
    virtual double evalClearanceToRobotShape(const double ox, const double oy) const = 0;
    void updateNavDynamicState(const TNavDynamicState& newState, const bool force_update = false);
    const TNavDynamicState& getCurrentNavDynamicState() const;

    void initialize(
        const std::string& cacheFilename = std::string(),
        const bool verbose = true
        );

    void deinitialize();
    bool isInitialized() const;
    uint16_t getAlphaValuesCount() const;
    uint16_t getPathCount() const;
    double index2alpha(uint16_t k) const;
    uint16_t alpha2index(double alpha) const;
    double getRefDistance() const;
    void initTPObstacles(std::vector<double>& TP_Obstacles) const;

    void initTPObstacleSingle(
        uint16_t k,
        double& TP_Obstacle_k
        ) const;

    double getScorePriority() const;
    void setScorePriority(double prior);
    void setScorePriorty(double prior);
    unsigned getClearanceStepCount() const;
    void setClearanceStepCount(const uint16_t res);
    unsigned getClearanceDecimatedPaths() const;
    void setClearanceDecimatedPaths(const uint16_t num);

    virtual void renderPathAsSimpleLine(
        const uint16_t k,
        mrpt::viz::CSetOfLines& gl_obj,
        const double decimate_distance = 0.1,
        const double max_path_distance = -1.0
        ) const;

    bool debugDumpInFiles(const std::string& ptg_name) const;
    virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase& cfg, const std::string& sSection);
    virtual void saveToConfigFile(mrpt::config::CConfigFileBase& target, const std::string& section) const;
    virtual void add_robotShape_to_setOfLines(mrpt::viz::CSetOfLines& gl_shape, const mrpt::poses::CPose2D& origin = mrpt::poses::CPose2D()) const = 0;
    void initClearanceDiagram(ClearanceDiagram& cd) const;
    void updateClearance(const double ox, const double oy, ClearanceDiagram& cd) const;

    virtual void evalClearanceSingleObstacle(
        const double ox,
        const double oy,
        const uint16_t k,
        ClearanceDiagram::dist2clearance_t& inout_realdist2clearance,
        bool treat_as_obstacle = true
        ) const;

    static CParameterizedTrajectoryGenerator::Ptr CreatePTG(
        const std::string& ptgClassName,
        const mrpt::config::CConfigFileBase& cfg,
        const std::string& sSection,
        const std::string& sKeyPrefix
        );

    static std::string getOutputDebugPathPrefix();
    static void setOutputDebugPathPrefix(const std::string& path);
    static std::string& OUTPUT_DEBUG_PATH_PREFIX();

    static double Index2alpha(
        uint16_t k,
        const unsigned int num_paths
        );

    static uint16_t Alpha2index(
        double alpha,
        const unsigned int num_paths
        );

    static PTGCollisionBehavior getCollisionBehavior();
    static void setCollisionBehavior(PTGCollisionBehavior behavior);
    static PTGCollisionBehavior& COLLISION_BEHAVIOR();
};

// direct descendants

class CPTG_RobotShape_Circular;
class CPTG_RobotShape_Polygonal;

Inherited Members

public:
    // typedefs

    typedef std::shared_ptr<CObject> Ptr;
    typedef std::shared_ptr<const CObject> ConstPtr;
    typedef std::unique_ptr<CObject> UniquePtr;
    typedef std::unique_ptr<const CObject> ConstUniquePtr;
    typedef std::shared_ptr<CSerializable> Ptr;
    typedef std::shared_ptr<const CSerializable> ConstPtr;

    // methods

    mrpt::rtti::CObject::Ptr duplicateGetSmartPtr() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    virtual CObject* clone() const = 0;
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    CLoadableOptions& operator = (const CLoadableOptions&);
    CLoadableOptions& operator = (CLoadableOptions&&);
    virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase& source, const std::string& section) = 0;
    void loadFromConfigFileName(const std::string& config_file, const std::string& section);
    virtual void saveToConfigFile(mrpt::config::CConfigFileBase& target, const std::string& section) const;
    void saveToConfigFileName(const std::string& config_file, const std::string& section) const;
    void dumpToConsole() const;
    virtual void dumpToTextStream(std::ostream& out) const;

Construction

CParameterizedTrajectoryGenerator()

Default ctor.

Must call loadFromConfigFile() before initialization

Methods

virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const

Returns information about the class of an object in runtime.

virtual std::string getDescription() const = 0

Gets a short textual description of the PTG and its parameters.

virtual std::optional<std::pair<int, double>> inverseMap_WS2TP(
    double x,
    double y,
    double tolerance_dist = 0.10
    ) const = 0

Maps a Workspace point (x,y), in the robot frame, to the TP-Space coordinates of the closest point of the closest trajectory.

Parameters:

x
y	Query point coordinates, relative to the robot frame [m].
tolerance_dist	How far the closest trajectory point may lie from (x,y) for the mapping to be accepted [m].

Returns:

The pair (k path index, distance normalized so that getRefDistance() maps to 1), or std::nullopt if (x,y) is out of the PTG domain within that tolerance.

virtual bool PTG_IsIntoDomain(double x, double y) const

Returns true if (x,y) is within the PTG domain.

The default implementation just calls inverseMap_WS2TP().

virtual bool isBijectiveAt(] uint16_t k, ] uint32_t step) const

Returns true if a given TP-Space point maps to a unique point in Workspace, and viceversa.

Default implementation returns true.

virtual mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(uint16_t k) const = 0

Converts a discretized “alpha” value into a feasible motion command or action.

See derived classes for the meaning of these actions

virtual mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const = 0

Returns an empty kinematic velocity command object of the type supported by this PTG.

Can be queried to determine the expected kinematic interface of the PTG.

virtual size_t getPathStepCount(uint16_t k) const = 0

Access path k ([0,N-1]=>[-pi,pi] in alpha): number of discrete “steps” along the trajectory.

May be actual steps from a numerical integration or an arbitrary small length for analytical PTGs.

See also:

getAlphaValuesCount()

virtual mrpt::math::TPose2D getPathPose(uint16_t k, uint32_t step) const = 0

Access path k ([0,N-1]=>[-pi,pi] in alpha): pose of the vehicle at discrete step step.

See also:

getPathStepCount(), getAlphaValuesCount(), getPathTwist()

virtual mrpt::math::TTwist2D getPathTwist(uint16_t k, uint32_t step) const

Gets velocity (“twist”) for path k ([0,N-1]=>[-pi,pi] in alpha), at vehicle discrete step step.

The default implementation in this base class uses numerical differentiation to estimate the twist (vx,vy,omega). Velocity is given in “global” coordinates, relative to the starting pose of the robot at t=0 for this PTG path.

See also:

getPathStepCount(), getAlphaValuesCount(), getPathPose()

virtual double getPathDist(uint16_t k, uint32_t step) const = 0

Access path k ([0,N-1]=>[-pi,pi] in alpha): traversed distance at discrete step step.

Returns:

Distance in pseudometers (real distance, NOT normalized to [0,1] for [0,refDist])

See also:

getPathStepCount(), getAlphaValuesCount()

virtual double getPathStepDuration() const = 0

Returns the duration (in seconds) of each “step”.

See also:

getPathStepCount()

virtual double getMaxLinVel() const = 0

Returns the maximum linear velocity expected from this PTG [m/s].

virtual double getMaxAngVel() const = 0

Returns the maximum angular velocity expected from this PTG [rad/s].

virtual std::optional<uint32_t> getPathStepForDist(uint16_t k, double dist) const = 0

Access path k ([0,N-1]=>[-pi,pi] in alpha): largest step count for which the traversed distance is <dist

Parameters:

dist	Distance in pseudometers (real distance, NOT normalized to [0,1] for [0,refDist])

Returns:

std::nullopt if no step fulfills the condition for the given trajectory k (e.g. out of reference distance).

See also:

getPathStepCount(), getAlphaValuesCount()

bool getPathStepForDist(uint16_t k, double dist, uint32_t& out_step) const

Deprecated Use the std::optional-returning overload.

Note that out_step is now left untouched when no step fulfills the condition.

uint32_t getPathStepForDistClamped(uint16_t k, double dist) const

Like getPathStepForDist(), but returning the last step of path k instead of failing when dist lies beyond the end of the path.

virtual void updateTPObstacle(
    double ox,
    double oy,
    std::vector<double>& tp_obstacles
    ) const = 0

Updates the radial map of closest TP-Obstacles given a single obstacle point at (ox,oy)

The length of tp_obstacles is not checked for efficiency since this method is potentially called thousands of times per navigation timestap, so it is left to the user responsibility to provide a valid buffer.

tp_obstacles must be initialized with initTPObstacle() before call.

Parameters:

tp_obstacles	A vector of length getAlphaValuesCount(), initialized with initTPObstacles() (collision-free ranges, in “pseudometers”, un-normalized).
ox	Obstacle point (X), relative coordinates wrt origin of the PTG.
oy	Obstacle point (Y), relative coordinates wrt origin of the PTG.

virtual void updateTPObstacleSingle(
    double ox,
    double oy,
    uint16_t k,
    double& tp_obstacle_k
    ) const = 0

Like updateTPObstacle() but for one direction only (k) in TP-Space.

tp_obstacle_k must be initialized with initTPObstacleSingle() before call (collision-free ranges, in “pseudometers”, un-normalized).

virtual void updateTPObstacles(
    const float* xs,
    const float* ys,
    std::size_t n,
    std::vector<double>& tp_obstacles
    ) const

Batched updateTPObstacle() for n obstacle points, in relative coordinates wrt the origin of the PTG: the result is the same as calling updateTPObstacle() for each point.

This default implementation does exactly that; PTGs with a faster batched evaluation override it. tp_obstacles must be initialized with initTPObstacles() before call.

virtual void loadDefaultParams()

Loads a set of default parameters into the PTG.

Users normally will call loadFromConfigFile() instead, this method is provided exclusively for the PTG-configurator tool.

virtual bool supportVelCmdNOP() const

Returns true if it is possible to stop sending velocity commands to the robot and, still, the robot controller will be able to keep following the last sent trajectory (“NOP” velocity commands).

Default implementation returns “false”.

virtual bool supportSpeedAtTarget() const

Returns true if this PTG takes into account the desired velocity at target.

See also:

updateNavDynamicState()

virtual double maxTimeInVelCmdNOP(int path_k) const

Only for PTGs supporting supportVelCmdNOP() : this is the maximum time (in seconds) for which the path can be followed without re-issuing a new velcmd.

Note that this is only an absolute maximum duration, navigation implementations will check for many other conditions. Default method in the base virtual class returns 0.

Parameters:

path_k

Queried path k index [0,N-1]

virtual double getActualUnloopedPathLength(] uint16_t k) const

Returns the actual distance (in meters) of the path, discounting possible circular loops of the path (e.g.

if it comes back to the origin). Default: refDistance

virtual double evalPathRelativePriority(] uint16_t k, ] double target_distance) const

Query the PTG for the relative priority factor (0,1) of this PTG, in comparison to others, if the k-th path is to be selected.

virtual double getMaxRobotRadius() const = 0

Returns an approximation of the robot radius.

virtual bool isPointInsideRobotShape(const double x, const double y) const = 0

Returns true if the point lies within the robot shape.

virtual double evalClearanceToRobotShape(const double ox, const double oy) const = 0

Evals the clearance from an obstacle (ox,oy) in coordinates relative to the robot center.

Zero or negative means collision.

void updateNavDynamicState(const TNavDynamicState& newState, const bool force_update = false)

To be invoked by the navigator before each navigation step, to let the PTG to react to changing dynamic conditions.

See also:

onNewNavDynamicState(), m_nav_dyn_state

void initialize(
    const std::string& cacheFilename = std::string(),
    const bool verbose = true
    )

Must be called after setting all PTG parameters and before requesting converting obstacles to TP-Space, inverseMap_WS2TP(), etc.

void deinitialize()

This must be called to de-initialize the PTG if some parameter is to be changed.

After changing it, call initialize again

bool isInitialized() const

Returns true if initialize() has been called and there was no errors, so the PTG is ready to be queried for paths, obstacles, etc.

uint16_t getAlphaValuesCount() const

Get the number of different, discrete paths in this family.

uint16_t getPathCount() const

Get the number of different, discrete paths in this family.

double index2alpha(uint16_t k) const

Alpha value for the discrete corresponding value.

See also:

alpha2index

uint16_t alpha2index(double alpha) const

Discrete index value for the corresponding alpha value.

See also:

index2alpha

void initTPObstacles(std::vector<double>& TP_Obstacles) const

Resizes and populates the initial appropriate contents in a vector of tp-obstacles (collision-free ranges, in “pseudometers”, un-normalized).

See also:

updateTPObstacle()

double getScorePriority() const

When used in path planning, a multiplying factor (default=1.0) for the scores for this PTG.

Assign values <1 to PTGs with low priority.

void setScorePriorty(double prior)

Deprecated Misspelled; use setScorePriority().

virtual void renderPathAsSimpleLine(
    const uint16_t k,
    mrpt::viz::CSetOfLines& gl_obj,
    const double decimate_distance = 0.1,
    const double max_path_distance = -1.0
    ) const

Returns the representation of one trajectory of this PTG as a 3D OpenGL object (a simple curved line).

Parameters:

k	The 0-based index of the selected trajectory (discrete “alpha” parameter).
gl_obj	Output object.
decimate_distance	Minimum distance between path points (in meters).
max_path_distance	If >=0, cut the path at this distance (in meters).

bool debugDumpInFiles(const std::string& ptg_name) const

Dump PTG trajectories in four text files: .

/reactivenav.logs/PTGs/PTGi_{x,y,phi,d}.txt Text files are loadable from MATLAB/Octave, and can be visualized with the script [MRPT_DIR]/scripts/viewPTG.m The directory “./reactivenav.logs/PTGs” will be created if doesn’t exist.

Returns:

false on any error writing to disk.

See also:

OUTPUT_DEBUG_PATH_PREFIX

virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)

Parameters accepted by this base class:

• ${sKeyPrefix}num_paths : The number of different paths in this family (number of discrete alpha values).

• ${sKeyPrefix}ref_distance : The maximum distance in PTGs [meters]

• ${sKeyPrefix}score_priority : When used in path planning, a multiplying factor (default=1.0) for the scores for this PTG. Assign values <1 to PTGs with low priority.

virtual void saveToConfigFile(mrpt::config::CConfigFileBase& target, const std::string& section) const

This method saves the options to a “.ini”-like file or memory-stored string list.

See also:

loadFromConfigFile, saveToConfigFileName

virtual void add_robotShape_to_setOfLines(mrpt::viz::CSetOfLines& gl_shape, const mrpt::poses::CPose2D& origin = mrpt::poses::CPose2D()) const = 0

Auxiliary function for rendering.

void initClearanceDiagram(ClearanceDiagram& cd) const

Must be called to resize a CD to its correct size, before calling updateClearance().

Creates getClearanceStepCount() entries per decimated path, keyed by the normalized [0,1] TP-Space distance of the pose each one will be measured at.

void updateClearance(const double ox, const double oy, ClearanceDiagram& cd) const

Updates the clearance diagram given one (ox,oy) obstacle point, in coordinates relative to the PTG path origin.

Parameters:

cd	The clearance will be updated here.

virtual void evalClearanceSingleObstacle(
    const double ox,
    const double oy,
    const uint16_t k,
    ClearanceDiagram::dist2clearance_t& inout_realdist2clearance,
    bool treat_as_obstacle = true
    ) const

Evals the robot clearance at each of the poses sampled along path k, one per entry of inout_realdist2clearance, and keeps in each map value the minimum of its current content and the newly computed clearance.

In case of collision, clearance is zero.

Map keys are normalized [0,1] TP-Space distances, and so are the values (both divided by getRefDistance()). The keys are only read for the collision heuristics; the sampled poses are determined by the number of entries, evenly spread over the path steps.

Parameters:

treat_as_obstacle

true: normal use for obstacles; false: compute shortest distances to a target point (no collision)

static CParameterizedTrajectoryGenerator::Ptr CreatePTG(
    const std::string& ptgClassName,
    const mrpt::config::CConfigFileBase& cfg,
    const std::string& sSection,
    const std::string& sKeyPrefix
    )

The class factory for creating a PTG from a list of parameters in a section of a given config file (physical file or in memory).

Possible parameters are:

• Those explained in CParameterizedTrajectoryGenerator::loadFromConfigFile()

• Those explained in the specific PTG being created (see list of derived classes)

ptgClassName can be any PTG class name which has been registered as any other mrpt::serialization::CSerializable class.

Parameters:

std::logic_error

On invalid or missing parameters.

static std::string getOutputDebugPathPrefix()

Returns the path used as default output in, for example, debugDumpInFiles.

(Default=”./reactivenav.logs/”)

static void setOutputDebugPathPrefix(const std::string& path)

Sets the path used as default output in, for example, debugDumpInFiles.

static std::string& OUTPUT_DEBUG_PATH_PREFIX()

Deprecated Use getOutputDebugPathPrefix() / setOutputDebugPathPrefix()

static PTGCollisionBehavior getCollisionBehavior()

Returns the behavior when there is an obstacle inside the robot shape at the beginning of a PTG trajectory.

Default value: PTGCollisionBehavior::BACK_AWAY

static void setCollisionBehavior(PTGCollisionBehavior behavior)

Sets the behavior when there is an obstacle inside the robot shape at the beginning of a PTG trajectory.

static PTGCollisionBehavior& COLLISION_BEHAVIOR()

Deprecated Use getCollisionBehavior() / setCollisionBehavior()