ZYY Robot Arm Model¶
The zyy robot model simulates a three-DOF robot arm structured as a two-link arm on a rotating base. The base joint1 rotates around the vertical Z axis, followed by a shoulder joint2 around the horizontal Y axis and elbow joint3, parallel to joint2 around the Y axis. The reference pose is straight up. The arm has a distance sensor at the end. The model is intended as a demonstration testbed for zyy kinematics.
The link geometry uses only cylinder primitives. The same geometry is referenced to use as bounding objects for collision and automatic calculation of physics parameters. The base object has a NULL Physics object so it does not move, simulating a rigid connection to the ground.
The end sensor is represented by a yellow cone. It is implemented as a DistanceSensor of limited range, pointing outward along the link axis from the end of the second link.
This model is demonstrated in the zyy.wbt world.
Screenshot of Webots zyy robot arm robot model.¶
Screenshot of Webots sample world zyy.wbt with two zyy robot arms in the HL A11 room.¶
System Kinematics¶
Kinematic diagram of the zyy robot arm model.¶
The bodies are as follows:
name |
color |
notes |
|---|---|---|
fixed |
blue |
base object fixed to the ground |
base |
blue |
upper base element rotating around the Z axis |
link1 |
red |
proximal link, attaches to base at ‘shoulder’ |
link2 |
green |
the distal link, attaches to link1 at ‘elbow’ |
The joints are as follows:
name |
parent |
child |
notes |
|---|---|---|---|
joint1 |
fixed |
base |
the base pivot, includes motor1 |
joint2 |
base |
link1 |
the ‘shoulder’, includes motor2 |
joint3 |
link1 |
link2 |
the ‘elbow’, includes motor3 |
The joint limits are as follows:
name |
min deg |
max deg |
description |
|---|---|---|---|
joint1 |
-120 |
120 |
symmetric pivot around vertical axis |
joint2 |
-5 |
90 |
from slight reverse down to link1 horizontal |
joint3 |
-5 |
120 |
from slight reverse down to an acute retraction |
The axes are as follows:
name |
direction |
notes |
|---|---|---|
joint1 |
along Z |
located above the origin |
joint2 |
along Y |
located above the origin |
joint3 |
along Y |
located at the end of link1 |
The motors and sensors are named as follows:
name |
notes |
|---|---|
motor1 |
RotationalMotor on joint1 |
motor2 |
RotationalMotor on joint2 |
motor3 |
RotationalMotor on joint3 |
joint1 |
PositionSensor on joint1 |
joint2 |
PositionSensor on joint2 |
joint3 |
PositionSensor on joint3 |
endRangeSensor |
DistanceSensor at end of link2 |
Reference pose of the zyy robot arm model: all joints are zero when the arm is pointing straight up.¶
zyy.proto¶
The robot model has been encapsulated in a .proto file for easy reuse. The model includes user-accessible link length parameters to demonstrate procedural scaling.
1#VRML_SIM R2023b utf8
2#
3# Two-link actuated arm featuring three degreees of freedom. The first axis is
4# a base rotation around Z, followed by two arm joints rotating around Y. The
5# graphics use only cylinder and cone primitives. The base has NULL physics so
6# it will be fixed in place. The two link lengths are adjustable parameters to
7# demonstrate using procedural elements in the prototype. The link physics
8# properties are specified using density so the dynamics will also scale, but
9# the motor parameters are constant. The end includes a distance sensor pointed
10# along the second link axis.
11#
12# documentation url: https://courses.ideate.cmu.edu/16-375
13# license: No copyright, 2020-2026 Garth Zeglin. This file is explicitly placed in the public domain.
14# template language: javascript
15
16PROTO zyy [
17 field SFVec3f translation 0 0 0
18 field SFRotation rotation 0 1 0 0
19 field SFFloat link1Length 0.5
20 field SFFloat link2Length 0.5
21 field SFString controller "zyy"
22 field SFString name ""
23 field SFString customData ""
24]
25{
26 Robot {
27 # connect properties to user-visible data fields
28 translation IS translation
29 rotation IS rotation
30 controller IS controller
31 name IS name
32 customData IS customData
33
34 # calculate derived parameters
35 %<
36 let halfLink1Len = fields.link1Length.value / 2;
37 let halfLink2Len = fields.link2Length.value / 2;
38 >%
39
40 # define the kinematic tree
41 children [
42 # add a default radio receiver and transmitter
43 Receiver {
44 channel 1
45 }
46 Emitter {
47 channel 1
48 }
49
50 # the cylindrical base shape is wrapped in a Pose
51 # to position it within the robot body coordinates
52 DEF fixedBaseShape Pose {
53 translation 0 0 0.05
54 children [
55 Shape {
56 appearance DEF blueAppearance PBRAppearance {
57 baseColor 0.21529 0.543008 0.99855
58 metalness 0.5
59 roughness 0.5
60 }
61 geometry Cylinder {
62 height 0.1
63 radius 0.2
64 }
65 }
66 ]
67 }
68
69 # define the base Z-axis pivot connecting the fixed base and the rotating base
70 HingeJoint {
71 jointParameters HingeJointParameters {
72 axis 0 0 1
73 # limit travel to (-120, 120) degrees
74 minStop -2.0943951023931953
75 maxStop 2.0943951023931953
76 }
77 device [
78 PositionSensor {
79 name "joint1"
80 }
81 RotationalMotor {
82 name "motor1"
83 acceleration 2
84 maxVelocity 3.14
85 maxTorque 20
86 }
87 ]
88 # define rotating base element
89 endPoint Solid {
90 translation 0 0 0.120
91 name "base"
92 children [
93 DEF rotatingBaseShape Pose {
94 translation 0 0 0.050
95 children [
96 # rotating base geometry
97 Shape {
98 appearance USE blueAppearance
99 geometry Cylinder {
100 height 0.100
101 radius 0.200
102 subdivision 6
103 }
104 }
105 ]
106 }
107 # define the j2 Y axis pivot connecting the rotating base and the first arm link
108 HingeJoint {
109 jointParameters HingeJointParameters {
110 axis 0 1 0
111 anchor 0 0 0.170
112 # limit travel to (-5, 90) degrees
113 minStop -0.08726646259971647
114 maxStop 1.5707963267948966
115 }
116 device [
117 PositionSensor {
118 name "joint2"
119 }
120 RotationalMotor {
121 name "motor2"
122 acceleration 2
123 maxVelocity 3.14
124 maxTorque 20
125 }
126 ]
127 # start definition of the first link
128 endPoint Solid {
129 # place the shape origin halfway along the first link;
130 # this vector is in base coordinates, Z points along
131 # the link in the neutral pose
132 translation 0 0 %<=halfLink1Len+0.17>%
133
134 children [
135 # define the j3 Y-axis 'elbow' pivot connecting the links
136 HingeJoint {
137 jointParameters HingeJointParameters {
138 axis 0 1 0
139 # place the elbow joint axis at the end of the first
140 # link; position is relative to link1 origin
141 anchor 0 0 %<= halfLink1Len >%
142 dampingConstant 0.1
143 # limit travel to (-5, 120) degrees
144 minStop -0.08726646259971647
145 maxStop 2.0943951023931953
146 }
147 device [
148 PositionSensor {
149 name "joint3"
150 }
151 RotationalMotor {
152 name "motor3"
153 acceleration 2
154 maxVelocity 6.28
155 maxTorque 15
156 }
157 ]
158 # define the second link
159 endPoint Solid {
160 # place the link2 origin halfway along the second link
161 translation 0 0 %<= halfLink1Len+halfLink2Len>%
162 children [
163 # the cylindrical link shape is wrapped in a Pose
164 # to position it within the link2 coordinates
165 DEF link2Shape Pose {
166 # the Cylinder shape coordinates use Z as the
167 # long axis; this 90 deg rotation around Y
168 # places the lengthwise Z axis along the link.
169 children [
170 Shape {
171 appearance DEF greenAppearance PBRAppearance {
172 baseColor 0.413001 1 0.33489
173 metalness 0.5
174 roughness 0.5
175 }
176 geometry Cylinder {
177 height IS link2Length
178 radius 0.05
179 }
180 }
181 ]
182 } # end link2 Shape
183 # add a visual hub to the base of link2, not part of the bounding object
184 Pose {
185 rotation 1 0 0 1.5708
186 translation 0 0 %<= -halfLink2Len>%
187 children [
188 Shape {
189 appearance USE greenAppearance
190 geometry Cylinder {
191 height 0.1
192 radius 0.05
193 }
194 }
195 ]
196 } # end Pose around link2 base hub
197 # define a DistanceSensor attached to the second link Solid node
198 DistanceSensor {
199 translation 0 0 %<= halfLink2Len>%
200 name "endRangeSensor"
201
202 # the sensor lookup table implicitly defines the maximum range and the units, each
203 # entry is [distance, value, noise]
204 lookupTable [
205 0 0 0
206 0.9 0.9 0 # 0.9 meters reads as 0.9 meters
207 ]
208 resolution 0.001 # assume millimeter resolution
209 numberOfRays 5
210 aperture 0.1
211 children [
212 Pose {
213 # flip the cone representing the sensor so the base is up
214 rotation 0 1 0 3.14159
215 children [
216 Shape {
217 appearance PBRAppearance {
218 baseColor 1 0.99028 0.0584421
219 roughness 0.5
220 metalness 0.5
221 emissiveColor 1 0.99028 0.0584421
222 emissiveIntensity 0.2
223 }
224 geometry Cone {
225 bottomRadius 0.02
226 height 0.1
227 }
228 }
229 ]
230 }
231 ]
232 } # end DistanceSensor
233 ] # end link2 Solid children
234 # top-level properties of link2
235 name "link2"
236 boundingObject USE link2Shape
237 physics Physics {
238 # Assume the link is a thin-walled aluminum tube with 50 mm
239 # radius and 2 mm wall thickness. Aluminum has a density of
240 # 2700 kg/m^3, but this will be scaled by the ratio of the
241 # tube cross-section to the solid cylinder cross-section
242 # assumed by the simulator. Note that the moment of inertia
243 # around the long axis will be underestimated.
244 # density = 2700 * (R_outer**2 - R_inner**2) / R_outer**2
245 density 211.7
246 mass -1
247 }
248 }
249 }
250 # finish the definition of link1 with a shape
251 # node in the 'children' list
252 DEF link1Shape Pose {
253 children [
254 Shape {
255 appearance DEF redAppearance PBRAppearance {
256 baseColor 00.990494 0.516915 0.468254
257 metalness 0.5
258 roughness 0.5
259 }
260 geometry Cylinder {
261 height IS link1Length
262 radius 0.05
263 }
264 }
265 ]
266 }
267 # add a visual hub to the base of link1, not part of the bounding object
268 Pose {
269 rotation 1 0 0 1.5708
270 translation 0 0 %<= -halfLink1Len>%
271 children [
272 Shape {
273 appearance USE redAppearance
274 geometry Cylinder {
275 height 0.1
276 radius 0.05
277 }
278 }
279 ]
280 } # end Pose around link1 base hub
281 ] # close the children list of the link1 node
282 # top-level properties of link1
283 name "link1"
284 boundingObject USE link1Shape
285 physics Physics {
286 # See notes for link2 density; this assumes the same geometry.
287 density 211.7
288 mass -1
289 } # close the Physics for the first arm link
290 } # close the Solid in the HingeJoint endpoint
291 } # close the HingeJoint between the base and the first arm link
292 ] # close the children of the rotating base
293 # the rotating robot base participates in collisions
294 boundingObject USE rotatingBaseShape
295 # the rotating robot base has some mass and physics
296 physics Physics {
297 mass 2.0
298 density -1
299 }
300 } # close the Solid in the base axis HingeJoint endpoint
301 } # close the HingeJoint between the fixed and rotating base
302 ] # close the children list of the base Robot node
303
304 # the fixed robot base participates in collision
305 boundingObject USE fixedBaseShape
306
307 # the base of the robot itself has NULL physics to simulate being fixed to the ground
308
309 } # close the Robot definition
310}
Sample Control Code¶
1# zyy.py
2#
3# Sample Webots controller file for driving a two-link arm with three 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("zyy.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# Import the bezier module from the same directory. Note: this requires numpy.
19import bezier
20
21# Import the third-party numpy library for matrix calculations.
22# Note: this can be installed using 'pip3 install numpy' or 'pip3 install scipy'.
23import numpy as np
24
25# Define the time step in milliseconds between controller updates.
26EVENT_LOOP_DT = 20
27
28################################################################
29class ZYY(Robot):
30 def __init__(self):
31
32 super(ZYY, self).__init__()
33 self.robot_name = self.getName()
34 print("%s: controller connected." % (self.robot_name))
35
36 # Attempt to randomize the random library sequence.
37 random.seed(time.time())
38
39 # Initialize geometric constants. These should match
40 # the current geometry of the robot.
41 self.base_z = 0.29 # joint2 axis height: 0.12 fixed base height + 0.17 j2 z offset
42 self.link1 = 0.5 # link1 length, i.e. distance between j2 and j3
43 self.link2 = 0.5 # link2 length, i.e. distance between j3 and end
44
45 # Fetch handles for the joint motors.
46 self.motor1 = self.getDevice('motor1')
47 self.motor2 = self.getDevice('motor2')
48 self.motor3 = self.getDevice('motor3')
49
50 # Adjust the motor controller properties.
51 self.motor1.setAvailableTorque(20.0)
52 self.motor2.setAvailableTorque(15.0)
53 self.motor3.setAvailableTorque(10.0)
54
55 # Adjust the low-level controller gains.
56 print("%s: setting PID gains." % (self.robot_name))
57 self.motor1.setControlPID(100.0, 0.0, 25.0)
58 self.motor2.setControlPID( 50.0, 0.0, 15.0)
59 self.motor3.setControlPID( 50.0, 0.0, 15.0)
60
61 # Fetch handles for the joint sensors.
62 self.joint1 = self.getDevice('joint1')
63 self.joint2 = self.getDevice('joint2')
64 self.joint3 = self.getDevice('joint3')
65
66 # Specify the sampling rate for the joint sensors.
67 self.joint1.enable(EVENT_LOOP_DT)
68 self.joint2.enable(EVENT_LOOP_DT)
69 self.joint3.enable(EVENT_LOOP_DT)
70
71 # Connect to the end sensor.
72 self.end_sensor = self.getDevice("endRangeSensor")
73 self.end_sensor.enable(EVENT_LOOP_DT) # set sampling period in milliseconds
74 self.end_sensor_interval = 1000
75 self.end_sensor_timer = 1000
76
77 # Initialize behavior state machines.
78 self.state_timer = 2*EVENT_LOOP_DT
79 self.state_index = 0
80 self._init_spline()
81 return
82
83 #================================================================
84 def endpoint_forward_kinematics(self, q):
85 """Compute the forward kinematics for the end point. Returns the
86 body-coordinate XYZ Cartesian position of the end point for a given joint
87 angle vector.
88 :param q: three-element list with [q1, q2, q3] joint angles in radians
89 :return: three-element list with endpoint [x,y,z] location
90
91 """
92 j1 = q[0]
93 j2 = q[1]
94 j3 = q[2]
95
96 return [(self.link1*math.sin(j2) + self.link2*math.sin(j2 + j3))*math.cos(j1),
97 (self.link1*math.sin(j2) + self.link2*math.sin(j2 + j3))*math.sin(j1),
98 self.base_z + self.link1*math.cos(j2) + self.link2*math.cos(j2 + j3)]
99
100 #================================================================
101 def endpoint_inverse_kinematics(self, target):
102 """Compute the joint angles for a target end position. The target is a
103 XYZ Cartesian position vector in body coordinates, and the result vector
104 is a joint angles as list [j1, j2, j3]. If the target is out of reach,
105 returns the closest pose. With j1 between -pi and pi, and j2 and j3
106 limited to positive rotations, the solution is always unique.
107 """
108
109 x = target[0]
110 y = target[1]
111 z = target[2]
112
113 # the joint1 base Z rotation depends only upon x and y
114 j1 = math.atan2(y, x)
115
116 # distance within the XY plane from the origin to the endpoint projection
117 xy_radial = math.sqrt(x*x + y*y)
118
119 # find the Z offset from the J2 horizontal plane to the end point
120 end_z = z - self.base_z
121
122 # angle between the J2 horizonta plane and the endpoint
123 theta = math.atan2(end_z, xy_radial)
124
125 # radial distance from the J2 axis to the endpoint
126 radius = math.sqrt(x*x + y*y + end_z*end_z)
127
128 # use the law of cosines to compute the elbow angle solely as a function of the
129 # link lengths and the radial distance from j2 to end
130 # R**2 = l1**2 + l2**2 - 2*l1*l2*cos(pi - elbow)
131 acosarg = (radius*radius - self.link1**2 - self.link2**2) / (-2 * self.link1 * self.link2)
132 if acosarg < -1.0: elbow_supplement = math.pi
133 elif acosarg > 1.0: elbow_supplement = 0.0
134 else: elbow_supplement = math.acos(acosarg)
135
136 print("theta:", theta, "radius:", radius, "acosarg:", acosarg)
137
138 # use the law of sines to find the angle at the bottom vertex of the triangle defined by the links
139 # radius / sin(elbow_supplement) = l2 / sin(alpha)
140 if radius > 0.0:
141 alpha = math.asin(self.link2 * math.sin(elbow_supplement) / radius)
142 else:
143 alpha = 0.0
144
145 # calculate the joint angles
146 return [j1, math.pi/2 - theta - alpha, math.pi - elbow_supplement]
147
148
149 #================================================================
150 # motion primitives
151
152 def go_joints(self, target):
153 """Issue a position command to move to the given endpoint position expressed in joint angles."""
154
155 self.motor1.setPosition(target[0])
156 self.motor2.setPosition(target[1])
157 self.motor3.setPosition(target[2])
158 # print("%s: moving to (%f, %f, %f)" % (self.robot_name, target[0], target[1], target[2]));
159
160
161 #================================================================
162 # Polling function to process sensor input at different timescales.
163 def poll_sensors(self):
164 self.end_sensor_timer -= EVENT_LOOP_DT
165 if self.end_sensor_timer < 0:
166 self.end_sensor_timer += self.end_sensor_interval
167
168 # read the distance sensor
169 distance = self.end_sensor.getValue()
170
171 if distance < 0.9:
172 # print("%s: range sensor detected obstacle at %f." % (self.robot_name, distance))
173 pass
174
175 #================================================================
176
177 # Define a joint-space movement sequence as a Bezier cubic spline trajectory
178 # specified in degrees. This should have three rows per spline segment.
179
180 _path = np.array([ # the first waypoint is implicitly zero
181 [ 0, 0, 0],
182 [ 5, 30, 30],
183 [ 5, 30, 30], # waypoint
184 [ 5, 30, 30],
185 [ 5, 30, 30],
186 [ 5, 30, 90], # waypoint
187 [ 5, 30, 30],
188 [ 5, 30, 30],
189 [ 5, 30, 30], # waypoint
190 [ 5, 30, 30],
191 [ 0, 0, 0],
192 [ 0, 0, 0], # waypoint
193 [ 0, 0, 0],
194 [ 45, 30, 30],
195 [ 45, 30, 30], # waypoint
196 [ 45, 30, 30],
197 [ 45, 30, 30],
198 [ 45, 30, 90], # waypoint
199 [ 45, 30, 30],
200 [ 45, 30, 30],
201 [ 45, 30, 30], # waypoint
202 [ 45, 30, 30],
203 [ 0, 0, 0],
204 [ 0, 0, 0], # waypoint (should be zero for continuity)
205 ])
206
207 def _init_spline(self):
208 self.interpolator = bezier.PathSpline(axes=3)
209 self.interpolator.set_tempo(30.0)
210 self.interpolator.add_spline(self._path)
211 self._last_segment_count = self.interpolator.segments()
212
213 def poll_spline_path(self):
214 """Update function to loop a cubic spline trajectory."""
215 degrees = self.interpolator.update(dt=EVENT_LOOP_DT*0.001)
216 radians = [math.radians(theta) for theta in degrees]
217
218 # mirror the base rotation of the right robot for symmetric motion
219 if 'right' in self.robot_name:
220 radians[0] *= -1
221
222 self.go_joints(radians)
223 # print("u: ", self.interpolator.u, "knots:", self.interpolator.knots.shape[0])
224
225 segment_count = self.interpolator.segments()
226 if segment_count != self._last_segment_count:
227 self._last_segment_count = segment_count
228 print("Starting next spline segment.")
229
230 if self.interpolator.is_done():
231 print("Restarting the spline path.")
232 self.interpolator.add_spline(self._path)
233
234 #================================================================
235 # Define joint-space movement sequences. For convenience the joint angles
236 # are specified in degrees, then converted to radians for the controllers.
237 _right_poses = [[0, 0, 0],
238 [-45, 0, 60],
239 [45, 60, 60],
240 [0, 60, 0],
241 ]
242
243 _left_poses = [[0, 0, 0],
244 [45, 0, 60],
245 [-45, 60, 60],
246 [0, 60, 0],
247 ]
248
249 #================================================================
250 def poll_sequence_activity(self):
251 """State machine update function to walk through a series of poses at regular intervals."""
252
253 # Update the state timer
254 self.state_timer -= EVENT_LOOP_DT
255
256 # If the timer has elapsed, reset the timer and update the outputs.
257 if self.state_timer < 0:
258 self.state_timer += 2000
259
260 # Look up the next pose.
261 if 'left' in self.robot_name:
262 next_pose = self._left_poses[self.state_index]
263 self.state_index = (self.state_index + 1) % len(self._left_poses)
264 else:
265 next_pose = self._right_poses[self.state_index]
266 self.state_index = (self.state_index + 1) % len(self._right_poses)
267
268 # Convert the pose to radians and issue to the motor controllers.
269 angles = [math.radians(next_pose[0]), math.radians(next_pose[1]), math.radians(next_pose[2])]
270 self.go_joints(angles)
271
272
273 #================================================================
274 def run(self):
275 # Run loop to execute a periodic script until the simulation quits.
276 # If the controller returns -1, the simulator is quitting.
277 while self.step(EVENT_LOOP_DT) != -1:
278 # Read simulator clock time.
279 self.sim_time = self.getTime()
280
281 # Read sensor values.
282 self.poll_sensors()
283
284 # Update the activity state machine.
285 # self.poll_sequence_activity()
286
287 # Update the spline trajectory generator.
288 self.poll_spline_path()
289
290
291################################################################
292# Start the script.
293robot = ZYY()
294robot.run()