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 R2025a 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-2026 Garth Zeglin. This file is explicitly placed in the public domain.
10# template language: javascript
11PROTO wobbly [
12 field SFVec3f translation 0 0 0
13 field SFRotation rotation 0 1 0 0
14 field SFString controller "wobbly"
15 field SFString name "Wobbly"
16 field SFFloat wheelRadius 0.1
17 field SFFloat axleLength 0.14
18 field SFColor bodyColor 0.0820075 0.364731 0.8
19 field SFColor wheelColor 1 0 0
20 field SFString customData ""
21]
22{
23 Robot {
24 # connect properties to user-visible data fields
25 translation IS translation
26 rotation IS rotation
27 controller IS controller
28 name IS name
29 customData IS customData
30
31 # calculate derived parameters
32 %<
33 let counterweightHeight = fields.wheelRadius.value
34 let counterweightOffset = 0.75*fields.wheelRadius.value
35 let counterweightTop = 1.25*fields.wheelRadius.value
36 let sensorHeight = 3*fields.wheelRadius.value
37 let stalkHeight = sensorHeight - counterweightTop
38 let stalkOffset = counterweightTop + 0.5*stalkHeight
39 >%
40
41 children [
42 # The body group contains three cylinders: the massive counterweight at
43 # bottom, a thin stalk rising above it, topped by the sensor disc.
44 # All these shapes participate in collision; the axle is kept separate
45 # as it is just for rendering.
46 DEF bodyShape Group {
47 children [
48 # counterweight
49 Transform {
50 translation 0 0 %<= counterweightOffset >%
51 rotation 1 0 0 0
52 children [
53 Shape {
54 appearance DEF bodyAppearance PBRAppearance {
55 baseColor IS bodyColor
56 roughness 0.5
57 metalness 0.5
58 }
59
60 geometry Cylinder {
61 height %<= counterweightHeight >%
62 radius 0.05
63 }
64 }
65 ]
66 }
67 # sensor disc
68 Transform {
69 translation 0 0 %<= sensorHeight >%
70 rotation 1 0 0 0
71 children [
72 Shape {
73 appearance USE bodyAppearance
74 geometry Cylinder {
75 height 0.01
76 radius 0.025
77 }
78 }
79 ]
80 }
81 # stalk
82 Transform {
83 translation 0 0 %<= stalkOffset >%
84 rotation 1 0 0 0
85 children [
86 Shape {
87 appearance USE bodyAppearance
88 geometry Cylinder {
89 height %<= stalkHeight >%
90 radius 0.01
91 }
92 }
93 ]
94 }
95 ]
96 }
97 # Visible axle shape, not part of the boundingObject.
98 DEF axleShape Transform {
99 translation 0 0 %<= fields.wheelRadius.value >%
100 rotation 1 0 0 1.5707963267948966
101 children [
102 Shape {
103 appearance USE bodyAppearance
104 geometry Cylinder {
105 height %<= fields.axleLength.value + 0.02 >%
106 radius 0.005
107 }
108 }
109 ]
110 }
111 # Define the left wheel axis, pointing in the +Y direction along the axle.
112 HingeJoint {
113 jointParameters HingeJointParameters {
114 axis 0 1 0
115 anchor 0 0 %<= fields.wheelRadius.value >%
116 }
117 device [
118 RotationalMotor {
119 name "left wheel motor"
120 acceleration 10
121 maxTorque 2
122 }
123 PositionSensor {
124 name "left wheel sensor"
125 }
126 ]
127 # Define the left wheel solid, offset along +Y along the axle.
128 endPoint DEF leftWheel Solid {
129 translation 0 %<= 0.5*fields.axleLength.value >% %<= fields.wheelRadius.value >%
130 rotation -1 0 0 1.5707963267948966
131 children [
132 # Define the left wheel shape and appearance, which is used by the right wheel solid.
133 DEF wheelShape Transform {
134 rotation 0 0 1 0
135 children [
136 Shape {
137 appearance PBRAppearance {
138 baseColor IS wheelColor
139 roughness 0.5
140 metalness 0.5
141 }
142 geometry Cylinder {
143 height 0.01
144 radius %<= fields.wheelRadius.value >%
145 }
146 }
147 ]
148 }
149 ]
150 name "left wheel"
151 boundingObject USE wheelShape
152 physics DEF wheelPhysics Physics {
153 density 600
154 }
155 }
156 }
157 # Define the right wheel axis, also pointing in the +Y direction along the axle
158 # so positive wheel rotations will move forward.
159 HingeJoint {
160 jointParameters HingeJointParameters {
161 axis 0 1 0
162 anchor 0 0 %<= fields.wheelRadius.value >%
163 }
164 device [
165 RotationalMotor {
166 name "right wheel motor"
167 acceleration 10
168 maxTorque 2
169 }
170 PositionSensor {
171 name "right wheel sensor"
172 }
173 ]
174 # Define the right wheel solid, offset along -Y along the axle.
175 # The wheel shape is inherited from the left wheel.
176 endPoint DEF rightWheel Solid {
177 translation 0 %<= -0.5*fields.axleLength.value >% %<= fields.wheelRadius.value >%
178 rotation 1 0 0 1.5707963267948966
179 children [
180 USE wheelShape
181 ]
182 name "right wheel"
183 boundingObject USE wheelShape
184 physics USE wheelPhysics
185 }
186 }
187 # Define the two range sensors. They are located on the sensor disc on
188 # top of the body stalk, each pointed off-axis by 1.0 radian.
189 DEF leftEyeSensor DistanceSensor {
190 translation 0.019 0.016 %<= sensorHeight >%
191 rotation 0 0 1 0.7
192 children [
193 DEF eyeShape Transform {
194 rotation 0 1 0 1.57
195 children [
196 Shape {
197 appearance PBRAppearance {
198 baseColor 0.975691 0.981481 0.0252992
199 roughness 0.5
200 metalness 0.5
201 }
202 geometry Cylinder {
203 height 0.004
204 radius 0.005
205 }
206 }
207 ]
208 }
209 ]
210 name "leftDistanceSensor"
211 lookupTable [
212 0 0.025 0
213 1 1 0
214 ]
215 numberOfRays 2
216 aperture 0.4
217 }
218 DEF rightEyeSensor DistanceSensor {
219 translation 0.019 -0.016 %<= sensorHeight >%
220 rotation 0 0 1 -0.7
221 children [
222 USE eyeShape
223 ]
224 name "rightDistanceSensor"
225 lookupTable [
226 0 0.025 0
227 1 1 0
228 ]
229 numberOfRays 2
230 aperture 0.4
231 }
232 # add a default radio receiver and transmitter
233 Receiver {
234 }
235 Emitter {
236 }
237 # add a position-sensing 'GPS' device located at the center of the axle
238 GPS {
239 translation 0 0 %<= fields.wheelRadius.value >%
240 }
241 # add a rotation-sensing 'compass' returning only the X and Y components of the North vector
242 Compass {
243 zAxis FALSE
244 }
245 # add an accelerometer located at the center of the axle to measure both gravity and reactive forces
246 Accelerometer {
247 translation 0 0 %<= fields.wheelRadius.value >%
248 }
249
250 ] # close children list of Robot
251 boundingObject USE bodyShape
252 physics Physics {
253 density -1
254 mass 3
255 }
256 } # close Robot
257}
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-2026, 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.getString()
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]
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()