Arm Theater¶
This sample world includes a pair of double pendula configured as actuated two-link arms. This model demonstrates several key concepts: rigid body dynamics, collision modeling, and PD control.
The world file is arm-theater.wbt, which can be found within the Webots.zip archive.
Screenshot of Webots model of the arm-theater robots. Each robot is independently driven by a Python script.¶
World File¶
The world file specifies all the elements of the scene: camera position, background scenery, lighting, fixed objects, and robots. The robots are modeled as separate .proto scripts so the world file is short and fairly readable, since only parameters changed from the defaults are specified.
1#VRML_SIM R2023b utf8
2
3EXTERNPROTO "../protos/HL-A11-room.proto"
4EXTERNPROTO "../protos/clock.proto"
5EXTERNPROTO "../protos/two-link.proto"
6
7WorldInfo {
8}
9Viewpoint {
10 orientation 0 0 -1 3.14159
11 position 7.5 0 1.3
12 followType "None"
13}
14Background {
15 skyColor [
16 0.1 0.1 0.1
17 ]
18}
19SpotLight {
20 attenuation 0 0 1
21 beamWidth 0.3
22 cutOffAngle 0.4
23 direction -2 -2 1.2
24 intensity 5
25 location 1.5 2 0.1
26}
27SpotLight {
28 attenuation 0 0 1
29 beamWidth 0.3
30 cutOffAngle 0.4
31 direction -2 2 1.2
32 intensity 5
33 location 1.5 -2 0.1
34}
35HL-A11-room {
36}
37clock {
38 translation -1.5 0 1
39 rotation 0.5773509358554485 0.5773509358554485 0.5773489358556708 2.0944
40}
41two-link {
42 translation 0 1.0 0.5
43 rotation 0.707107 0 0.707107 -3.14159
44 name "right-two-link"
45 controller "arm_theater"
46}
47two-link {
48 translation 0 -1.0 0.5
49 rotation 0.707107 0 0.707107 -3.14159
50 name "left-two-link"
51 controller "arm_theater"
52}
Proto Files¶
The proto files are a mixture of VRML and embedded Lua. They can be browsed directly from the course site:
The two-link arm is documented is detail on Two-Link Robot Model. The clock is documented in detail on Clock Robot Model.
Sample Two-Link Robot Control Code¶
The controller triggers a sequence of poses stored in _left_poses and
_right_poses. The two-link structure is regulated in position mode using a
simulated PID controller provided within Webots.
1# arm_theater.py
2#
3# Sample Webots controller file for driving a two-link arm with two driven
4# joints. This example provides inverse kinematics for performing
5# position-controlled trajectories.
6
7# No copyright, 2020-2024, Garth Zeglin. This file is
8# explicitly placed in the public domain.
9
10print("arm_theater.py waking up.")
11
12# Import the Webots simulator API.
13from controller import Robot
14
15# Import the standard Python math library.
16import math, random, time
17
18# Define the time step in milliseconds between controller updates.
19EVENT_LOOP_DT = 100
20
21################################################################
22class TwoLink(Robot):
23 def __init__(self):
24
25 super(TwoLink, self).__init__()
26 self.robot_name = self.getName()
27 print("%s: controller connected." % (self.robot_name))
28
29 # Attempt to randomize the random library sequence.
30 random.seed(time.time())
31
32 # Initialize geometric constants. These should match
33 # the current geometry of the robot.
34 self.link1_length = 0.5
35 self.link2_length = 0.5
36
37 # Fetch handle for the 'base' and 'elbow' joint motors.
38 self.motor1 = self.getDevice('motor1')
39 self.motor2 = self.getDevice('motor2')
40
41 # Adjust the motor controller properties.
42 self.motor1.setAvailableTorque(15.0)
43 self.motor2.setAvailableTorque(10.0)
44
45 # Adjust the low-level controller gains.
46 print("%s: setting PID gains." % (self.robot_name))
47 self.motor1.setControlPID(50.0, 0.0, 15.0)
48 self.motor2.setControlPID(50.0, 0.0, 15.0)
49
50 # Fetch handles for the joint sensors.
51 self.joint1 = self.getDevice('joint1')
52 self.joint2 = self.getDevice('joint2')
53
54 # Specify the sampling rate for the joint sensors.
55 self.joint1.enable(EVENT_LOOP_DT)
56 self.joint2.enable(EVENT_LOOP_DT)
57
58 # Connect to the end sensor.
59 self.end_sensor = self.getDevice("endRangeSensor")
60 self.end_sensor.enable(EVENT_LOOP_DT) # set sampling period in milliseconds
61 self.end_sensor_interval = 1000
62 self.end_sensor_timer = 1000
63
64 # Initialize behavior state machines.
65 self.state_timer = 2*EVENT_LOOP_DT
66 self.state_index = 0
67 return
68
69 #================================================================
70 def forward_kinematics(self, q):
71 """Compute the forward kinematics. Returns the body-coordinate XY Cartesian
72 position of the elbow and endpoint for a given joint angle vector.
73 Note that in the arm theater model the body X coordinate is straight up, body Y
74 is to the left.
75
76 :param q: two-element list with [q1, q2] joint angles
77 :return: tuple (elbow, end) of two-element lists with [x,y] locations
78 """
79
80 elbow = [self.link1_length * math.cos(q[0]), self.link1_length * math.sin(q[0])]
81 end = [elbow[0] + self.link2_length * math.cos(q[0]+q[1]), elbow[1] + self.link2_length * math.sin(q[0]+q[1])]
82 return elbow, end
83
84 #================================================================
85 def endpoint_inverse_kinematics(self, target):
86 """Compute two inverse kinematics solutions for a target end position. The
87 target is a XY Cartesian position vector in body coordinates, and the
88 result vectors are joint angles as lists [q0, q1]. If the target is out
89 of reach, returns the closest pose.
90 """
91
92 # find the position of the point in polar coordinates
93 radiussq = target[0]**2 + target[1]**2
94 radius = math.sqrt(radiussq)
95
96 # theta is the angle of target point w.r.t. X axis, same origin as arm
97 theta = math.atan2(target[1], target[0])
98
99 # use the law of cosines to compute the elbow angle
100 # R**2 = l1**2 + l2**2 - 2*l1*l2*cos(pi - elbow)
101 # both elbow and -elbow are valid solutions
102 acosarg = (radiussq - self.link1_length**2 - self.link2_length**2) / (-2 * self.link1_length * self.link2_length)
103 if acosarg < -1.0: elbow_supplement = math.pi
104 elif acosarg > 1.0: elbow_supplement = 0.0
105 else: elbow_supplement = math.acos(acosarg)
106
107 # use the law of sines to find the angle at the bottom vertex of the triangle defined by the links
108 # radius / sin(elbow_supplement) = l2 / sin(alpha)
109 if radius > 0.0:
110 alpha = math.asin(self.link2_length * math.sin(elbow_supplement) / radius)
111 else:
112 alpha = 0.0
113
114 # compute the two solutions with opposite elbow sign
115 soln1 = [theta - alpha, math.pi - elbow_supplement]
116 soln2 = [theta + alpha, elbow_supplement - math.pi]
117
118 return soln1, soln2
119 #================================================================
120 # motion primitives
121
122 def go_joints(self, target):
123 """Issue a position command to move to the given endpoint position expressed in joint angles."""
124
125 # arbitrarily pick the first solution
126 self.motor1.setPosition(target[0])
127 self.motor2.setPosition(target[1])
128 print("%s: moving to (%f, %f)" % (self.robot_name, target[0], target[1]));
129
130 def go_target(self, target):
131 """Issue a position command to move to the given endpoint position expressed in Cartesian coordinates."""
132 p1, p2 = self.endpoint_inverse_kinematics(target)
133
134 # arbitrarily pick the first solution
135 self.motor1.setPosition(p1[0])
136 self.motor2.setPosition(p1[1])
137 print("%s: target (%f, %f), moving to (%f, %f)" % (self.robot_name, target[0], target[1], p1[0], p1[1]))
138
139 #================================================================
140 # Polling function to process sensor input at different timescales.
141 def poll_sensors(self):
142 self.end_sensor_timer -= EVENT_LOOP_DT
143 if self.end_sensor_timer < 0:
144 self.end_sensor_timer += self.end_sensor_interval
145
146 # read the distance sensor
147 distance = self.end_sensor.getValue()
148
149 if distance < 0.9:
150 print("%s: range sensor detected obstacle at %f." % (self.robot_name, distance))
151
152 #================================================================
153 # Define joint-space movement sequences. For convenience the joint angles
154 # are specified in degrees, then converted to radians for the controllers.
155 _right_poses = [[0, 0],
156 [0, 60],
157 [60, 60],
158 [60, 0],
159 ]
160
161 _left_poses = [[0, 0],
162 [0, -60],
163 [-60, -60],
164 [-60, 0],
165 ]
166
167 #================================================================
168 def poll_sequence_activity(self):
169 """State machine update function to walk through a series of poses at regular intervals."""
170
171 # Update the state timer
172 self.state_timer -= EVENT_LOOP_DT
173
174 # If the timer has elapsed, reset the timer and update the outputs.
175 if self.state_timer < 0:
176 self.state_timer += 2000
177
178 # Look up the next pose.
179 if 'left' in self.robot_name:
180 next_pose = self._left_poses[self.state_index]
181 self.state_index = (self.state_index + 1) % len(self._left_poses)
182 else:
183 next_pose = self._right_poses[self.state_index]
184 self.state_index = (self.state_index + 1) % len(self._right_poses)
185
186 # Convert the pose to radians and issue to the motor controllers.
187 angles = [math.radians(next_pose[0]), math.radians(next_pose[1])]
188 self.go_joints(angles)
189
190 #================================================================
191 def run(self):
192 # Run loop to execute a periodic script until the simulation quits.
193 # If the controller returns -1, the simulator is quitting.
194 while self.step(EVENT_LOOP_DT) != -1:
195 # Read simulator clock time.
196 self.sim_time = self.getTime()
197
198 # Read sensor values.
199 self.poll_sensors()
200
201 # Update the activity state machine.
202 self.poll_sequence_activity()
203
204
205################################################################
206# Start the script.
207robot = TwoLink()
208robot.run()