Array¶
This sample project includes a robot which consists of a grid of actuators driving pivoting passive elements. This model demonstrates several key concepts: underactuated systems, generative movement, and scripted modeling.
This model is demonstrated in the array.wbt world. The base link for all
actuators has a NULL Physics object so it does not move, simulating a rigid
connection to the ground. Each link has a simple box boundingObject for
collision detection and physics mass parameter calculations.
Screenshot of Webots model of the array robot.¶
Sample Robot Control Code¶
1# array.py
2#
3# Sample Webots controller file for driving the array
4# of underactuated two-joint devices.
5#
6# No copyright, 2020-2023, Garth Zeglin. This file is
7# explicitly placed in the public domain.
8
9print("loading array.py...")
10
11import math
12import random
13import numpy as np
14
15# Import the Webots simulator API.
16from controller import Supervisor
17
18# Define the time step in milliseconds between controller updates.
19EVENT_LOOP_DT = 20
20
21################################################################
22# The sample controller is defined as an class which is then instanced as a
23# singleton control object. This is conventional Python practice and also
24# is closer to practices involving the physical hardware.
25
26class ArrayRobot(Supervisor):
27 def __init__(self):
28
29 # Initialize the superclass which implements the Robot API.
30 super().__init__()
31
32 robot_name = self.getName()
33 print("%s: controller connected." % (robot_name))
34
35 # Read back the array dimensions. This is possible because the Robot is
36 # configured with Supervisor permissions.
37 num_x = self.getSelf().getField('num_x').value
38 num_y = self.getSelf().getField('num_y').value
39 print("Array has %d x %d units." % (num_x, num_y))
40
41 # Fetch handles for the motors into a 2D array. The naming scheme is defined in
42 # the proto file for the array.
43 j = []
44 for x in range(1,num_x+1):
45 row = []
46 for y in range(1,num_y+1):
47 name = "motor%d%d" % (x, y)
48 # print("Fetching motor", name)
49 row.append(self.getDevice(name))
50 j.append(row)
51 self.joints = j
52
53 # Configure the motors for velocity control by setting
54 # the position targets to infinity.
55 for row in self.joints:
56 for motor in row:
57 motor.setPosition(float('inf'))
58 motor.setVelocity(0.0)
59
60 # Create a numpy array with a velocity for each motor.
61 self.velocity = np.zeros((num_x, num_y))
62
63 # Create some additional constant index arrays useful for generative functions.
64 # Each of these has the same shape as the motor array.
65 # x_mesh varies only the grid 'X' direction, y_mesh varies only along the grid 'Y' direction.
66 self.y_mesh, self.x_mesh = np.meshgrid(np.linspace(0, num_y-1, num_y), np.linspace(0, num_x-1, num_x))
67
68 # Initialize the performance state.
69 self.state = 'init'
70 self.new_state = True
71 self.state_timer = 0
72 self.current_time = 0.0
73
74 return
75
76 #----------------------------------------------------------------
77 # internal method to set the velocity of each motor to the current velocity grid value
78 def _update_motor(self):
79 for vel_row, motor_row in zip(self.velocity, self.joints):
80 for vel, motor in zip(vel_row, motor_row):
81 motor.setVelocity(vel)
82
83 #----------------------------------------------------------------
84 # set every motor to zero velocity
85 def stop_all(self):
86 self.velocity[:,:] = 0.0
87 self._update_motor()
88
89 # set every motor to a specific velocity
90 def unison(self, velocity):
91 self.velocity[:,:] = velocity
92 self._update_motor()
93
94 # mirror the left half velocity onto the right half
95 def lateral_mirror(self):
96 width = self.velocity.shape[1]
97 halfwidth = width // 2
98 self.velocity[:,halfwidth:width] = -self.velocity[:, (halfwidth-1)::-1]
99 self._update_motor()
100
101 # set every motor to a reasonable random velocity (positive and negative)
102 def randomize_velocity(self):
103 self.velocity = 3 - 6*np.random.rand(*self.velocity.shape)
104 self._update_motor()
105
106 # set every motor to wave function varying along the X direction. Note: in the sample model, the
107 # array has been rotated so this is vertical
108 def wave_x(self):
109 period = 3.0
110 phase_velocity = 2*math.pi / period
111 phase = self.x_mesh
112 self.velocity = 3 * np.sin((phase_velocity * self.current_time) + 0.5*phase)
113 self._update_motor()
114
115 # set every motor to wave function varying along the Y direction. Note: in the sample model, this
116 # is the horizontal direction across the room.
117 def wave_y(self):
118 period = 3.0
119 phase_velocity = 2*math.pi / period
120 phase = self.y_mesh
121 self.velocity = 1.5 + 1.5 * np.sin((phase_velocity * self.current_time) + 0.3*phase)
122 self._update_motor()
123
124 #----------------------------------------------------------------
125 # Implement state machine for the overall performance state, updated
126 # at the main event rate.
127 def poll_performance_state(self):
128
129 # implement a basic timer used by many states
130 timer_expired = False
131 self.state_timer -= 0.001*EVENT_LOOP_DT
132 if self.state_timer < 0:
133 timer_expired = True
134
135 # implement entry flag logic used by many state
136 new_state = self.new_state
137 self.new_state = False
138
139 # -----------------------------------
140
141 # Evaluate the side-effects and transition rules for each state
142 if self.state == 'init':
143 if new_state:
144 self.state_timer = 1.0 # initial pause
145 self.stop_all()
146
147 elif timer_expired:
148 self._transition('spinny')
149
150 elif self.state == 'spinny':
151 # apply side effects on entry
152 if new_state:
153 print("Entering", self.state)
154 self.state_timer = 3.0
155 self.unison(-6.0)
156 self.lateral_mirror()
157
158 # apply transition rules
159 elif timer_expired:
160 self._transition('horizontal_wave')
161
162 elif self.state == 'horizontal_wave':
163 # apply side effects on entry
164 if new_state:
165 print("Entering", self.state)
166 self.state_timer = 6.0
167
168 # apply transition rules
169 elif timer_expired:
170 self._transition('vertical_wave')
171
172 # apply continuous side-effects
173 else:
174 self.wave_y()
175
176 elif self.state == 'vertical_wave':
177 # apply side effects on entry
178 if new_state:
179 print("Entering", self.state)
180 self.state_timer = 6.0
181
182 # apply transition rules
183 elif timer_expired:
184 self._transition('tremble')
185
186 # apply continuous side-effects
187 else:
188 self.wave_x()
189
190 elif self.state == 'tremble':
191 # apply side effects on entry
192 if new_state:
193 print("Entering", self.state)
194 self.state_timer = 3.0
195
196 # apply transition rules
197 elif timer_expired:
198 self._transition('rest')
199
200 # apply continuous side-effects
201 else:
202 self.randomize_velocity()
203
204 elif self.state == 'rest':
205 if new_state:
206 print("Entering", self.state)
207 self.state_timer = 6.0
208 self.stop_all()
209
210 elif timer_expired:
211 self._transition('spinny')
212
213 else:
214 print("Invalid state, resetting.")
215 self.state = 'init'
216
217 # internal methods for state machine transitions
218 def _transition(self, new_state):
219 self.new_state = True
220 self.state = new_state
221 self.state_timer = 0
222
223 #----------------------------------------------------------------
224 def run(self):
225 # Run loop to execute a periodic script until the simulation quits.
226 # If the controller returns -1, the simulator is quitting.
227
228 while self.step(EVENT_LOOP_DT) != -1:
229 # Read simulator clock time.
230 self.current_time = self.getTime()
231
232 # Poll the performance state machine.
233 self.poll_performance_state()
234
235
236################################################################
237# If running directly from Webots as a script, the following will begin execution.
238# This will have no effect when this file is imported as a module.
239
240if __name__ == "__main__":
241 robot = ArrayRobot()
242 robot.run()
Proto File¶
The robot is modeled in a proto file to allow scripted generation of the units. The number of units can be varied after creation and the robot model will be regenerated. The proto file is VRML with embedded Javascript.
1#VRML_SIM R2023b utf8
2# documentation url: https://courses.ideate.cmu.edu/16-375
3# Array of underactuated two-body devices for course exercises. The graphics
4# use imported STL files for ease of customization. Each body uses a simple
5# bounding box for collision and physics. Each unit base has NULL physics so it
6# will be fixed in place.
7# license: No copyright, 2020-2026 Garth Zeglin. This file is explicitly placed in the public domain.
8# template language: javascript
9
10PROTO array [
11 field SFVec3f translation 0 0 0
12 field SFRotation rotation 0 1 0 0
13 field SFString controller "array"
14 field SFString name "array"
15 field SFInt32 num_x 4
16 field SFInt32 num_y 5
17 field SFFloat spacing_x 0.3
18 field SFFloat spacing_y 0.3
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 # enable the bodies within the system to directly interact
31 selfCollision TRUE
32
33 # enable additional API access for the controller to read back parameters
34 supervisor TRUE
35
36 children [
37 # 2D nested loops to create each motorized unit
38 %< for (let x = 1; x <= fields.num_x.value; x++) { >%
39 %< for (let y = 1; y <= fields.num_y.value; y++) { >%
40
41 # define individualized names
42 %< let motor_name = "\"motor" + x + y + "\""; >%
43 %< let sensor_name = "\"joint" + x + y + "\""; >%
44 %< let link1_name = "\"link1" + x + y + "\""; >%
45
46 # define the base of each unit representing the individual motor and mounting
47 Pose {
48 # locate the base to match each hinge joint location
49 translation %<= fields.spacing_x.value * (x-1) >% %<= fields.spacing_y.value * (y-1) >% 0.02
50 children [
51 Shape {
52 appearance PBRAppearance {
53 baseColor 0.180392 0 1
54 metalness 0.5
55 }
56 geometry Cylinder {
57 height 0.04
58 radius 0.1
59 }
60 }
61 ] # close children of base Pose
62 } # close the base Pose
63
64 # define the first pivot of each unit, representing a driven motor axis
65 HingeJoint {
66 jointParameters HingeJointParameters {
67 axis 0 0 1
68 # locate the body origin within the robot coordinates
69 anchor %<= fields.spacing_x.value * (x-1) >% %<= fields.spacing_y.value * (y-1) >% 0.02
70 }
71 device [
72 RotationalMotor {
73 name %<= motor_name >%
74 }
75 ]
76 # define the body of the first driven link of each unit
77 endPoint Solid {
78 # locate the link1 body origin within the overall robot coordinates
79 translation %<= fields.spacing_x.value * (x-1) >% %<= fields.spacing_y.value * (y-1) >% 0.05
80
81 # provide a unique name for the link1 Solid to avoid warnings
82 name %<= link1_name >%
83
84 # define the collision and physics properties for link1
85 boundingObject Box {
86 size 0.15 0.15 0.006
87 }
88 physics Physics {
89 density -1
90 mass 2
91 }
92
93 # define the link1 graphics
94 children [ # first link Solid
95 Pose {
96 # locate the link1 visible model within link2 coordinates
97 translation 0 0 -0.003
98 children [ # first link Pose
99 Shape {
100 appearance PBRAppearance {
101 baseColor 0.772549 0.2 0.192157
102 metalness 0.5
103 roughness 0.5
104 }
105 # import the visible geometry; the physics properties and
106 # collision use the boundingObject field which defines a
107 # simple box
108 geometry Mesh {
109 url ["../stl/arrow.stl"]
110 }
111 }
112 ] # close children of first link Pose
113 }
114
115 # define the second pivot of each unit, representing a passive rotation axis
116 HingeJoint {
117 jointParameters HingeJointParameters {
118 axis 0 0 1
119 # locate the joint axis within the link1 coordinates
120 anchor 0.050 0 0
121
122 # provide a slight friction to dissipate energy
123 dampingConstant 0.01
124 }
125
126 # define the body of the second (undriven) link of each unit
127 endPoint Solid {
128 # locate the origin of link2 within the link1 coordinates
129 translation 0.075 0 0.010
130
131 # define the collision and physics properties for link2
132 boundingObject Box {
133 size 0.15 0.15 0.006
134 }
135 physics Physics {
136 density -1
137 mass 2
138 }
139
140 # define the link2 graphics
141 children [ # second link Solid
142 Pose {
143 # locate the link2 visible model within link2 coordinates
144 translation 0 0 -0.003
145 children [ # second link Pose
146 Shape {
147 appearance PBRAppearance {
148 baseColor 0.192157 0.772549 0.589319
149 metalness 0.5
150 roughness 0.5
151 }
152 geometry Mesh {
153 url ["../stl/arrow.stl"]
154 }
155 }
156 ] # close children of second link Pose
157 }
158 ] # close children of second link Solid
159 }
160 }
161 ] # close children of first link Solid
162 } # end the link1 Solid
163 } # end first HingeJoint
164 %< } >% # end inner loop to create each element
165 %< } >% # end outer loop to create each element
166 ] # close children of Robot
167 } # close the Robot definition
168}
World File¶
1#VRML_SIM R2023b utf8
2
3EXTERNPROTO "../protos/HL-A11-room.proto"
4EXTERNPROTO "../protos/array.proto"
5
6WorldInfo {
7}
8Viewpoint {
9 orientation 0.06085219663820096 0.008555279899415609 -0.998110122857267 3.076199129834454
10 position 4.97232 0.572286 1.7
11 followType "None"
12}
13Background {
14 skyColor [
15 0.1 0.1 0.1
16 ]
17}
18SpotLight {
19 attenuation 0 0 1
20 beamWidth 0.3
21 cutOffAngle 0.4
22 direction -2 -2 1
23 intensity 5
24 location 1.5 2 0.1
25 castShadows TRUE
26}
27SpotLight {
28 attenuation 0 0 1
29 beamWidth 0.3
30 cutOffAngle 0.4
31 direction -2 2 1
32 intensity 5
33 location 1.5 -2 0.1
34 castShadows TRUE
35}
36HL-A11-room {
37}
38array {
39 translation 0 -1.05 1.8
40 rotation 0 1 0 1.5708
41 num_x 6
42 num_y 8
43}