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 joint1 |
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 }
76 ]
77 # start definition of the first link
78 endPoint Solid {
79 # place the shape origin halfway along the first link;
80 # this vector is in body coordinates, X points along
81 # the link in the neutral pose
82 translation %{=halfLink1Len}% 0 0.25
83 children [
84 # define the 'elbow' pivot connecting the links
85 HingeJoint {
86 jointParameters HingeJointParameters {
87 axis 0 0 1
88 # place the elbow joint axis at the end of the first
89 # link; position is relative to link1 origin
90 anchor %{= halfLink1Len }% 0 0
91 dampingConstant 0.1
92 }
93 device [
94 PositionSensor {
95 name "joint2"
96 }
97 RotationalMotor {
98 name "motor2"
99 acceleration 2
100 maxVelocity 6.28
101 maxTorque 1.5
102 }
103 ]
104 # define the second link
105 endPoint Solid {
106 # place the link2 origin halfway along the second link
107 translation %{= halfLink1Len+halfLink2Len}% 0 0.1
108 children [
109 # the cylindrical link shape is wrapped in a Transform
110 # to position it within the link2 coordinates
111 DEF link2Shape Transform {
112 # the Cylinder shape coordinates use Z as the
113 # long axis; this 90 deg rotation around Y
114 # places the lengthwise Z axis along the link.
115 rotation 0 1 0 1.5708
116 children [
117 Shape {
118 appearance DEF greenAppearance PBRAppearance {
119 baseColor 0.413001 1 0.33489
120 metalness 0
121 }
122 geometry Cylinder {
123 height IS link2Length
124 radius 0.05
125 }
126 }
127 ]
128 } # end link2 Shape
129 # add a visual hub to the base of link2, not part of the bounding object
130 Transform {
131 rotation 0 1 0 0
132 translation %{= -halfLink2Len}% 0 0
133 children [
134 Shape {
135 appearance USE greenAppearance
136 geometry Cylinder {
137 height 0.1
138 radius 0.05
139 }
140 }
141 ]
142 } # end Transform around link2 base hub
143 # define a DistanceSensor attached to the second link Solid node
144 DistanceSensor {
145 translation %{= halfLink2Len}% 0 0
146 name "endRangeSensor"
147
148 # the sensor lookup table implicitly defines the maximum range and the units, each
149 # entry is [distance, value, noise]
150 lookupTable [
151 0 0 0
152 0.9 0.9 0 # 0.9 meters reads as 0.9 meters
153 ]
154 resolution 0.001 # assume millimeter resolution
155 numberOfRays 5
156 aperture 0.1
157 children [
158 Transform {
159 rotation 0 -1 0 1.5708
160 children [
161 Shape {
162 appearance PBRAppearance {
163 baseColor 1 0.99028 0.0584421
164 roughness 0.5
165 metalness 0.5
166 emissiveColor 1 0.99028 0.0584421
167 emissiveIntensity 0.2
168 }
169 geometry Cone {
170 bottomRadius 0.02
171 height 0.1
172 }
173 }
174 ]
175 }
176 ]
177 } # end DistanceSensor
178 ] # end link2 Solid children
179 # top-level properties of link2
180 name "link2"
181 boundingObject USE link2Shape
182 physics Physics {
183 # Assume the link is a thin-walled aluminum tube with 50 mm
184 # radius and 2 mm wall thickness. Aluminum has a density of
185 # 2700 kg/m^3, but this will be scaled by the ratio of the
186 # tube cross-section to the solid cylinder cross-section
187 # assumed by the simulator. Note that the moment of inertia
188 # around the long axis will be underestimated.
189 # density = 2700 * (R_outer**2 - R_inner**2) / R_outer**2
190 density 211.7
191 mass -1
192 }
193 }
194 }
195 # finish the definition of link1 with a shape
196 # node in the 'children' list
197 DEF link1Shape Transform {
198 rotation 0 1 0 1.5708
199 children [
200 Shape {
201 appearance DEF redAppearance PBRAppearance {
202 baseColor 00.990494 0.516915 0.468254
203 metalness 0
204 }
205 geometry Cylinder {
206 height IS link1Length
207 radius 0.05
208 }
209 }
210 ]
211 }
212 # add a visual hub to the base of link1, not part of the bounding object
213 Transform {
214 rotation 1 0 0 0
215 translation %{= -halfLink1Len}% 0 0
216 children [
217 Shape {
218 appearance USE redAppearance
219 geometry Cylinder {
220 height 0.1
221 radius 0.05
222 }
223 }
224 ]
225 } # end Transform around link1 base hub
226 # add a visual hub to the end of link1, not part of the bounding object
227 Transform {
228 rotation 1 0 0 0
229 translation %{= halfLink1Len}% 0 0
230 children [
231 Shape {
232 appearance USE redAppearance
233 geometry Cylinder {
234 height 0.1
235 radius 0.05
236 }
237 }
238 ]
239 } # end Transform around link1 end hub
240 ] # close the children list of the link1 node
241 # top-level properties of link1
242 name "link1"
243 boundingObject USE link1Shape
244 physics Physics {
245 # See notes for link2 density; this assumes the same geometry.
246 density 211.7
247 mass -1
248 }
249 }
250 }
251 ] # close the children list of the base node
252 # define top-level properties of the base
253 boundingObject USE baseObject
254
255 # the base of the robot itself has NULL physics to simulate being fixed to the ground
256 # physics Physics { density -1 mass 10 }
257
258 } # close the Robot definition
259}
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