# Move to a Pose Control

In this section, we present the logic of PathFinderController that drives a car from a start pose (x, y, theta) to a goal pose. A simulation of moving to a pose control is presented below.

## Position Control of non-Holonomic Systems

This section explains the logic of a position controller for systems with constraint (non-Holonomic system).

The position control of a 1-DOF (Degree of Freedom) system is quite straightforward. We only need to compute a position error and multiply it with a proportional gain to create a command. The actuator of the system takes this command and drive the system to the target position. This controller can be easily extended to higher dimensions (e.g., using Kp_x and Kp_y gains for a 2D position control). In these systems, the number of control commands is equal to the number of degrees of freedom (Holonomic system).

To describe the configuration of a car on a 2D plane, we need three DOFs (i.e., x, y, and theta). But to drive a car we only need two control commands (theta_engine and theta_steering_wheel). This difference is because of a constraint between the x and y DOFs. The relationship between the delta_x and delta_y is governed by the theta_steering_wheel.

Note that a car is normally a non-Holonomic system but if the road is slippery, the car turns into a Holonomic system and thus it needs three independent commands to be controlled.

## PathFinderController class

### Constructor

PathFinderController(Kp_rho, Kp_alpha, Kp_beta)


Constructs an instantiate of the PathFinderController for navigating a 3-DOF wheeled robot on a 2D plane.

Parameters:

• Kp_rho : The linear velocity gain to translate the robot along a line towards the goal
• Kp_alpha : The angular velocity gain to rotate the robot towards the goal
• Kp_beta : The offset angular velocity gain accounting for smooth merging to the goal angle (i.e., it helps the robot heading to be parallel to the target angle.)

### Member function(s)

calc_control_command(x_diff, y_diff, theta, theta_goal)


Returns the control command for the linear and angular velocities as well as the distance to goal

Parameters:

• x_diff : The position of target with respect to current robot position in x direction
• y_diff : The position of target with respect to current robot position in y direction
• theta : The current heading angle of robot with respect to x axis
• theta_goal : The target angle of robot with respect to x axis

Returns:

• rho : The distance between the robot and the goal position
• v : Command linear velocity
• w : Command angular velocity

## How does the Algorithm Work

The distance between the robot and the goal position, $$\rho$$, is computed as

$\rho = \sqrt{(x_{robot} - x_{target})^2 + (y_{robot} - y_{target})^2}.$

The distance $$\rho$$ is used to determine the robot speed. The idea is to slow down the robot as it gets closer to the target.

(1)$v = K_P{_\rho} \times \rho\qquad$

Note that for your applications, you need to tune the speed gain, $$K_P{_\rho}$$ to a proper value.

To turn the robot and align its heading, $$\theta$$, toward the target position (not orientation), $$\rho \vec{u}$$, we need to compute the angle difference $$\alpha$$.

$\alpha = (\arctan2(y_{diff}, x_{diff}) - \theta + \pi) mod (2\pi) - \pi$

The term $$mod(2\pi)$$ is used to map the angle to $$[-\pi, \pi)$$ range.

Lastly to correct the orientation of the robot, we need to compute the orientation error, $$\beta$$, of the robot.

$\beta = (\theta_{goal} - \theta - \alpha + \pi) mod (2\pi) - \pi$

Note that to cancel out the effect of $$\alpha$$ when the robot is at the vicinity of the target, the term

$$-\alpha$$ is included.

The final angular speed command is given by

(2)$\omega = K_P{_\alpha} \alpha - K_P{_\beta} \beta\qquad$

The linear and angular speeds (Equations (1) and (2)) are the output of the algorithm.

## Move to a Pose Robot (Class)

This program (move_to_pose_robot.py) provides a Robot class to define different robots with different specifications. Using this class, you can simulate different robots simultaneously and compare the effect of your parameter settings.

Note: The robot class is based on PathFinderController class in ‘the move_to_pose.py’.

### Constructor

Robot(name, color, max_linear_speed, max_angular_speed, path_finder_controller)


Constructs an instantiate of the 3-DOF wheeled Robot navigating on a 2D plane

Parameters:

• name : (string) The name of the robot
• color : (string) The color of the robot
• max_linear_speed : (float) The maximum linear speed that the robot can go
• max_angular_speed : (float) The maximum angular speed that the robot can rotate about its vertical axis
• path_finder_controller : (PathFinderController) A configurable controller to finds the path and calculates command linear and angular velocities.

### Member function(s)

set_start_target_poses(pose_start, pose_target)


Sets the start and target positions of the robot.

Parameters:

• pose_start : (Pose) Start postion of the robot (see the Pose class)
• pose_target : (Pose) Target postion of the robot (see the Pose class)
move(dt)


Move the robot for one time step increment

Parameters:

• dt : <float> time increment