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

#include <nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp>

Public Member 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. More...

	~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. More...

void	setSlowdownRadius (const double slowdown_radius)
	Set the slowdown radius. More...

void	setSpeedLimit (const double v_linear_min, const double v_linear_max, const double v_angular_max)
	Update the velocity limits. More...

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. More...

geometry_msgs::msg::Twist	calculateRegularVelocity (const geometry_msgs::msg::Pose &target, const bool &backward=false)
	Compute linear and angular velocities command using the curvature. More...

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. More...

Protected Member 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). More...

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. More...

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

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.

Detailed Description

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

Definition at line 32 of file smooth_control_law.hpp.

Constructor & Destructor Documentation

◆ SmoothControlLaw()

nav2_graceful_controller::SmoothControlLaw::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.

Definition at line 23 of file smooth_control_law.cpp.

Member Function Documentation

◆ calculateCurvature()

double nav2_graceful_controller::SmoothControlLaw::calculateCurvature	(	double	r,
		double	phi,
		double	delta
	)

protected

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

Definition at line 114 of file smooth_control_law.cpp.

References k_delta_, and k_phi_.

Referenced by calculateRegularVelocity().

Here is the caller graph for this function:

◆ calculateNextPose()

geometry_msgs::msg::Pose nav2_graceful_controller::SmoothControlLaw::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

Definition at line 98 of file smooth_control_law.cpp.

References calculateRegularVelocity().

Here is the call graph for this function:

◆ calculateRegularVelocity() [1/2]

geometry_msgs::msg::Twist nav2_graceful_controller::SmoothControlLaw::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.

Definition at line 92 of file smooth_control_law.cpp.

References calculateRegularVelocity().

Here is the call graph for this function:

◆ calculateRegularVelocity() [2/2]

geometry_msgs::msg::Twist nav2_graceful_controller::SmoothControlLaw::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.

Definition at line 53 of file smooth_control_law.cpp.

References beta_, calculateCurvature(), lambda_, slowdown_radius_, v_angular_max_, v_linear_max_, and v_linear_min_.

Referenced by calculateNextPose(), and calculateRegularVelocity().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setCurvatureConstants()

void nav2_graceful_controller::SmoothControlLaw::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.

Definition at line 31 of file smooth_control_law.cpp.

References beta_, k_delta_, k_phi_, and lambda_.

◆ setSlowdownRadius()

void nav2_graceful_controller::SmoothControlLaw::setSlowdownRadius ( const double slowdown_radius )

Set the slowdown radius.

Parameters

slowdown_radius Radial threshold applied to the slowdown rule.

Definition at line 40 of file smooth_control_law.cpp.

References slowdown_radius_.

◆ setSpeedLimit()

void nav2_graceful_controller::SmoothControlLaw::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.

Definition at line 45 of file smooth_control_law.cpp.

References v_angular_max_, v_linear_max_, and v_linear_min_.

Member Data Documentation

◆ k_delta_

double nav2_graceful_controller::SmoothControlLaw::k_delta_

protected

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.

Definition at line 154 of file smooth_control_law.hpp.

Referenced by calculateCurvature(), and setCurvatureConstants().

◆ k_phi_

double nav2_graceful_controller::SmoothControlLaw::k_phi_

protected

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.

Definition at line 144 of file smooth_control_law.hpp.

Referenced by calculateCurvature(), and setCurvatureConstants().

The documentation for this class was generated from the following files:

nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp
nav2_graceful_controller/src/smooth_control_law.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ SmoothControlLaw()

Member Function Documentation

◆ calculateCurvature()

◆ calculateNextPose()

◆ calculateRegularVelocity() [1/2]

◆ calculateRegularVelocity() [2/2]

◆ setCurvatureConstants()

◆ setSlowdownRadius()

◆ setSpeedLimit()

Member Data Documentation

◆ k_delta_

◆ k_phi_