Wobbly Robot Model¶
The wobbly robot model simulates a two-wheeled differential-drive mobile
robot with a low center of gravity. The body is designed with enough ground
clearance to tilt, but the low center of mass will wobble it back upright. The
body includes a mast with two distance sensors, a radio receiver and emitter,
GPS receiver for location, compass for heading, and three-axis accelerometer.
Each wheel has both a rotational motor and a position sensor.
The shape is constructed entirely of primitive cylinders.The same geometry is referenced to use as bounding objects for collision and automatic calculation of physics parameters.
The user-accessible parameters include the wheel radius, axle length, body color, and wheel color. The kinematics, shape, and physics will parametrically scale, but the actuator parameters do not change.
This model is demonstrated in the wobbly-demo.wbt world.
Screenshot of Webots model of a diff-drive ‘wobbly’ robot with low center of gravity.¶
System Kinematics and Components¶
The bodies are as follows:
name |
color |
notes |
|---|---|---|
body |
blue |
the core solid including counterweight, stalk, axle, and sensor disc |
left wheel |
red |
left wheel disc, cylinder with parameterized radius |
right wheel |
red |
right wheel disc, cylinder with parameterized radius |
The joints are as follows:
name |
parent |
child |
notes |
|---|---|---|---|
(6DOF free) |
world |
body |
mobile body; body +X axis is ‘forward’, +Z is ‘up’ |
left wheel |
body |
left wheel |
rotational joint on axle, parameterized Z position |
right wheel |
body |
right wheel |
rotational joint on axle, parameterized Z position |
The joint axes are as follows:
name |
direction |
notes |
|---|---|---|
left wheel |
along +Y |
along axle; direction chosen so positive rotation moves forward |
right wheel |
along +Y |
along axle; direction chosen so positive rotation moves forward |
The motors and sensors are named as follows:
name |
notes |
|---|---|
left wheel motor |
RotationalMotor on left wheel |
right wheel motor |
RotationalMotor on right wheel |
left wheel sensor |
PositionSensor on left wheel |
right wheel sensor |
PositionSensor on right wheel |
leftDistanceSensor |
DistanceSensor near top of body, aimed left of enter |
rightDistanceSensor |
DistanceSensor near top of body, aimed right of center |
gps |
GPS locator mounted on body in center of axle |
compass |
compass mounted on body in center of axle |
accelerometer |
three-axis accelerometer mounted on body in center of axle |
receiver |
radio Receiver |
emitter |
radio Emitter |
wobbly.proto¶
The robot model has been encapsulated in a .proto file for easy reuse. The model includes user-accessible scaling and color parameters.
1#VRML_SIM R2022a utf8
2# Two-wheeled differential-drive mobile robot with a low center of gravity. The
3# body is designed with enough ground clearance to tilt, but the low center of
4# mass will wobble it back upright. The body includes a mast with two distance
5# sensors, a radio receiver and emitter, and a GPS receiver. Each wheel has
6# both a rotational motor and a position sensor. The shape is constructed
7# entirely of primitive cylinders.
8# documentation url: https://courses.ideate.cmu.edu/16-375
9# license: No copyright, 2020-2022 Garth Zeglin. This file is explicitly placed in the public domain.
10PROTO wobbly [
11 field SFVec3f translation 0 0 0
12 field SFRotation rotation 0 1 0 0
13 field SFString controller "wobbly"
14 field SFString name "Wobbly"
15 field SFFloat wheelRadius 0.1
16 field SFFloat axleLength 0.14
17 field SFColor bodyColor 0.0820075 0.364731 0.8
18 field SFColor wheelColor 1 0 0
19 field SFString customData ""
20]
21{
22 Robot {
23 # connect properties to user-visible data fields
24 translation IS translation
25 rotation IS rotation
26 controller IS controller
27 name IS name
28 customData IS customData
29
30 # calculate derived parameters
31 %{
32 local counterweightHeight = fields.wheelRadius.value
33 local counterweightOffset = 0.75*fields.wheelRadius.value
34 local counterweightTop = 1.25*fields.wheelRadius.value
35 local sensorHeight = 3*fields.wheelRadius.value
36 local stalkHeight = sensorHeight - counterweightTop
37 local stalkOffset = counterweightTop + 0.5*stalkHeight
38 }%
39
40 children [
41 # The body group contains three cylinders: the massive counterweight at
42 # bottom, a thin stalk rising above it, topped by the sensor disc.
43 # All these shapes participate in collision; the axle is kept separate
44 # as it is just for rendering.
45 DEF bodyShape Group {
46 children [
47 # counterweight
48 Transform {
49 translation 0 0 %{= counterweightOffset }%
50 rotation 1 0 0 0
51 children [
52 Shape {
53 appearance DEF bodyAppearance PBRAppearance {
54 baseColor IS bodyColor
55 roughness 0.5
56 metalness 0.5
57 }
58
59 geometry Cylinder {
60 height %{= counterweightHeight }%
61 radius 0.05
62 }
63 }
64 ]
65 }
66 # sensor disc
67 Transform {
68 translation 0 0 %{= sensorHeight }%
69 rotation 1 0 0 0
70 children [
71 Shape {
72 appearance USE bodyAppearance
73 geometry Cylinder {
74 height 0.01
75 radius 0.025
76 }
77 }
78 ]
79 }
80 # stalk
81 Transform {
82 translation 0 0 %{= stalkOffset }%
83 rotation 1 0 0 0
84 children [
85 Shape {
86 appearance USE bodyAppearance
87 geometry Cylinder {
88 height %{= stalkHeight }%
89 radius 0.01
90 }
91 }
92 ]
93 }
94 ]
95 }
96 # Visible axle shape, not part of the boundingObject.
97 DEF axleShape Transform {
98 translation 0 0 %{= fields.wheelRadius.value }%
99 rotation 1 0 0 1.5707963267948966
100 children [
101 Shape {
102 appearance USE bodyAppearance
103 geometry Cylinder {
104 height %{= fields.axleLength.value + 0.02 }%
105 radius 0.005
106 }
107 }
108 ]
109 }
110 # Define the left wheel axis, pointing in the +Y direction along the axle.
111 HingeJoint {
112 jointParameters HingeJointParameters {
113 axis 0 1 0
114 anchor 0 0 %{= fields.wheelRadius.value }%
115 }
116 device [
117 RotationalMotor {
118 name "left wheel motor"
119 acceleration 10
120 maxTorque 2
121 }
122 PositionSensor {
123 name "left wheel sensor"
124 }
125 ]
126 # Define the left wheel solid, offset along +Y along the axle.
127 endPoint DEF leftWheel Solid {
128 translation 0 %{= 0.5*fields.axleLength.value }% %{= fields.wheelRadius.value }%
129 rotation -1 0 0 1.5707963267948966
130 children [
131 # Define the left wheel shape and appearance, which is used by the right wheel solid.
132 DEF wheelShape Transform {
133 rotation 0 0 1 0
134 children [
135 Shape {
136 appearance PBRAppearance {
137 baseColor IS wheelColor
138 roughness 0.5
139 metalness 0.5
140 }
141 geometry Cylinder {
142 height 0.01
143 radius %{= fields.wheelRadius.value }%
144 }
145 }
146 ]
147 }
148 ]
149 name "left wheel"
150 boundingObject USE wheelShape
151 physics DEF wheelPhysics Physics {
152 density 600
153 }
154 }
155 }
156 # Define the right wheel axis, also pointing in the +Y direction along the axle
157 # so positive wheel rotations will move forward.
158 HingeJoint {
159 jointParameters HingeJointParameters {
160 axis 0 1 0
161 anchor 0 0 %{= fields.wheelRadius.value }%
162 }
163 device [
164 RotationalMotor {
165 name "right wheel motor"
166 acceleration 10
167 maxTorque 2
168 }
169 PositionSensor {
170 name "right wheel sensor"
171 }
172 ]
173 # Define the right wheel solid, offset along -Y along the axle.
174 # The wheel shape is inherited from the left wheel.
175 endPoint DEF rightWheel Solid {
176 translation 0 %{= -0.5*fields.axleLength.value }% %{= fields.wheelRadius.value }%
177 rotation 1 0 0 1.5707963267948966
178 children [
179 USE wheelShape
180 ]
181 name "right wheel"
182 boundingObject USE wheelShape
183 physics USE wheelPhysics
184 }
185 }
186 # Define the two range sensors. They are located on the sensor disc on
187 # top of the body stalk, each pointed off-axis by 1.0 radian.
188 DEF leftEyeSensor DistanceSensor {
189 translation 0.019 0.016 %{= sensorHeight }%
190 rotation 0 0 1 0.7
191 children [
192 DEF eyeShape Transform {
193 rotation 0 1 0 1.57
194 children [
195 Shape {
196 appearance PBRAppearance {
197 baseColor 0.975691 0.981481 0.0252992
198 roughness 0.5
199 metalness 0.5
200 }
201 geometry Cylinder {
202 height 0.004
203 radius 0.005
204 }
205 }
206 ]
207 }
208 ]
209 name "leftDistanceSensor"
210 lookupTable [
211 0 0.025 0
212 1 1 0
213 ]
214 numberOfRays 2
215 aperture 0.4
216 }
217 DEF rightEyeSensor DistanceSensor {
218 translation 0.019 -0.016 %{= sensorHeight }%
219 rotation 0 0 1 -0.7
220 children [
221 USE eyeShape
222 ]
223 name "rightDistanceSensor"
224 lookupTable [
225 0 0.025 0
226 1 1 0
227 ]
228 numberOfRays 2
229 aperture 0.4
230 }
231 # add a default radio receiver and transmitter
232 Receiver {
233 }
234 Emitter {
235 }
236 # add a position-sensing 'GPS' device located at the center of the axle
237 GPS {
238 translation 0 0 %{= fields.wheelRadius.value }%
239 }
240 # add a rotation-sensing 'compass' returning only the X and Y components of the North vector
241 Compass {
242 zAxis FALSE
243 }
244 # add an accelerometer located at the center of the axle to measure both gravity and reactive forces
245 Accelerometer {
246 translation 0 0 %{= fields.wheelRadius.value }%
247 }
248
249 ] # close children list of Robot
250 boundingObject USE bodyShape
251 physics Physics {
252 density -1
253 mass 3
254 }
255 } # close Robot
256}
Sample Control Code¶
1# wobbly.py
2
3# Sample Webots controller file for driving the wobbly diff-drive
4# mobile robot.
5
6# No copyright, 2020-2021, Garth Zeglin. This file is
7# explicitly placed in the public domain.
8
9print("loading wobbly.py...")
10
11# Import the Webots simulator API.
12from controller import Robot
13
14# Import standard Python libraries.
15import math, random, time
16
17# Define the time step in milliseconds between controller updates.
18EVENT_LOOP_DT = 200
19
20################################################################
21class Wobbly(Robot):
22 def __init__(self):
23
24 super(Wobbly, self).__init__()
25 self.robot_name = self.getName()
26 print("%s: controller connected." % (self.robot_name))
27
28 # Attempt to randomize the random library sequence.
29 random.seed(time.time())
30
31 # Initialize geometric constants. These should match
32 # the current geometry of the robot.
33 self.wheel_radius = 0.1
34 self.axle_length = 0.14
35
36 # Fetch handles for the wheel motors
37 self.l_motor = self.getDevice('left wheel motor')
38 self.r_motor = self.getDevice('right wheel motor')
39
40 # Adjust the low-level controller gains.
41 print("%s: setting PID gains." % (self.robot_name))
42 self.l_motor.setControlPID(1.0, 0.0, 0.1)
43 self.r_motor.setControlPID(1.0, 0.0, 0.1)
44
45 # Fetch handles for the wheel joint sensors.
46 self.l_pos_sensor = self.getDevice('left wheel sensor')
47 self.r_pos_sensor = self.getDevice('right wheel sensor')
48
49 # Specify the sampling rate for the joint sensors.
50 self.l_pos_sensor.enable(EVENT_LOOP_DT)
51 self.r_pos_sensor.enable(EVENT_LOOP_DT)
52
53 # Connect to the eye sensors.
54 self.l_eye_sensor = self.getDevice('leftDistanceSensor')
55 self.r_eye_sensor = self.getDevice('rightDistanceSensor')
56 self.l_eye_sensor.enable(EVENT_LOOP_DT)
57 self.r_eye_sensor.enable(EVENT_LOOP_DT)
58
59 # Connect to the radio emitter and receiver.
60 self.receiver = self.getDevice('receiver')
61 self.emitter = self.getDevice('emitter')
62 self.radio_interval = 1000
63 self.radio_timer = 0
64 self.receiver.enable(self.radio_interval)
65
66 # Maintain a table of peer robot locations received over the radio.
67 self.peers = {}
68
69 # Connect to the GPS position sensor.
70 self.gps = self.getDevice('gps')
71 self.gps_timer = 0
72 self.gps_interval = 1000
73 self.gps.enable(self.gps_interval)
74 self.gps_location = [0, 0, 0.1] # reference pose value
75
76 # Connect to the compass orientation sensor.
77 self.compass = self.getDevice('compass')
78 self.compass_timer = 0
79 self.compass_interval = 200
80 self.compass.enable(self.compass_interval)
81
82 # State variables for reporting compass readings. The heading is the
83 # body forward (body +X) direction expressed in degrees clockwise from
84 # North (world +Y). The compass_vector is the direction of North
85 # expressed in body coordinates. The neutral pose points East.
86 self.heading = 90
87 self.compass_vector = [0,1] # reference pose value
88 self.heading_error = 0
89
90 # Connect to the accelerometer sensor.
91 self.accelerometer = self.getDevice('accelerometer')
92 self.accelerometer_timer = 0
93 self.accelerometer_interval = 200
94 self.accelerometer.enable(self.accelerometer_interval)
95 self.accel_vector = [0, 0, 9.81] # reference pose value
96
97 # Initialize generic behavior state machine variables.
98 self.state_timer = 0 # timers in milliseconds
99 self.state_index = 0 # current state
100 self.target_heading = 0
101 self.target_velocity = 0
102 return
103
104 #================================================================
105 # Polling function to process sensor input at different timescales.
106 def poll_sensors(self):
107
108 self.gps_timer -= EVENT_LOOP_DT
109 if self.gps_timer < 0:
110 self.gps_timer += self.gps_interval
111 location = self.gps.getValues()
112 if not math.isnan(location[0]):
113 self.gps_location = location
114 # print("%s GPS: %s" % (self.robot_name, location))
115
116 self.compass_timer -= EVENT_LOOP_DT
117 if self.compass_timer < 0:
118 self.compass_timer += self.compass_interval
119 orientation = self.compass.getValues()
120 if not math.isnan(orientation[0]) and not math.isnan(orientation[1]):
121 # For heading, 0 degrees is North, 90 is East, 180 is South, 270 is West.
122 # The world is assumed configured 'ENU' so X is East and Y is North.
123 # The robot 'front' is along body +X, so the neutral pose is facing East.
124 self.heading = math.fmod(2*math.pi + math.atan2(orientation[1], orientation[0]), 2*math.pi) * (180.0/math.pi)
125 self.compass_vector = orientation[0:2]
126 # print("%s Compass: %s, heading %3.0f deg" % (self.robot_name, self.compass_vector, heading))
127
128 self.accelerometer_timer -= EVENT_LOOP_DT
129 if self.accelerometer_timer < 0:
130 self.accelerometer_timer += self.accelerometer_interval
131 self.accel_vector = self.accelerometer.getValues()
132 # The accelerometer will read [0, 0, 9.81] when stationary in the reference pose.
133 # print("%s Accelerometer: %s" % (self.robot_name, self.accel_vector))
134
135 return
136
137 #================================================================
138 # Polling function to process radio and network input at different timescales.
139 def poll_communication(self):
140 self.radio_timer -= EVENT_LOOP_DT
141 if self.radio_timer < 0:
142 self.radio_timer += self.radio_interval
143 while self.receiver.getQueueLength() > 0:
144 packet = self.receiver.getData()
145 # print("%s Receiver: %s" % (self.robot_name, packet))
146 tokens = packet.split()
147 if len(tokens) != 5:
148 print("%s malformed packet: %s" % (self.robot_name, packet))
149 else:
150 name = tokens[0].decode() # convert bytestring to Unicode
151 if self.peers.get(name) is None:
152 print("%s receiver: new peer observed: %s" % (self.robot_name, name))
153
154 self.peers[name] = {'location' : [float(tokens[1]), float(tokens[2]), float(tokens[3])],
155 'heading' : float(tokens[4]),
156 'timestamp' : self.getTime(),
157 }
158
159 # done with packet processing
160 self.receiver.nextPacket()
161
162 # Transmit a status message at the same rate
163 name_token = self.robot_name.replace(" ","_")
164 status = "%s %.2f %.2f %.2f %.0f" % (name_token, self.gps_location[0], self.gps_location[1],
165 self.gps_location[2] - self.wheel_radius, self.heading)
166
167 # emitter requires a bytestring, not a Python Unicode string
168 data = status.encode()
169
170 # print("%s emitter: sending %s" % (self.robot_name, data))
171 self.emitter.send(data)
172
173 #================================================================
174 # motion primitives
175 def go_forward(self, velocity):
176 """Command the motor to turn at the rate which produce the ground velocity
177 specified in meters/sec. Negative values turn backward. """
178
179 # velocity control mode
180 self.l_motor.setPosition(math.inf)
181 self.r_motor.setPosition(math.inf)
182
183 # calculate the rotational rate in radians/sec based on the wheel radius
184 theta_dot = velocity / self.wheel_radius
185 self.l_motor.setVelocity(theta_dot)
186 self.r_motor.setVelocity(theta_dot)
187 return
188
189 def go_rotate(self, rot_velocity):
190 """Command the motors to turn in place at the rate which produce the rotational
191 velocity specified in radians/sec. Negative values turn
192 backward."""
193
194 # velocity control mode
195 self.l_motor.setPosition(math.inf)
196 self.r_motor.setPosition(math.inf)
197
198 # calculate the difference in linear velocity of the wheels
199 linear_velocity = self.axle_length * rot_velocity
200
201 # calculate the net rotational rate in radians/sec based on the wheel radius
202 theta_dot = linear_velocity / self.wheel_radius
203
204 # apply the result symmetrically to the wheels
205 self.l_motor.setVelocity( 0.5*theta_dot)
206 self.r_motor.setVelocity(-0.5*theta_dot)
207 return
208
209 def heading_difference(self, target, current):
210 """Calculate a directional error in degrees, always returning a value on (-180, 180]."""
211 err = target - current
212 # fold the range of values to (-180, 180]
213 if err > 180.0:
214 return err - 360
215 elif err <= -180.0:
216 return err + 360
217 else:
218 return err
219
220 def go_heading(self, target_heading):
221 """Rotate toward the heading specifed in positive degrees, with 0 at North (+Y),
222 90 at East (+X). This assume the compass-reading process is
223 active."""
224
225 # find the directional error in degrees
226 self.heading_error = self.heading_difference(target_heading, self.heading)
227
228 # apply a linear mapping from degrees error to rotational velocity in radians/sec
229 rot_vel = 0.02 * self.heading_error
230 self.go_rotate(rot_vel)
231 # print("go_heading: %f, %f, %f" % (target_heading, self.heading, rot_vel))
232 return
233
234 def go_still(self):
235 """Actively damp any wobble to come to rest in place."""
236 # map an error in X acceleration to a linear velocity
237 vel = -0.05 * self.accel_vector[0]
238 self.go_forward(vel)
239 # print("go_still: %f, %f" % (self.accel_vector[0], vel))
240 return
241
242 #================================================================
243 def peer_heading_distance(self, record):
244 """Given a peer record, return a tuple (heading, distance) with the compass
245 heading and distance in meters of the peer from this robot current
246 location."""
247 loc = record['location']
248 dx = loc[0] - self.gps_location[0]
249 dy = loc[1] - self.gps_location[1]
250 distance = math.sqrt(dx*dx + dy*dy)
251 heading = math.fmod(2*math.pi + math.atan2(dx, dy), 2*math.pi) * (180.0/math.pi)
252 return heading, distance
253
254 def nearest_peer(self, range=2.0):
255 """Locate the nearest peer (as reported by radio) within the given range.
256 Returns either None or a dictionary with the location record."""
257 result = None
258 best = math.inf
259 for name in self.peers:
260 record = self.peers[name]
261 heading, dist = self.peer_heading_distance(record)
262 if dist < best:
263 result = record
264 best = dist
265 return result
266
267 #================================================================
268 def poll_wandering_activity(self):
269 """State machine update function to aimlessly wander around the world."""
270
271 # This machine always transitions at regular intervals.
272 timer_expired = False
273 if self.state_timer < 0:
274 self.state_timer += 3000
275 timer_expired = True
276
277 # Evaluate the side-effects and transition rules for each state.
278 if self.state_index == 0:
279 print("Init state, entering cycle.")
280 self.state_index = 1
281
282 elif self.state_index == 1:
283 self.go_forward(0.2)
284
285 if timer_expired:
286 self.state_index += 1
287
288 elif self.state_index == 2:
289 self.go_heading(self.target_heading)
290
291 if timer_expired:
292 self.state_index += 1
293
294 elif self.state_index == 3:
295 self.go_still()
296
297 if timer_expired:
298 self.state_index += 1
299
300 elif self.state_index == 4:
301 self.go_rotate(math.pi / 6)
302
303 if timer_expired:
304 self.state_index = 1
305 self.target_heading = random.randint(0,360)
306
307 else:
308 print("%s: invalid state, resetting." % (self.robot_name))
309 self.state_index = 0
310
311 if timer_expired:
312 print("%s: transitioning to state %s" % (self.robot_name, self.state_index))
313
314 return
315
316 #================================================================
317 def poll_following_activity(self):
318 """State machine update function to always move toward the nearest peer."""
319
320 if self.state_timer < 0:
321 self.state_timer += 1000
322
323 # periodically test if there is a nearby peer
324 nearest = self.nearest_peer()
325 if nearest is None:
326 self.state_index = 1
327 else:
328 self.state_index = 2
329 heading, distance = self.peer_heading_distance(nearest)
330 self.target_heading = heading
331 self.target_velocity = 0.1 * distance
332 print("%s: peer to follow at %f deg, %f meters" % (self.robot_name, heading, distance))
333
334 # always either stabilize, turn, or move
335 if self.state_index < 1:
336 self.go_still()
337
338 else:
339 heading_err = self.heading_difference(self.target_heading, self.heading)
340 if abs(heading_err) > 20.0 or abs(self.target_velocity) < 0.05:
341 self.go_heading(self.target_heading)
342 else:
343 self.go_forward(self.target_velocity)
344
345 #================================================================
346 def run(self):
347 # Run loop to execute a periodic script until the simulation quits.
348 # If the controller returns -1, the simulator is quitting.
349 while self.step(EVENT_LOOP_DT) != -1:
350 # Read simulator clock time.
351 self.sim_time = self.getTime()
352
353 # Read sensor values.
354 self.poll_sensors()
355
356 # Check the radio and/or network.
357 self.poll_communication()
358
359 # Update the activity state machine.
360 self.state_timer -= EVENT_LOOP_DT
361
362 # This will run some open-loop motion. One robot will be the leader, the rest will follow.
363 mode = self.getCustomData()
364 if mode == 'leader':
365 self.poll_wandering_activity()
366 else:
367 self.poll_following_activity()
368
369
370################################################################
371# Start the script.
372robot = Wobbly()
373robot.run()