Class SmoothControlLaw

• Defined in File smooth_control_law.hpp

Class Documentation

class SmoothControlLaw

Smooth control law for graceful motion based on “A smooth control law for graceful motion” (Jong Jin Park and Benjamin Kuipers).

Public Functions

SmoothControlLaw(double k_phi, double k_delta, double beta, double lambda, double slowdown_radius, double v_linear_min, double v_linear_max, double v_angular_max)

Constructor for nav2_graceful_controller::SmoothControlLaw.

Parameters:

• k_phi – Ratio of the rate of change in phi to the rate of change in r.

• k_delta – Constant factor applied to the heading error feedback.

• beta – Constant factor applied to the path curvature: dropping velocity.

• lambda – Constant factor applied to the path curvature for sharpness.

• slowdown_radius – Radial threshold applied to the slowdown rule.

• v_linear_min – Minimum linear velocity.

• v_linear_max – Maximum linear velocity.

• v_angular_max – Maximum angular velocity.

~SmoothControlLaw() = default

Destructor for nav2_graceful_controller::SmoothControlLaw.

void setCurvatureConstants(const double k_phi, const double k_delta, const double beta, const double lambda)

Set the values that define the curvature.

Parameters:

• k_phi – Ratio of the rate of change in phi to the rate of change in r.

• k_delta – Constant factor applied to the heading error feedback.

• beta – Constant factor applied to the path curvature: dropping velocity.

• lambda – Constant factor applied to the path curvature for sharpness.

void setSlowdownRadius(const double slowdown_radius)

Set the slowdown radius.

Parameters:

slowdown_radius – Radial threshold applied to the slowdown rule.

void setSpeedLimit(const double v_linear_min, const double v_linear_max, const double v_angular_max)

Update the velocity limits.

Parameters:

• v_linear_min – The minimum absolute velocity in the linear direction.

• v_linear_max – The maximum absolute velocity in the linear direction.

• v_angular_max – The maximum absolute velocity in the angular direction.

geometry_msgs::msg::Twist calculateRegularVelocity(const geometry_msgs::msg::Pose &target, const geometry_msgs::msg::Pose &current, const bool &backward = false)

Compute linear and angular velocities command using the curvature.

Parameters:

• target – Pose of the target in the robot frame.

• current – Current pose of the robot in the robot frame.

• backward – If true, the robot is moving backwards. Defaults to false.

Returns:

Velocity command.

geometry_msgs::msg::Twist calculateRegularVelocity(const geometry_msgs::msg::Pose &target, const bool &backward = false)

Compute linear and angular velocities command using the curvature.

Parameters:

• target – Pose of the target in the robot frame.

• backward – If true, the robot is moving backwards. Defaults to false.

Returns:

Velocity command.

geometry_msgs::msg::Pose calculateNextPose(const double dt, const geometry_msgs::msg::Pose &target, const geometry_msgs::msg::Pose &current, const bool &backward = false)

Calculate the next pose of the robot by generating a velocity command using the curvature and the current pose.

Parameters:

• dt – Time step.

• target – Pose of the target in the robot frame.

• current – Current pose of the robot in the robot frame.

• backward – If true, the robot is moving backwards. Defaults to false.

Returns:

geometry_msgs::msg::Pose

Protected Functions

double calculateCurvature(double r, double phi, double delta)

Calculate the path curvature using a Lyapunov-based feedback control law from “A smooth control law for graceful motion” (Jong Jin Park and Benjamin Kuipers).

Parameters:

• r – Distance between the robot and the target.

• phi – Orientation of target with respect to the line of sight from the robot to the target.

• delta – Steering angle of the robot.

Returns:

The curvature

Protected Attributes

double k_phi_

Ratio of the rate of change in phi to the rate of change in r. Controls the convergence of the slow subsystem.

If this value is equal to zero, the controller will behave as a pure waypoint follower. A high value offers extreme scenario of pose-following where theta is reduced much faster than r.

Note: This variable is called k1 in earlier versions of the paper.

double k_delta_

Constant factor applied to the heading error feedback. Controls the convergence of the fast subsystem.

The bigger the value, the robot converge faster to the reference heading.

Note: This variable is called k2 in earlier versions of the paper.

double beta_

Constant factor applied to the path curvature. This value must be positive. Determines how fast the velocity drops when the curvature increases.

double lambda_

Constant factor applied to the path curvature. This value must be greater or equal to 1. Determines the sharpness of the curve: higher lambda implies sharper curves.

double slowdown_radius_

Radial threshold applied to the slowdown rule.

double v_linear_min_

Minimum linear velocity.

double v_linear_max_

Maximum linear velocity.

double v_angular_max_

Maximum angular velocity.