class mrpt::nav::CPTG_DiffDrive_C

A PTG for circular paths (“C” type PTG in papers).

  • Compatible kinematics : differential-driven / Ackermann steering

  • Compatible robot shape : Arbitrary 2D polygon

  • PTG parameters : Use the app ptg-configurator

This PT generator functions are:

\[v(\alpha) = V_{MAX} sign(K)\]
\[\omega(\alpha) = \dfrac{\alpha}{\pi} W_{MAX} sign(K)\]

So, the radius of curvature of each trajectory is constant for each “alpha” value (the trajectory parameter):

\[R(\alpha) = \dfrac{v}{\omega} = \dfrac{V_{MAX}}{W_{MAX}} \dfrac{\pi}{\alpha}\]

from which a minimum radius of curvature can be set by selecting the appropriate values of V_MAX and W_MAX, knowning that \(\alpha \in (-\pi,\pi)\).

C-PTG path examples C-PTG path examples C-PTG path examples C-PTG path examples

[Before MRPT 1.5.0 this was named CPTG1]

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

class CPTG_DiffDrive_C: public mrpt::nav::CPTG_DiffDrive_CollisionGridBased
{
public:
    // construction

    CPTG_DiffDrive_C();

    CPTG_DiffDrive_C(
        const mrpt::config::CConfigFileBase& cfg,
        const std::string& sSection
        );

    //
methods

    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 std::string getDescription() const;

    virtual bool inverseMap_WS2TP(
        double x,
        double y,
        int& out_k,
        double& out_normalized_d,
        double tolerance_dist = 0.10
        ) const;

    virtual bool PTG_IsIntoDomain(double x, double y) const;

    virtual void ptgDiffDriveSteeringFunction(
        float alpha,
        float t,
        float x,
        float y,
        float phi,
        float& v,
        float& w
        ) const;

    virtual void loadDefaultParams();
};

Inherited Members

public:
    // structs

    struct TNavDynamicState;
    struct TCellForLambdaFunction;

    // classes

    class CCollisionGrid;

    //
methods

    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;
    virtual std::string getDescription() const = 0;

    virtual bool inverseMap_WS2TP(
        double x,
        double y,
        int& out_k,
        double& out_normalized_d,
        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 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 bool getPathStepForDist(uint16_t k, double dist, uint32_t& out_step) const = 0;
    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 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);

    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;
    void initTPObstacles(std::vector<double>& TP_Obstacles) const;
    double getScorePriority() const;

    virtual void renderPathAsSimpleLine(
        const uint16_t k,
        mrpt::opengl::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::opengl::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& OUTPUT_DEBUG_PATH_PREFIX();
    static PTG_collision_behavior_t& COLLISION_BEHAVIOR();
    const mrpt::math::CPolygon& getRobotShape() const;
    virtual void setRefDistance(const double refDist);
    const TNavDynamicState& getCurrentNavDynamicState() const;
    double getRefDistance() const;
    void initTPObstacleSingle(uint16_t k, double& TP_Obstacle_k) const;
    void setScorePriorty(double prior);
    unsigned getClearanceStepCount() const;
    void setClearanceStepCount(const unsigned res);
    unsigned getClearanceDecimatedPaths() const;
    void setClearanceDecimatedPaths(const unsigned num);
    void updateClearancePost(ClearanceDiagram& cd, const std::vector<double>& TP_obstacles) const;
    static void static_add_robotShape_to_setOfLines(mrpt::opengl::CSetOfLines& gl_shape, const mrpt::poses::CPose2D& origin, const mrpt::math::CPolygon& robotShape);
    static double Index2alpha(uint16_t k, const unsigned int num_paths);
    static uint16_t Alpha2index(double alpha, const unsigned int num_paths);

    virtual void ptgDiffDriveSteeringFunction(
        float alpha,
        float t,
        float x,
        float y,
        float phi,
        float& v,
        float& w
        ) const = 0;

Methods

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 std::string getDescription() const

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

virtual bool inverseMap_WS2TP(
    double x,
    double y,
    int& out_k,
    double& out_normalized_d,
    double tolerance_dist = 0.10
    ) const

Computes the closest (alpha,d) TP coordinates of the trajectory point closest to the Workspace (WS) Cartesian coordinates (x,y), relative to the current robot frame.

Parameters:

x

X coordinate of the query point, relative to the robot frame.

y

Y coordinate of the query point, relative to the robot frame.

out_k

Trajectory parameter index (discretized alpha value, 0-based index).

out_d

Trajectory distance, normalized such that D_max becomes 1.

Returns:

true if the distance between (x,y) and the actual trajectory point is below the given tolerance (in meters).

virtual bool PTG_IsIntoDomain(double x, double y) const

Returns the same than inverseMap_WS2TP() but without any additional cost.

The default implementation just calls inverseMap_WS2TP() and discards (k,d).

virtual void ptgDiffDriveSteeringFunction(
    float alpha,
    float t,
    float x,
    float y,
    float phi,
    float& v,
    float& w
    ) const

The main method to be implemented in derived classes: it defines the differential-driven differential equation.

virtual void loadDefaultParams()

Loads a set of default parameters; provided exclusively for the PTG-configurator tool.