diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 9f9dc20516..839de8bf23 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -29,6 +29,7 @@ repos: args: ["--allow-multiple-documents"] - id: debug-statements - id: end-of-file-fixer + exclude_types: [svg] - id: mixed-line-ending - id: trailing-whitespace exclude_types: [rst] diff --git a/doc/images/swerve_drive.drawio b/doc/images/swerve_drive.drawio new file mode 100644 index 0000000000..7f79cad6a5 --- /dev/null +++ b/doc/images/swerve_drive.drawio @@ -0,0 +1,664 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/swerve_drive.svg b/doc/images/swerve_drive.svg new file mode 100644 index 0000000000..7afefd3c18 --- /dev/null +++ b/doc/images/swerve_drive.svg @@ -0,0 +1,4 @@ + + + +
0
1
2
3
l
0
1
2
3
Rotating Clockwise
w
0
1
2
3
Moving Forward
%3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%26lt%3Bfont%20style%3D%26quot%3Bcolor%3A%20light-dark(rgb(255%2C%20255%2C%20255)%2C%20rgb(255%2C%20255%2C%20255))%3B%26quot%3B%26gt%3B0%26lt%3B%2Ffont%26gt%3B%22%20style%3D%22text%3Bhtml%3D1%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3Bresizable%3D0%3Bpoints%3D%5B%5D%3Bautosize%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3B%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%2282%22%20y%3D%22185%22%20width%3D%2230%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E%3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%26lt%3Bfont%20style%3D%26quot%3Bcolor%3A%20light-dark(rgb(255%2C%20255%2C%20255)%2C%20rgb(255%2C%20255%2C%20255))%3B%26quot%3B%26gt%3B0%26lt%3B%2Ffont%26gt%3B%22%20style%3D%22text%3Bhtml%3D1%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3Bresizable%3D0%3Bpoints%3D%5B%5D%3Bautosize%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3B%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%2282%22%20y%3D%22185%22%20width%3D%2230%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E
0
1
2
3
Moving Right
0
1
2
3
Moving Left
 \ No newline at end of file diff --git a/doc/mobile_robot_kinematics.rst b/doc/mobile_robot_kinematics.rst index b861733ee7..4bf285972e 100644 --- a/doc/mobile_robot_kinematics.rst +++ b/doc/mobile_robot_kinematics.rst @@ -109,6 +109,91 @@ The body twist of the robot can be obtained from the wheel velocities by using t \omega_n \end{bmatrix} + + +Swerve Drive Robots +,,,,,,,,,,,,,,,,,,, + +The below explains the kinematics of omnidirectional drive robots using four swerve modules, each with independently controlled steering and driving motors. It follows the coordinate conventions defined in `REP-103 `__. + +.. image:: images/swerve_drive.svg + :align: center + :alt: Swerve Drive Robot + +* :math:`x_b, y_b` is the robot's body-frame coordinate system, located at the geometric center of the robot. +* :math:`x_w, y_w` is the world coordinate system. +* :math:`v_{b,x}` is the robot's linear velocity on the x-axis. +* :math:`v_{b,y}` is the robot's linear velocity on the y-axis. +* :math:`\omega_{b,z}` is the robot's angular velocity on the z-axis. +* :math:`l` is the wheelbase (distance between front and rear wheels). +* :math:`w` is the track width (distance between left and right wheels). +* Red arrows on wheel :math:`i` signify the direction of the wheel's velocity :math:`v_i`. + +Each swerve module :math:`i`, for :math:`i = 0, 1, 2, 3` (typically front-left, front-right, back-left, back-right) is located at :math:`(l_{i,x}, l_{i,y})` relative to the center, typically: + +* Front-left: :math:`(l/2, w/2)` +* Front-right: :math:`(l/2, -w/2)` +* Back-left: :math:`(-l/2, w/2)` +* Back-right: :math:`(-l/2, -w/2)` + +**Inverse Kinematics** + +For each module :math:`i` at position :math:`(l_{i,x}, l_{i,y})`, the velocity vector is: + +.. math:: + + \begin{bmatrix} + v_{i,x} \\ + v_{i,y} + \end{bmatrix} + = + \begin{bmatrix} + v_{b,x} - \omega_{b,z} l_{i,y} \\ + v_{b,y} + \omega_{b,z} l_{i,x} + \end{bmatrix} + +The wheel velocity :math:`v_i` and steering angle :math:`\phi_i` are: + +.. math:: + + v_i = \sqrt{v_{i,x}^2 + v_{i,y}^2} + +.. math:: + + \phi_i = \arctan2(v_{i,y}, v_{i,x}) + + +**Odometry** + +The body twist of the robot is computed from the wheel velocities :math:`v_i` and steering angles :math:`\phi_i`. Each module’s velocity components in the body frame are: + +.. math:: + + v_{i,x} = v_i \cos(\phi_i), \quad v_{i,y} = v_i \sin(\phi_i) + +The chassis velocities are calculated as: + +.. math:: + + v_{b,x} = \frac{1}{4} \sum_{i=0}^{3} v_{i,x}, \quad v_{b,y} = \frac{1}{4} \sum_{i=0}^{3} v_{i,y} + +.. math:: + + \omega_{b,z} = \frac{\sum_{i=0}^{3} (v_{i,y} l_{i,x} - v_{i,x} l_{i,y})}{\sum_{i=0}^{3} (l_{i,x}^2 + l_{i,y}^2)} + + + +Odometry updates the robot’s pose (:math:`x`, :math:`y`, :math:`\theta`) in the global frame using the computed chassis velocities. The global velocities are: + +.. math:: + + v_{x,\text{global}} = v_{b,x} \cos(\theta) - v_{b,y} \sin(\theta) + +.. math:: + + v_{y,\text{global}} = v_{b,x} \sin(\theta) + v_{b,y} \cos(\theta) + + Nonholonomic Wheeled Mobile Robots .....................................