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 Lua scripting.
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-2023 Garth Zeglin. This file is explicitly placed in the public domain.
8
9PROTO array [
10 field SFVec3f translation 0 0 0
11 field SFRotation rotation 0 1 0 0
12 field SFString controller "array"
13 field SFString name "array"
14 field SFInt32 num_x 4
15 field SFInt32 num_y 5
16 field SFFloat spacing_x 0.3
17 field SFFloat spacing_y 0.3
18 field SFString customData ""
19]
20{
21 Robot {
22 # connect properties to user-visible data fields
23 translation IS translation
24 rotation IS rotation
25 controller IS controller
26 name IS name
27 customData IS customData
28
29 # enable the bodies within the system to directly interact
30 selfCollision TRUE
31
32 # enable additional API access for the controller to read back parameters
33 supervisor TRUE
34
35 children [
36 # 2D nested loops to create each motorized unit
37 %{ for x = 1, fields.num_x.value do }%
38 %{ for y = 1, fields.num_y.value do }%
39
40 # define individualized names
41 %{ local motor_name = "\"motor" .. x .. y .. "\"" }%
42 %{ local sensor_name = "\"joint" .. x .. y .. "\"" }%
43 %{ local link1_name = "\"link1" .. x .. y .. "\"" }%
44
45 # define the base of each unit representing the individual motor and mounting
46 Pose {
47 # locate the base to match each hinge joint location
48 translation %{= fields.spacing_x.value * (x-1) }% %{= fields.spacing_y.value * (y-1) }% 0.02
49 children [
50 Shape {
51 appearance PBRAppearance {
52 baseColor 0.180392 0 1
53 metalness 0.5
54 }
55 geometry Cylinder {
56 height 0.04
57 radius 0.1
58 }
59 }
60 ] # close children of base Pose
61 } # close the base Pose
62
63 # define the first pivot of each unit, representing a driven motor axis
64 HingeJoint {
65 jointParameters HingeJointParameters {
66 axis 0 0 1
67 # locate the body origin within the robot coordinates
68 anchor %{= fields.spacing_x.value * (x-1) }% %{= fields.spacing_y.value * (y-1) }% 0.02
69 }
70 device [
71 RotationalMotor {
72 name %{= motor_name }%
73 }
74 ]
75 # define the body of the first driven link of each unit
76 endPoint Solid {
77 # locate the link1 body origin within the overall robot coordinates
78 translation %{= fields.spacing_x.value * (x-1) }% %{= fields.spacing_y.value * (y-1) }% 0.05
79
80 # provide a unique name for the link1 Solid to avoid warnings
81 name %{= link1_name }%
82
83 # define the collision and physics properties for link1
84 boundingObject Box {
85 size 0.15 0.15 0.006
86 }
87 physics Physics {
88 density -1
89 mass 2
90 }
91
92 # define the link1 graphics
93 children [ # first link Solid
94 Pose {
95 # locate the link1 visible model within link2 coordinates
96 translation 0 0 -0.003
97 children [ # first link Pose
98 Shape {
99 appearance PBRAppearance {
100 baseColor 0.772549 0.2 0.192157
101 metalness 0.5
102 roughness 0.5
103 }
104 # import the visible geometry; the physics properties and
105 # collision use the boundingObject field which defines a
106 # simple box
107 geometry Mesh {
108 url ["../stl/arrow.stl"]
109 }
110 }
111 ] # close children of first link Pose
112 }
113
114 # define the second pivot of each unit, representing a passive rotation axis
115 HingeJoint {
116 jointParameters HingeJointParameters {
117 axis 0 0 1
118 # locate the joint axis within the link1 coordinates
119 anchor 0.050 0 0
120
121 # provide a slight friction to dissipate energy
122 dampingConstant 0.01
123 }
124
125 # define the body of the second (undriven) link of each unit
126 endPoint Solid {
127 # locate the origin of link2 within the link1 coordinates
128 translation 0.075 0 0.010
129
130 # define the collision and physics properties for link2
131 boundingObject Box {
132 size 0.15 0.15 0.006
133 }
134 physics Physics {
135 density -1
136 mass 2
137 }
138
139 # define the link2 graphics
140 children [ # second link Solid
141 Pose {
142 # locate the link2 visible model within link2 coordinates
143 translation 0 0 -0.003
144 children [ # second link Pose
145 Shape {
146 appearance PBRAppearance {
147 baseColor 0.192157 0.772549 0.589319
148 metalness 0.5
149 roughness 0.5
150 }
151 geometry Mesh {
152 url ["../stl/arrow.stl"]
153 }
154 }
155 ] # close children of second link Pose
156 }
157 ] # close children of second link Solid
158 }
159 }
160 ] # close children of first link Solid
161 } # end the link1 Solid
162 } # end first HingeJoint
163 %{ end }% # end inner loop to create each element
164 %{ end }% # end outer loop to create each element
165 ] # close children of Robot
166 } # close the Robot definition
167}
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}