Pendulum de Deux¶
This sample project includes a pair of double pendula configured as actuated two-link arms. This model demonstrates several key concepts: rigid body dynamics, collision modeling, PD control, and keyboard input.
This model is demonstrated in the pendulum-de-deux.wbt world.
Screenshot of Webots model of the pendulum-de-deux robots. Each robot is independent; one has scripted motion, the other is keyboard driven.¶
Sample Robot Control Code¶
The controller implements keyboard input to trigger stored poses. The two-link structure is regulated using a PD controller using the arm actuators in torque mode.
1# dbl_pend.py
2#
3# Sample Webots controller file for driving a
4# fully actuated double pendulum. With both actuators
5# set to zero torque the device will act like a chaotic
6# double pendulum. Otherwise this code implements
7# PD position control to drive towards a pose.
8#
9# No copyright, 2020-2022, Garth Zeglin. This file is
10# explicitly placed in the public domain.
11
12print("loading dbl_pend.py...")
13
14# Import the Webots simulator API.
15from controller import Robot
16from controller import Keyboard
17
18# Import the standard Python math library.
19import math
20
21# Define the time step in milliseconds between
22# controller updates.
23EVENT_LOOP_DT = 20
24
25################################################################
26
27# Request a proxy object representing the robot to control.
28robot = Robot()
29name = robot.getName()
30print(f"dbl_pend.py waking up for {name}...")
31
32# Enable computer keyboard input for user control.
33keyboard = Keyboard()
34keyboard.enable(EVENT_LOOP_DT)
35
36# Fetch handles for the joint sensors.
37j1 = robot.getDevice('joint1')
38j2 = robot.getDevice('joint2')
39
40# Specify the sampling rate for the joint sensors.
41j1.enable(EVENT_LOOP_DT)
42j2.enable(EVENT_LOOP_DT)
43
44# Fetch handle for the 'base' joint motor. In this
45# example the motor will be controlled as a torque
46# motor, bypassing the Webots PID control.
47motor1 = robot.getDevice('motor1')
48motor1.setTorque(0.0)
49
50motor2 = robot.getDevice('motor2')
51motor2.setTorque(0.0)
52
53# set properties in RotationalMotor nodes:
54motor1.setAvailableTorque(4)
55motor2.setAvailableTorque(1)
56
57# Specify proportional and derivative (damping) gains
58# for the position controllers in the main event loop.
59P_gain1 = 8.0 # units are N-m / radian
60D_gain1 = 0.2 # units are N-m / (radian/sec)
61
62P_gain2 = 4.0 # units are N-m / radian
63D_gain2 = 0.1 # units are N-m / (radian/sec)
64
65# Soft torque limits to avoid error messages.
66max_tau1 = 4.0 # units are N-m
67max_tau2 = 1.0 # units are N-m
68
69# Initial position targets.
70q_d1 = None
71q_d2 = None
72
73# The controller estimates velocities using finite
74# differencing of the position sensors; these variables
75# hold the previous state.
76last_q1 = 0
77last_q2 = 0
78
79################################################################
80# Define reference poses mapped to keyboard keys.
81
82poses = {
83 't' : (0.0, 0.0), # straight up
84 'r' : (0.5, -1.0), # elbow to machine right (-Y)
85 'l' : (-0.5, 1.0), # elbow to machine left (+Y)
86 'b' : (math.pi, -math.pi), # elbow at bottom, link2 pointing up
87 'w' : (2*math.pi, 0.0), # straight up, but wound a full positive rotation
88}
89
90# Set joint position targets to a reference pose indicated by a single character string.
91def set_pose(key):
92 global q_d1, q_d2
93 # special case: 'p' will enter a passive zero-torque mode
94 if key == 'p':
95 q_d1 = None
96 q_d2 = None
97 else:
98 # look for a reference pose
99 pose = poses.get(key)
100 if pose is not None:
101 q_d1 = pose[0]
102 q_d2 = pose[1]
103
104################################################################
105# Metronomic choreography for automatic machine.
106def set_automatic_targets(t):
107 phase = math.fmod(t, 5.0) / 5.0
108 if phase < 0.5:
109 set_pose('l')
110 else:
111 set_pose('r')
112
113################################################################
114
115# Run an event loop until the simulation quits,
116# indicated by the step function returning -1.
117while robot.step(EVENT_LOOP_DT) != -1:
118
119 # Read simulator clock time.
120 t = robot.getTime()
121
122 # Read the new joint positions.
123 q1 = j1.getValue()
124 q2 = j2.getValue()
125
126 # Estimate current velocities in radians/sec using finite differences.
127 qd1 = (q1 - last_q1) / (0.001 * EVENT_LOOP_DT)
128 qd2 = (q2 - last_q2) / (0.001 * EVENT_LOOP_DT)
129 last_q1 = q1
130 last_q2 = q2
131
132 # Read any computer keyboard keypresses. Returns -1 or an integer keycode while a key is held down.
133 if name == 'left':
134 key = keyboard.getKey()
135 if key != -1:
136 # convert the integer key number to a lowercase single-character string,
137 # then set the target pose indicated by the key
138 set_pose(chr(key).lower())
139
140 else:
141 # the other devices run an automatic timed sequence
142 set_automatic_targets(t)
143
144 # Calculate new motor torques, limit them, and apply them to the system.
145 if q_d1 is None:
146 tau1 = 0.0
147 else:
148 tau1 = P_gain1 * (q_d1 - q1) - D_gain1 * qd1
149 tau1 = min(max(tau1, -max_tau1), max_tau1)
150
151 if q_d2 is None:
152 tau2 = 0.0
153 else:
154 tau2 = P_gain2 * (q_d2 - q2) - D_gain2 * qd2
155 tau2 = min(max(tau2, -max_tau2), max_tau2)
156
157 motor1.setTorque(tau1)
158 motor2.setTorque(tau2)
World File¶
The robot is directly modeled in the scene tree so all parameters can be visible. In general it is more flexible to transfer models to .proto files so they can be instantiated more than once.
1#VRML_SIM R2022a utf8
2WorldInfo {
3}
4Viewpoint {
5 orientation 0.1275445447336565 0.09988879918262115 -0.9867900571583291 2.2125166207147218
6 position 3.627435367384922 5.521290034201931 2.6622807309719057
7 followType "None"
8}
9Background {
10 skyColor [
11 0.1 0.1 0.1
12 ]
13}
14DirectionalLight {
15 direction 0.4 -0.5 -1
16 intensity 3
17 castShadows TRUE
18}
19RectangleArena {
20 rotation 1.8366025517039032e-06 -1.836602551703903e-06 0.9999999999966269 1.5707963267982696
21 floorSize 2 2
22}
23Robot {
24 translation 0.5 0 0
25 rotation 0 0 1 1.5708
26 children [
27 DEF baseObject Transform {
28 translation 0 0 0.6
29 children [
30 Shape {
31 appearance DEF baseColor PaintedWood {
32 colorOverride 0.21529 0.543008 0.99855
33 }
34 geometry Box {
35 size 0.3 0.5 1.2
36 }
37 }
38 ]
39 }
40 HingeJoint {
41 jointParameters HingeJointParameters {
42 anchor 0 0 1
43 }
44 device [
45 PositionSensor {
46 name "joint1"
47 }
48 RotationalMotor {
49 name "motor1"
50 controlPID 1 0 0
51 maxVelocity 3.14
52 minPosition -10
53 maxPosition 10
54 maxTorque 20
55 }
56 ]
57 endPoint Solid {
58 translation 0.15 0 1
59 rotation 1 0 0 0
60 children [
61 DEF link1Shape Transform {
62 translation 0.08 0 0.1
63 children [
64 Shape {
65 appearance DEF link1Color PaintedWood {
66 colorOverride 0.990494 0.516915 0.468254
67 }
68 geometry Box {
69 size 0.1 0.3 1
70 }
71 }
72 ]
73 }
74 HingeJoint {
75 jointParameters HingeJointParameters {
76 anchor 0 0 0.5
77 dampingConstant 0.1
78 }
79 device [
80 PositionSensor {
81 name "joint2"
82 }
83 RotationalMotor {
84 name "motor2"
85 controlPID 1 0 0
86 maxVelocity 3.14
87 minPosition -10
88 maxPosition 10
89 maxTorque 20
90 }
91 ]
92 endPoint Solid {
93 translation 0.13 0 0.5
94 rotation 1 0 0 0
95 children [
96 DEF link2AShape Transform {
97 translation 0.07 0 0.2
98 children [
99 Shape {
100 appearance PaintedWood {
101 colorOverride 0.413001 1 0.33489
102 }
103 geometry Box {
104 size 0.1 0.1 0.6
105 }
106 }
107 ]
108 }
109 ]
110 name "link2"
111 boundingObject USE link2AShape
112 physics Physics {
113 density -1
114 mass 0.5
115 }
116 }
117 }
118 Transform {
119 translation 0.18 0 0.5
120 rotation 0 1 0 1.5708
121 children [
122 Shape {
123 appearance USE link1Color
124 geometry Cylinder {
125 height 0.2
126 radius 0.01
127 }
128 }
129 ]
130 }
131 ]
132 name "link1"
133 boundingObject USE link1Shape
134 physics Physics {
135 density -1
136 mass 0.5
137 }
138 }
139 }
140 Transform {
141 translation 0.2 0 1
142 rotation 0 1 0 1.5708
143 children [
144 Shape {
145 appearance USE baseColor
146 geometry Cylinder {
147 height 0.2
148 radius 0.01
149 }
150 }
151 ]
152 }
153 ]
154 name "left"
155 boundingObject USE baseObject
156 physics Physics {
157 density -1
158 mass 20
159 }
160 controller "dbl_pend"
161}
162Robot {
163 translation -0.5 0 0
164 rotation 0 0 1 1.5708
165 children [
166 DEF baseObject Transform {
167 translation 0 0 0.6
168 children [
169 Shape {
170 appearance DEF baseColor PaintedWood {
171 colorOverride 0.21529 0.543008 0.99855
172 }
173 geometry Box {
174 size 0.3 0.5 1.2
175 }
176 }
177 ]
178 }
179 HingeJoint {
180 jointParameters HingeJointParameters {
181 anchor 0 0 1
182 }
183 device [
184 PositionSensor {
185 name "joint1"
186 }
187 RotationalMotor {
188 name "motor1"
189 controlPID 1 0 0
190 maxVelocity 3.14
191 minPosition -10
192 maxPosition 10
193 maxTorque 20
194 }
195 ]
196 endPoint Solid {
197 translation 0.15 0 1
198 rotation 1 0 0 0
199 children [
200 DEF link1Shape Transform {
201 translation 0.08 0 0.1
202 children [
203 Shape {
204 appearance DEF link1Color PaintedWood {
205 colorOverride 0.990494 0.516915 0.468254
206 }
207 geometry Box {
208 size 0.1 0.3 1
209 }
210 }
211 ]
212 }
213 HingeJoint {
214 jointParameters HingeJointParameters {
215 anchor 0 0 0.5
216 dampingConstant 0.1
217 }
218 device [
219 PositionSensor {
220 name "joint2"
221 }
222 RotationalMotor {
223 name "motor2"
224 controlPID 1 0 0
225 maxVelocity 3.14
226 minPosition -10
227 maxPosition 10
228 maxTorque 20
229 }
230 ]
231 endPoint Solid {
232 translation 0.13 0 0.5
233 rotation 1 0 0 0
234 children [
235 DEF link2AShape Transform {
236 translation 0.07 0 0.2
237 children [
238 Shape {
239 appearance PaintedWood {
240 colorOverride 0.413001 1 0.33489
241 }
242 geometry Box {
243 size 0.1 0.1 0.6
244 }
245 }
246 ]
247 }
248 ]
249 name "link2"
250 boundingObject USE link2AShape
251 physics Physics {
252 density -1
253 mass 0.5
254 }
255 }
256 }
257 Transform {
258 translation 0.18 0 0.5
259 rotation 0 1 0 1.5708
260 children [
261 Shape {
262 appearance USE link1Color
263 geometry Cylinder {
264 height 0.2
265 radius 0.01
266 }
267 }
268 ]
269 }
270 ]
271 name "link1"
272 boundingObject USE link1Shape
273 physics Physics {
274 density -1
275 mass 0.5
276 }
277 }
278 }
279 Transform {
280 translation 0.2 0 1
281 rotation 0 1 0 1.5708
282 children [
283 Shape {
284 appearance USE baseColor
285 geometry Cylinder {
286 height 0.2
287 radius 0.01
288 }
289 }
290 ]
291 }
292 ]
293 name "right"
294 boundingObject USE baseObject
295 physics Physics {
296 density -1
297 mass 20
298 }
299 controller "dbl_pend"
300}