Skip to content

Commit 279cac9

Browse files
silanus23silanus23
committed
Add proposal for refactored steering controller library and six wheeler steering controller library
1 parent a10c564 commit 279cac9

File tree

4 files changed

+397
-0
lines changed

4 files changed

+397
-0
lines changed

‎doc/images/six_wheeler_steering.drawio‎

Lines changed: 164 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,164 @@
1+
<mxfile host="app.diagrams.net" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/138.0.0.0 Safari/537.36" version="28.0.6">
2+
<diagram name="Page-1" id="vPOEDBpcrtD8xKoLgtPd">
3+
<mxGraphModel dx="2850" dy="1153" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="850" pageHeight="1100" math="1" shadow="0">
4+
<root>
5+
<mxCell id="0" />
6+
<mxCell id="1" parent="0" />
7+
<mxCell id="fpfmOAwSDF_yLItyLWh9-1" value="" style="rounded=1;whiteSpace=wrap;html=1;direction=south;rotation=15;" parent="1" vertex="1">
8+
<mxGeometry x="240" y="760" width="60" height="120" as="geometry" />
9+
</mxCell>
10+
<mxCell id="fpfmOAwSDF_yLItyLWh9-3" value="" style="rounded=1;whiteSpace=wrap;html=1;direction=south;" parent="1" vertex="1">
11+
<mxGeometry x="600" y="470" width="60" height="120" as="geometry" />
12+
</mxCell>
13+
<mxCell id="fpfmOAwSDF_yLItyLWh9-4" value="" style="rounded=1;whiteSpace=wrap;html=1;direction=south;rotation=15;" parent="1" vertex="1">
14+
<mxGeometry x="500" y="760" width="60" height="120" as="geometry" />
15+
</mxCell>
16+
<mxCell id="fpfmOAwSDF_yLItyLWh9-5" value="" style="rounded=1;whiteSpace=wrap;html=1;direction=south;rotation=-15;" parent="1" vertex="1">
17+
<mxGeometry x="500" y="108" width="60" height="120" as="geometry" />
18+
</mxCell>
19+
<mxCell id="fpfmOAwSDF_yLItyLWh9-6" value="" style="rounded=1;whiteSpace=wrap;html=1;direction=south;" parent="1" vertex="1">
20+
<mxGeometry x="180" y="470" width="60" height="120" as="geometry" />
21+
</mxCell>
22+
<mxCell id="fpfmOAwSDF_yLItyLWh9-7" value="" style="rounded=1;whiteSpace=wrap;html=1;direction=south;rotation=-15;" parent="1" vertex="1">
23+
<mxGeometry x="240" y="130" width="60" height="120" as="geometry" />
24+
</mxCell>
25+
<mxCell id="fpfmOAwSDF_yLItyLWh9-8" value="" style="rounded=0;whiteSpace=wrap;html=1;" parent="1" vertex="1">
26+
<mxGeometry x="260" y="280" width="290" height="440" as="geometry" />
27+
</mxCell>
28+
<mxCell id="JtxlL-9a8NOayIRht31E-1" value="" style="endArrow=none;html=1;rounded=0;" parent="1" edge="1">
29+
<mxGeometry width="50" height="50" relative="1" as="geometry">
30+
<mxPoint x="50" y="520" as="sourcePoint" />
31+
<mxPoint x="450" y="520" as="targetPoint" />
32+
</mxGeometry>
33+
</mxCell>
34+
<mxCell id="JtxlL-9a8NOayIRht31E-4" value="" style="endArrow=none;html=1;rounded=0;" parent="1" edge="1">
35+
<mxGeometry width="50" height="50" relative="1" as="geometry">
36+
<mxPoint x="50" y="520" as="sourcePoint" />
37+
<mxPoint x="430" y="470" as="targetPoint" />
38+
</mxGeometry>
39+
</mxCell>
40+
<mxCell id="JtxlL-9a8NOayIRht31E-6" value="" style="endArrow=none;html=1;rounded=0;" parent="1" edge="1">
41+
<mxGeometry width="50" height="50" relative="1" as="geometry">
42+
<mxPoint x="269.5" y="200" as="sourcePoint" />
43+
<mxPoint x="269.5" y="40" as="targetPoint" />
44+
</mxGeometry>
45+
</mxCell>
46+
<mxCell id="JtxlL-9a8NOayIRht31E-7" value="" style="endArrow=none;html=1;rounded=0;" parent="1" edge="1">
47+
<mxGeometry width="50" height="50" relative="1" as="geometry">
48+
<mxPoint x="260" y="970" as="sourcePoint" />
49+
<mxPoint x="260" y="810" as="targetPoint" />
50+
</mxGeometry>
51+
</mxCell>
52+
<mxCell id="JtxlL-9a8NOayIRht31E-10" value="" style="endArrow=none;html=1;rounded=0;" parent="1" edge="1">
53+
<mxGeometry width="50" height="50" relative="1" as="geometry">
54+
<mxPoint x="310" y="190" as="sourcePoint" />
55+
<mxPoint x="270" y="40" as="targetPoint" />
56+
</mxGeometry>
57+
</mxCell>
58+
<mxCell id="JtxlL-9a8NOayIRht31E-19" value="" style="endArrow=none;dashed=1;html=1;dashPattern=1 3;strokeWidth=2;rounded=0;" parent="1" edge="1">
59+
<mxGeometry width="50" height="50" relative="1" as="geometry">
60+
<mxPoint x="-330" y="520" as="sourcePoint" />
61+
<mxPoint x="440" y="520" as="targetPoint" />
62+
</mxGeometry>
63+
</mxCell>
64+
<mxCell id="JtxlL-9a8NOayIRht31E-20" value="" style="endArrow=none;dashed=1;html=1;dashPattern=1 3;strokeWidth=2;rounded=0;" parent="1" edge="1">
65+
<mxGeometry width="50" height="50" relative="1" as="geometry">
66+
<mxPoint x="-340" y="520" as="sourcePoint" />
67+
<mxPoint x="270" y="200" as="targetPoint" />
68+
</mxGeometry>
69+
</mxCell>
70+
<mxCell id="JtxlL-9a8NOayIRht31E-21" value="" style="endArrow=none;dashed=1;html=1;dashPattern=1 3;strokeWidth=2;rounded=0;" parent="1" edge="1">
71+
<mxGeometry width="50" height="50" relative="1" as="geometry">
72+
<mxPoint x="-340" y="520" as="sourcePoint" />
73+
<mxPoint x="530" y="190" as="targetPoint" />
74+
</mxGeometry>
75+
</mxCell>
76+
<mxCell id="JtxlL-9a8NOayIRht31E-23" value="" style="endArrow=none;html=1;rounded=0;" parent="1" edge="1">
77+
<mxGeometry width="50" height="50" relative="1" as="geometry">
78+
<mxPoint x="260" y="970" as="sourcePoint" />
79+
<mxPoint x="330" y="840" as="targetPoint" />
80+
</mxGeometry>
81+
</mxCell>
82+
<mxCell id="UuVBM5KiAZeAReH1xgpa-2" value="$$\sqrt{d_2^2+(r-d_1)^2}$$&lt;div&gt;&lt;br&gt;&lt;/div&gt;" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;convertToSvg=1;" vertex="1" parent="1">
83+
<mxGeometry x="-20" y="360" width="210" height="60" as="geometry" />
84+
</mxCell>
85+
<mxCell id="UuVBM5KiAZeAReH1xgpa-4" value="&lt;div&gt;$$\sqrt{d_2^2+(r+d_1)^2}$$&lt;/div&gt;" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;convertToSvg=1;" vertex="1" parent="1">
86+
<mxGeometry x="-180" y="310" width="210" height="60" as="geometry" />
87+
</mxCell>
88+
<mxCell id="UuVBM5KiAZeAReH1xgpa-5" value="" style="endArrow=classic;startArrow=classic;html=1;rounded=0;" edge="1" parent="1">
89+
<mxGeometry width="50" height="50" relative="1" as="geometry">
90+
<mxPoint x="750" y="850" as="sourcePoint" />
91+
<mxPoint x="750" y="519.96" as="targetPoint" />
92+
</mxGeometry>
93+
</mxCell>
94+
<mxCell id="UuVBM5KiAZeAReH1xgpa-6" value="" style="endArrow=classic;startArrow=classic;html=1;rounded=0;" edge="1" parent="1">
95+
<mxGeometry width="50" height="50" relative="1" as="geometry">
96+
<mxPoint x="750" y="519.96" as="sourcePoint" />
97+
<mxPoint x="750" y="160" as="targetPoint" />
98+
</mxGeometry>
99+
</mxCell>
100+
<mxCell id="UuVBM5KiAZeAReH1xgpa-7" value="" style="endArrow=classic;startArrow=classic;html=1;rounded=0;" edge="1" parent="1">
101+
<mxGeometry width="50" height="50" relative="1" as="geometry">
102+
<mxPoint x="260" y="990" as="sourcePoint" />
103+
<mxPoint x="404.5" y="990" as="targetPoint" />
104+
</mxGeometry>
105+
</mxCell>
106+
<mxCell id="UuVBM5KiAZeAReH1xgpa-8" value="" style="endArrow=classic;startArrow=classic;html=1;rounded=0;" edge="1" parent="1">
107+
<mxGeometry width="50" height="50" relative="1" as="geometry">
108+
<mxPoint x="230" y="1040" as="sourcePoint" />
109+
<mxPoint x="404.5" y="1040" as="targetPoint" />
110+
</mxGeometry>
111+
</mxCell>
112+
<mxCell id="UuVBM5KiAZeAReH1xgpa-11" value="D4" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;" vertex="1" parent="1">
113+
<mxGeometry x="290" y="1050" width="60" height="30" as="geometry" />
114+
</mxCell>
115+
<mxCell id="UuVBM5KiAZeAReH1xgpa-13" value="D1" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;" vertex="1" parent="1">
116+
<mxGeometry x="318.36" y="950" width="60" height="30" as="geometry" />
117+
</mxCell>
118+
<mxCell id="UuVBM5KiAZeAReH1xgpa-15" value="D2" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;" vertex="1" parent="1">
119+
<mxGeometry x="770" y="340" width="60" height="30" as="geometry" />
120+
</mxCell>
121+
<mxCell id="UuVBM5KiAZeAReH1xgpa-16" value="D3" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;" vertex="1" parent="1">
122+
<mxGeometry x="770" y="730" width="60" height="30" as="geometry" />
123+
</mxCell>
124+
<mxCell id="UuVBM5KiAZeAReH1xgpa-19" value="" style="curved=1;endArrow=classic;html=1;rounded=0;" edge="1" parent="1">
125+
<mxGeometry width="50" height="50" relative="1" as="geometry">
126+
<mxPoint x="400" y="520" as="sourcePoint" />
127+
<mxPoint x="420" y="470" as="targetPoint" />
128+
<Array as="points">
129+
<mxPoint x="420" y="520" />
130+
<mxPoint x="430" y="490" />
131+
</Array>
132+
</mxGeometry>
133+
</mxCell>
134+
<mxCell id="UuVBM5KiAZeAReH1xgpa-21" value="" style="curved=1;endArrow=classic;html=1;rounded=0;" edge="1" parent="1">
135+
<mxGeometry width="50" height="50" relative="1" as="geometry">
136+
<mxPoint x="260" y="860" as="sourcePoint" />
137+
<mxPoint x="300" y="890" as="targetPoint" />
138+
<Array as="points">
139+
<mxPoint x="300" y="850" />
140+
</Array>
141+
</mxGeometry>
142+
</mxCell>
143+
<mxCell id="UuVBM5KiAZeAReH1xgpa-22" value="" style="curved=1;endArrow=classic;html=1;rounded=0;" edge="1" parent="1">
144+
<mxGeometry width="50" height="50" relative="1" as="geometry">
145+
<mxPoint x="270" y="180" as="sourcePoint" />
146+
<mxPoint x="300" y="150" as="targetPoint" />
147+
<Array as="points">
148+
<mxPoint x="300" y="180" />
149+
</Array>
150+
</mxGeometry>
151+
</mxCell>
152+
<mxCell id="UuVBM5KiAZeAReH1xgpa-24" value="" style="endArrow=classic;startArrow=classic;html=1;rounded=0;" edge="1" parent="1">
153+
<mxGeometry width="50" height="50" relative="1" as="geometry">
154+
<mxPoint x="-340" y="20" as="sourcePoint" />
155+
<mxPoint x="425" y="20" as="targetPoint" />
156+
</mxGeometry>
157+
</mxCell>
158+
<mxCell id="UuVBM5KiAZeAReH1xgpa-25" value="R" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;strokeColor=default;fillColor=none;strokeWidth=0;" vertex="1" parent="1">
159+
<mxGeometry x="-20" y="40" width="60" height="30" as="geometry" />
160+
</mxCell>
161+
</root>
162+
</mxGraphModel>
163+
</diagram>
164+
</mxfile>

‎doc/images/six_wheeler_steering.svg‎

Lines changed: 4 additions & 0 deletions
Loading

‎doc/six_wheeler_steering_controller.rst‎

Lines changed: 134 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,134 @@
1+
.. _doc_six_wheel_steering_controller:
2+
3+
six_wheel_steering_controller
4+
=============================
5+
6+
This page introduces the kinematic model for a six-wheeled rover featuring four-wheel steering, commonly used in rocker-bogie style vehicles.
7+
8+
Kinematics of the system
9+
------------------------
10+
11+
The controller uses a standard Ackermann-style kinematic model adapted for a six-wheel configuration. The core principle is that to avoid wheel slip during a turn, the axes of all wheels must intersect at a single point known as the **Instantaneous Center of Rotation (ICR)**.
12+
13+
The parameters shown in the diagram are essential for the kinematic calculations.
14+
15+
Diagram
16+
------------------
17+
.. image:: images/six_wheeler_steering.svg
18+
:width: 600px
19+
:align: center
20+
:alt: Six-wheel steering controller kinematic diagram
21+
22+
Inverse Kinematics
23+
------------------
24+
25+
Inverse kinematics determine the required joint commands from a desired vehicle body velocity (a ``Twist`` message).
26+
27+
**Steering Joint Angles**
28+
29+
Given a desired turning radius, :math:`R`, the required steering angles for the inner and outer front wheels are calculated as follows:
30+
31+
.. math::
32+
33+
\theta_{inner} = \arctan\left(\frac{d_3}{|R| - d_1}\right)
34+
35+
.. math::
36+
37+
\theta_{outer} = \arctan\left(\frac{d_3}{|R| + d_1}\right)
38+
39+
The rear wheels are commanded to the same angle magnitudes but in the opposite direction to articulate the turn.
40+
41+
**Drive Wheel Velocities**
42+
43+
The velocity of each drive wheel is proportional to its distance from the ICR. The vehicle's angular velocity, :math:`\omega_{center}`, is first determined from the desired linear velocity, :math:`v`:
44+
45+
.. math::
46+
47+
\omega_{center} = \frac{v}{R}
48+
49+
The linear velocity for each wheel (:math:`v_{wheel}`) is then found based on its unique turning radius. This is finally converted to the motor's angular velocity command, :math:`\omega_{wheel}`:
50+
51+
.. math::
52+
53+
\omega_{wheel} = \frac{v_{wheel}}{r_{wheel}}
54+
55+
Forward Kinematics
56+
------------------
57+
58+
Forward kinematics estimate the vehicle's current ``Twist`` from its joint states. The rover's linear velocity, :math:`v_x`, is estimated from the average velocity of the two non-steerable middle wheels.
59+
60+
.. math::
61+
62+
v_x = \frac{(\omega_{left\_middle} + \omega_{right\_middle})}{2} \cdot r_{wheel}
63+
64+
The vehicle's angular velocity, :math:`\omega_z`, is then derived from this linear velocity and an estimation of the current turning radius, :math:`R_{approx}`.
65+
66+
.. math::
67+
68+
\omega_z = \frac{v_x}{R_{approx}}
69+
70+
Controller Parameters
71+
---------------------
72+
73+
.. list-table:: Controller Parameters
74+
:widths: 20 80
75+
:header-rows: 1
76+
77+
* - Parameter
78+
- Description
79+
* - ``traction_joints``
80+
- A list of the six drive wheel joint names from the robot's URDF model.
81+
* - ``steering_joints``
82+
- A list of the four corner steering joint names from the robot's URDF model.
83+
* - ``d1``
84+
- Half the front/rear track width (meters). Lateral distance from the longitudinal center to the steering pivot.
85+
* - ``d2``
86+
- Longitudinal distance from rover center to the rear axle (meters).
87+
* - ``d3``
88+
- Longitudinal distance from rover center to the front axle (meters).
89+
* - ``d4``
90+
- Half the middle track width (meters). Lateral distance from the center to the middle wheel's center.
91+
* - ``wheel_radius``
92+
- Radius of the drive wheels (meters).
93+
* - ``odom_frame_id``
94+
- The name of the odometry frame. Default: ``odom``.
95+
* - ``base_frame_id``
96+
- The name of the robot's base frame. Default: ``base_link``.
97+
98+
Example Configuration
99+
---------------------
100+
101+
.. code-block:: yaml
102+
103+
six_wheel_steering_controller:
104+
ros__parameters:
105+
106+
# ---- JOINT CONFIGURATION ----
107+
# [ACTION REQUIRED] Replace with the exact joint names from your robot's URDF.
108+
traction_joints: [
109+
"front_left_wheel_joint",
110+
"middle_left_wheel_joint",
111+
"rear_left_wheel_joint",
112+
"front_right_wheel_joint",
113+
"middle_right_wheel_joint",
114+
"rear_right_wheel_joint"
115+
]
116+
steering_joints: [
117+
"front_left_steer_joint",
118+
"rear_left_steer_joint",
119+
"front_right_steer_joint",
120+
"rear_right_steer_joint"
121+
]
122+
123+
# ---- KINEMATIC PARAMETERS (in meters) ----
124+
# [ACTION REQUIRED] Measure these values from your specific rover.
125+
d1: 0.4
126+
d2: 0.5
127+
d3: 0.5
128+
d4: 0.45
129+
wheel_radius: 0.15
130+
131+
# ---- ODOMETRY CONFIGURATION ----
132+
odom_frame_id: "odom"
133+
base_frame_id: "base_link"
134+

‎steering_controllers_library/doc/refactored_steering_library.rst‎

Lines changed: 95 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,95 @@
1+
.. _proposal_refactored_steering_library:
2+
3+
Proposal: A Refactored, Plugin-Based Steering Controller Library
4+
================================================================
5+
6+
Motivation
7+
----------
8+
9+
The current ``steering_controllers_library`` has a monolithic design that tightly couples the core controller logic with specific kinematic implementations (Bicycle, Tricycle, Ackermann). This architecture presents several challenges:
10+
11+
1. **Lack of Extensibility:** Adding support for new, more complex kinematic models, such as the six-wheel, four-wheel-steering rover discussed in the six_wheel_steering_controllers.srt, requires modifying the core library itself. This is not a scalable solution.
12+
2. **Configuration Complexity:** The parameter interface is ambiguous and not easily adaptable to different vehicle types.
13+
3. **High Maintenance Burden:** The central library becomes increasingly complex with each new model, making it difficult to maintain and debug.
14+
15+
This document proposes a new architecture that addresses these issues by creating a truly **kinematics-agnostic** steering controller.
16+
17+
Proposed Architecture: A New, Additive Implementation
18+
------------------------------------------------------
19+
20+
The proposal is to introduce a new, refactored controller, ``generic_steering_controller``, to exist **alongside** the current library. The existing ``steering_controllers_library`` and its controllers will remain untouched, ensuring zero disruption for current users.
21+
22+
The new controller will be built on ``pluginlib``, separating the generic control logic from the specific mathematical models.
23+
24+
Core Components
25+
---------------
26+
27+
**1. The GenericSteeringController**
28+
29+
This will be the main controller class, inheriting from ``controller_interface::ChainableControllerInterface``. Its responsibilities will be limited to:
30+
31+
* Interfacing with the ``ros2_control`` framework and hardware interfaces.
32+
* Managing ROS 2 parameters, subscribers, and publishers.
33+
* Loading the appropriate kinematic model plugin at runtime based on a YAML parameter.
34+
* Calling the plugin's methods within the real-time update loop.
35+
36+
The controller itself will have **zero knowledge** of any specific robot kinematics.
37+
38+
**2. The KinematicModelBase Interface**
39+
40+
A new abstract base class, ``KinematicModelBase``, will be created to define the "contract" that all kinematic plugins must adhere to. This ensures a standard interface between the generic controller and any kinematic model.
41+
.. note::
42+
To enforce a clean public API and proper separation of concerns, this base class will be defined in its own header file (e.g., ``kinematic_model_base.hpp``).
43+
44+
45+
46+
.. code-block:: cpp
47+
48+
// A simplified view of the proposed interface
49+
namespace generic_steering_controller
50+
{
51+
class KinematicModelBase
52+
{
53+
public:
54+
virtual ~KinematicModelBase() = default;
55+
56+
// Called once to allow the plugin to configure itself
57+
virtual void configure(
58+
const rclcpp::Node::SharedPtr & node,
59+
const std::vector<std::string> & traction_joints,
60+
const std::vector<std::string> & steering_joints) = 0;
61+
62+
// Called in the real-time loop to calculate commands
63+
virtual std::pair<std::vector<double>, std::vector<double>>
64+
get_commands(double linear_vel, double angular_vel) = 0;
65+
66+
// Called in the real-time loop to update odometry
67+
virtual void update_odometry(const rclcpp::Duration & period) = 0;
68+
};
69+
}
70+
71+
Configuration Example
72+
---------------------
73+
74+
The configuration will be clean and two-tiered. The main controller reads its parameters, and the loaded plugin reads its own.
75+
76+
.. code-block:: yaml
77+
78+
generic_steering_controller:
79+
ros__parameters:
80+
# 1. Controller loads this plugin by name
81+
kinematics_plugin_name: "six_wheel_kinematics/SixWheelKinematics"
82+
83+
# Controller gets the joint lists
84+
traction_joints: ["j1", "j2", "j3", "j4", "j5", "j6"]
85+
steering_joints: ["s1", "s2", "s3", "s4"]
86+
87+
# 2. The loaded plugin reads its own specific parameters
88+
six_wheel_kinematics:
89+
ros__parameters:
90+
d1: 0.4
91+
d2: 0.5
92+
d3: 0.5
93+
d4: 0.45
94+
wheel_radius: 0.15
95+

0 commit comments

Comments
 (0)