27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191 | class TrikeModel:
"""
## Kinematic model of a trike
Trike model is a combination of a bycicle and and a differential drive kinematic models.
Key features are:
* the path curvature is governed by steering wheel
* movement command interface is `(velocity,steering angle)`
* differential speeds for the driving wheels are calculated from the driving curvature.
!!! note
The steering wheel axle can be behind driving wheels resulting in rear wheel steering
To achieve this, use negative `L` value.
### Geometry
!!! note
Axes in Geogebra model are different from the ones used in this model.
This model uses right handed coordinate system with x-axis pointing forward, y-axis pointing to the left and z-axis pointing up.
<iframe src="https://www.geogebra.org/calculator/rea6w9a2?embed" width="800" height="800" allowfullscreen style="border: 1px solid #e4e4e4;border-radius: 4px;" frameborder="0"></iframe>
"""
__slots__ = (
"L",
"B",
"wheel_diameter",
"left_wheel",
"right_wheel",
"wheels",
"steer",
"target_velocity",
"xy",
"theta",
"time",
)
def __init__(
self,
L: float = 1.0,
B: float = 0.8,
wheel_diameter: float = 0.4,
wheel_accel: float = 1.0,
steer_speed: float = math.radians(10.0),
max_velocity: Optional[float] = None,
) -> None:
"""create kinematic model of a trike
Args:
L (float, optional): length, distance between front and back axles. Defaults to 1.0.
B (float, optional): wheel base. Defaults to 0.8.
wheel_diameter (float, optional): Driving wheel diameter. Defaults to 0.4.
wheel_accel (float, optional): Driving wheel acceleration [m/s2]. Defaults to 1.0.
steer_speed (float, optional): Steering wheel speed [rad/s]. Defaults to 10.0 deg.
max_velocity (Optional[float], optional): maximum wheel velocity [m/s]. Defaults to None.
"""
self.L = L
self.B = B
self.wheel_diameter = wheel_diameter
self.target_velocity: float = 0.0
# define wheels
self.left_wheel = Wheel(wheel_diameter, wheel_accel, max_velocity=max_velocity)
self.right_wheel = Wheel(wheel_diameter, wheel_accel, max_velocity=max_velocity)
self.wheels = [self.left_wheel, self.right_wheel]
# steering wheel model
self.steer = LinearModel(roc=steer_speed)
# position
self.xy = Vector()
self.theta: float = 0.0
# time
self.time: float = 0.0
@property
def velocity(self) -> float:
"""actual velocity in m/s"""
return (self.wheels[0].velocity_ms + self.wheels[1].velocity_ms) / 2
@property
def steering_angle(self) -> float:
"""steering angle in radians"""
return self.steer.val
@property
def target_steer(self) -> float:
"""target steering angle in radians"""
return self.steer.setpoint
@target_steer.setter
def target_steer(self, angle: float) -> None:
"""set target steering angle in radians"""
self.steer.setpoint = angle
@property
def curvature(self) -> float:
"""driving curvature"""
return math.tan(self.steering_angle) / self.L
@property
def wheel_targets(self) -> tuple[float, float]:
"""driving wheel target velocities in m/s"""
# curvature - left turn is positive, right turn is negative
cb = self.curvature * self.B / 2 # curvature * half wheel base
if self.curvature < 0: # right turn, left wheel is faster
return (
self.target_velocity * (1 - cb),
self.wheels[0].velocity_ms * (1 + cb) / (1 - cb),
)
if self.curvature > 0: # left turn, right wheel is faster
return (
self.wheels[1].velocity_ms * (1 - cb) / (1 + cb),
self.target_velocity * (1 + cb),
)
# going straight
return (self.target_velocity, self.target_velocity)
# ---- old too simple code ----
# return (
# self.target_velocity * (1 - c * self.B / 2),
# self.target_velocity * (1 + c * self.B / 2),
# )
@property
def pose(self) -> Tuple[float, float, float]:
return (self.xy.x, self.xy.y, self.theta)
def step(self, dt: float) -> None:
"""update models and internal odometry, motion is governed by the steering wheel, driving wheels may slip"""
# update steering wheel
self.steer.step(dt)
# update driving wheel targets
for wheel, target in zip(self.wheels, self.wheel_targets):
wheel.set_velocity_ms(target)
wheel.step(dt)
# update position
# linear distance travelled
dS = (self.wheels[0].ds + self.wheels[1].ds) / 2
# update heading
self.theta += dS * self.curvature
self.xy += dS * Vector(math.cos(self.theta), math.sin(self.theta))
# update time
self.time += dt
|