Two-Link Robot Model¶
The two-link robot model simulates a planar two-link robot with an actuator at each joint and a sensor at the end. The model is intended as a demonstration testbed for two-link 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 sensor-demo.wbt and controls-demo.wbt worlds.
Screenshot of Webots model of fully-actuated two-link planar robot.¶
System Kinematics¶
The bodies are as follows:
name |
color |
notes |
|---|---|---|
base |
blue |
base object fixed to the ground |
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 |
base |
link1 |
the ‘shoulder’, includes motor1 |
joint2 |
link1 |
link2 |
the ‘elbow’, includes motor2 |
The axes are as follows:
name |
direction |
notes |
|---|---|---|
joint1 |
along Z |
located above the origin |
joint2 |
along Z |
located at the end of link1 |
The motors and sensors are named as follows:
name |
notes |
|---|---|
motor1 |
RotationalMotor on joint1 |
motor2 |
RotationalMotor on joint2 |
joint1 |
PositionSensor on joint1 |
joint2 |
PositionSensor on joint2 |
endRangeSensor |
DistanceSensor at end of link2 |
two-link.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 R2022a utf8
2# documentation url: https://courses.ideate.cmu.edu/16-375
3# Planar two-link actuated arm for course exercises. The graphics use only
4# primitives for clarity of the source. The base has NULL physics so it will be
5# fixed in place. The two link lengths are adjustable parameters to demonstrate
6# using procedural elements in the prototype. The link physics properties are
7# specified using density so the dynamics will also scale, but the motor
8# parameters are constant. The end includes a distance sensor pointed along the
9# axis.
10# license: No copyright, 2020-2022 Garth Zeglin. This file is explicitly placed in the public domain.
11PROTO two-link [
12 field SFVec3f translation 0 0 0
13 field SFRotation rotation 0 1 0 0
14 field SFFloat link1Length 0.5
15 field SFFloat link2Length 0.5
16 field SFString controller "two_link"
17 field SFString name ""
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 # calculate derived parameters
30 %{
31 local halfLink1Len = fields.link1Length.value / 2
32 local halfLink2Len = fields.link2Length.value / 2
33 }%
34
35 # define the kinematic tree
36 children [
37 # add a default radio receiver and transmitter
38 Receiver {
39 }
40 Emitter {
41 }
42
43 # the cylindrical base shape is wrapped in a Transform
44 # to position it within the robot body coordinates
45 DEF baseObject Transform {
46 translation 0 0 0.1
47 children [
48 Shape {
49 appearance PBRAppearance {
50 baseColor 0.21529 0.543008 0.99855
51 metalness 0
52 }
53 geometry Cylinder {
54 height 0.2
55 radius 0.2
56 }
57 }
58 ]
59 }
60 # define the base pivot joint connecting the base
61 # and the first link
62 HingeJoint {
63 jointParameters HingeJointParameters {
64 axis 0 0 1
65 }
66 device [
67 PositionSensor {
68 name "joint1"
69 }
70 RotationalMotor {
71 name "motor1"
72 acceleration 2
73 maxVelocity 3.14
74 # maxTorque 2
75 maxTorque 20
76 }
77 ]
78 # start definition of the first link
79 endPoint Solid {
80 # place the shape origin halfway along the first link;
81 # this vector is in body coordinates, X points along
82 # the link in the neutral pose
83 translation %{=halfLink1Len}% 0 0.25
84 children [
85 # define the 'elbow' pivot connecting the links
86 HingeJoint {
87 jointParameters HingeJointParameters {
88 axis 0 0 1
89 # place the elbow joint axis at the end of the first
90 # link; position is relative to link1 origin
91 anchor %{= halfLink1Len }% 0 0
92 dampingConstant 0.1
93 }
94 device [
95 PositionSensor {
96 name "joint2"
97 }
98 RotationalMotor {
99 name "motor2"
100 acceleration 2
101 maxVelocity 6.28
102 # maxTorque 1.5
103 maxTorque 15
104 }
105 ]
106 # define the second link
107 endPoint Solid {
108 # place the link2 origin halfway along the second link
109 translation %{= halfLink1Len+halfLink2Len}% 0 0.1
110 children [
111 # the cylindrical link shape is wrapped in a Transform
112 # to position it within the link2 coordinates
113 DEF link2Shape Transform {
114 # the Cylinder shape coordinates use Z as the
115 # long axis; this 90 deg rotation around Y
116 # places the lengthwise Z axis along the link.
117 rotation 0 1 0 1.5708
118 children [
119 Shape {
120 appearance DEF greenAppearance PBRAppearance {
121 baseColor 0.413001 1 0.33489
122 metalness 0
123 }
124 geometry Cylinder {
125 height IS link2Length
126 radius 0.05
127 }
128 }
129 ]
130 } # end link2 Shape
131 # add a visual hub to the base of link2, not part of the bounding object
132 Transform {
133 rotation 0 1 0 0
134 translation %{= -halfLink2Len}% 0 0
135 children [
136 Shape {
137 appearance USE greenAppearance
138 geometry Cylinder {
139 height 0.1
140 radius 0.05
141 }
142 }
143 ]
144 } # end Transform around link2 base hub
145 # define a DistanceSensor attached to the second link Solid node
146 DistanceSensor {
147 translation %{= halfLink2Len}% 0 0
148 name "endRangeSensor"
149
150 # the sensor lookup table implicitly defines the maximum range and the units, each
151 # entry is [distance, value, noise]
152 lookupTable [
153 0 0 0
154 0.9 0.9 0 # 0.9 meters reads as 0.9 meters
155 ]
156 resolution 0.001 # assume millimeter resolution
157 numberOfRays 5
158 aperture 0.1
159 children [
160 Transform {
161 rotation 0 -1 0 1.5708
162 children [
163 Shape {
164 appearance PBRAppearance {
165 baseColor 1 0.99028 0.0584421
166 roughness 0.5
167 metalness 0.5
168 emissiveColor 1 0.99028 0.0584421
169 emissiveIntensity 0.2
170 }
171 geometry Cone {
172 bottomRadius 0.02
173 height 0.1
174 }
175 }
176 ]
177 }
178 ]
179 } # end DistanceSensor
180 ] # end link2 Solid children
181 # top-level properties of link2
182 name "link2"
183 boundingObject USE link2Shape
184 physics Physics {
185 # Assume the link is a thin-walled aluminum tube with 50 mm
186 # radius and 2 mm wall thickness. Aluminum has a density of
187 # 2700 kg/m^3, but this will be scaled by the ratio of the
188 # tube cross-section to the solid cylinder cross-section
189 # assumed by the simulator. Note that the moment of inertia
190 # around the long axis will be underestimated.
191 # density = 2700 * (R_outer**2 - R_inner**2) / R_outer**2
192 density 211.7
193 mass -1
194 }
195 }
196 }
197 # finish the definition of link1 with a shape
198 # node in the 'children' list
199 DEF link1Shape Transform {
200 rotation 0 1 0 1.5708
201 children [
202 Shape {
203 appearance DEF redAppearance PBRAppearance {
204 baseColor 00.990494 0.516915 0.468254
205 metalness 0
206 }
207 geometry Cylinder {
208 height IS link1Length
209 radius 0.05
210 }
211 }
212 ]
213 }
214 # add a visual hub to the base of link1, not part of the bounding object
215 Transform {
216 rotation 1 0 0 0
217 translation %{= -halfLink1Len}% 0 0
218 children [
219 Shape {
220 appearance USE redAppearance
221 geometry Cylinder {
222 height 0.1
223 radius 0.05
224 }
225 }
226 ]
227 } # end Transform around link1 base hub
228 # add a visual hub to the end of link1, not part of the bounding object
229 Transform {
230 rotation 1 0 0 0
231 translation %{= halfLink1Len}% 0 0
232 children [
233 Shape {
234 appearance USE redAppearance
235 geometry Cylinder {
236 height 0.1
237 radius 0.05
238 }
239 }
240 ]
241 } # end Transform around link1 end hub
242 ] # close the children list of the link1 node
243 # top-level properties of link1
244 name "link1"
245 boundingObject USE link1Shape
246 physics Physics {
247 # See notes for link2 density; this assumes the same geometry.
248 density 211.7
249 mass -1
250 }
251 }
252 }
253 ] # close the children list of the base node
254 # define top-level properties of the base
255 boundingObject USE baseObject
256
257 # the base of the robot itself has NULL physics to simulate being fixed to the ground
258 # physics Physics { density -1 mass 10 }
259
260 } # close the Robot definition
261}
Sample Control Code¶
1# two_link.py
2#
3# Sample Webots controller file for driving the two-link arm
4# with two driven joints. This example simulates a passive
5# distal link by applying zero torque, then moves the
6# base joint in a periodic excitation.
7
8# No copyright, 2020-2021, Garth Zeglin. This file is
9# explicitly placed in the public domain.
10
11print("loading two_link.py...")
12
13# Import the Webots simulator API.
14from controller import Robot
15
16# Define the time step in milliseconds between controller updates.
17EVENT_LOOP_DT = 200
18
19# Request a proxy object representing the robot to control.
20robot = Robot()
21robot_name = robot.getName()
22print("%s: controller connected." % (robot_name))
23
24# Fetch handle for the 'base' and 'elbow' joint motors.
25j1 = robot.getDevice('motor1')
26j2 = robot.getDevice('motor2')
27
28# Configure the motor for velocity control by setting
29# the position targets to infinity.
30j1.setPosition(float('inf'))
31
32# Start out with a 3 radian/second target rotational
33# velocity (roughly 180 deg/sec).
34j1.setVelocity(3)
35
36# Configure the second motor to freewheel. Please note
37# this does not turn off the hinge friction. For reference see:
38# https://cyberbotics.com/doc/reference/motor
39# https://cyberbotics.com/doc/reference/rotationalmotor
40j2.setTorque(0.0)
41
42# Run loop to execute a periodic script until the simulation quits.
43# If the controller returns -1, the simulator is quitting.
44while robot.step(EVENT_LOOP_DT) != -1:
45 # Read simulator clock time.
46 t = robot.getTime()
47
48 # Change the target velocity in a cycle with a two-second period.
49 if int(t) % 2 == 0:
50 j1.setVelocity(0)
51 else:
52 j1.setVelocity(3)
53