Fix format to show contents properly (AtsushiSakai#608)

* Fix format to show contents properly

Lots of contents were not displayed correctly. I empty lines and :math: labels and fixed it.

* Rewrote one subscript as _{rho}

This is just a try to see if subscript renders properly.

* Added a gif animation for move to pose robot

* Added gif; Removed png

* Added the into

* Changed Param. Def. style to bold text

* Added algorithm logic and math

* Update docs/modules/control/move_to_a_pose_control/move_to_a_pose_control.rst

Co-authored-by: Atsushi Sakai <[email protected]>

* Update docs/modules/control/move_to_a_pose_control/move_to_a_pose_control.rst

Co-authored-by: Atsushi Sakai <[email protected]>

* Changed Eqs 1 and 2 color to black

* Added cross-reference labels for Equations 1 and 2

Co-authored-by: Atsushi Sakai <[email protected]>

Author: Muhammad-Yazdian

docs/modules/control/move_to_a_pose_control/move_to_a_pose_control.rst

@@ -1,57 +1,111 @@
-Move to a pose control
+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.
+
+Move to a Pose Control
 ----------------------
 
-This is a simulation of moving 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.
 
 .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/move_to_pose/animation.gif
 
 
+
 PathFinderController class
 ~~~~~~~~~~~~~~~~~~~~~~~~~~
 
 Constructor
 ~~~~~~~~~~~
 
 .. code-block:: ipython3
+
    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.)
+
+- | **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)
 ~~~~~~~~~~~~~~~~~~
 
 .. code-block:: ipython3
+
    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
+
+- | **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
+
+- | **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, :math:`\rho`, is computed as
+
+.. math::
+ \rho = \sqrt{(x_{robot} - x_{target})^2 + (y_{robot} - y_{target})^2}.
+
+The distance :math:`\rho` is used to determine the robot speed. The idea is to slow down the robot as it gets closer to the target.
+
+.. math::
+ v = K_P{_\rho} \times \rho\qquad
+ :label: eq1
+
+Note that for your applications, you need to tune the speed gain, :math:`K_P{_\rho}` to a proper value.
+
+To turn the robot and align its heading, :math:`\theta`, toward the target position (not orientation),  :math:`\rho \vec{u}`, we need to compute the angle difference :math:`\alpha`. 
+
+.. math::
+ \alpha = (\arctan2(y_{diff}, x_{diff}) - \theta + \pi) mod (2\pi) - \pi
+
+The term :math:`mod(2\pi)` is used to map the angle to :math:`[-\pi, \pi)` range.
+
+Lastly to correct the orientation of the robot, we need to compute the orientation error, :math:`\beta`, of the robot.
+
+.. math::
+ \beta = (\theta_{goal} - \theta - \alpha + \pi) mod (2\pi) - \pi
+
+Note that to cancel out the effect of :math:`\alpha` when the robot is at the vicinity of the target, the term 
+
+:math:`-\alpha` is included.
+
+The final angular speed command is given by
+
+.. math::
+ \omega = K_P{_\alpha} \alpha - K_P{_\beta} \beta\qquad
+ :label: eq2
+ 
+The linear and angular speeds (Equations :eq:`eq1` and :eq:`eq2`) 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.
 
-.. image:: https://user-images.githubusercontent.com/93126501/145834505-a8df8311-5445-413f-a96f-00460d47991c.png
-
-Note: A gif animation will be added soon.
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Control/move_to_pos_robot_class/animation.gif
 
-Note: The robot Class is based on PathFinderController class in 'the move_to_pose.py'.
+Note: The robot class is based on PathFinderController class in 'the move_to_pose.py'.
 
 Robot Class
 ~~~~~~~~~~~
@@ -60,36 +114,42 @@ Constructor
 ~~~~~~~~~~~
 
 .. code-block:: ipython3
+
     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.
+
+- | **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)
 ~~~~~~~~~~~~~~~~~~
 
 .. code-block:: ipython3
+
     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)
+
+- | **pose_start** : (Pose) Start postion of the robot (see the Pose class)
+- | **pose_target** : (Pose) Target postion of the robot (see the Pose class)
 
 .. code-block:: ipython3
+
     move(dt)
 
 Move the robot for one time step increment
 
 Parameters:
-- dt: <float> time increment
+
+- | **dt** : <float> time increment
 
 See Also 
 --------