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 R2023b utf8
2
3EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/appearances/protos/PaintedWood.proto"
4EXTERNPROTO "../protos/HL-A11-room.proto"
5
6WorldInfo {
7}
8Viewpoint {
9 orientation 0.03521347544237489 0.005229997199804745 -0.9993661282415764 2.900390900095265
10 position 7.05668589025691 1.8111557676050998 1.6673502780197862
11 followType "None"
12}
13Background {
14 skyColor [
15 0.1 0.1 0.1
16 ]
17}
18SpotLight {
19 attenuation 0 0 1
20 beamWidth 0.3
21 cutOffAngle 0.4
22 direction -2 -2 1
23 intensity 5
24 location 1.5 2 0.1
25}
26HL-A11-room {
27 rotation 0 0 1 0
28}
29Robot {
30 translation 0 -0.5 0
31 children [
32 DEF baseObject Pose {
33 translation 0 0 0.6
34 children [
35 Shape {
36 appearance DEF baseColor PaintedWood {
37 colorOverride 0.21529 0.543008 0.99855
38 }
39 geometry Box {
40 size 0.3 0.5 1.2
41 }
42 }
43 ]
44 }
45 HingeJoint {
46 jointParameters HingeJointParameters {
47 anchor 0 0 1
48 }
49 device [
50 PositionSensor {
51 name "joint1"
52 }
53 RotationalMotor {
54 name "motor1"
55 controlPID 1 0 0
56 maxVelocity 3.14
57 minPosition -10
58 maxPosition 10
59 maxTorque 20
60 }
61 ]
62 endPoint Solid {
63 translation 0.15 0 1
64 rotation 1 0 0 0
65 children [
66 DEF link1Shape Pose {
67 translation 0.08 0 0.1
68 children [
69 Shape {
70 appearance DEF link1Color PaintedWood {
71 colorOverride 0.990494 0.516915 0.468254
72 }
73 geometry Box {
74 size 0.1 0.3 1
75 }
76 }
77 ]
78 }
79 HingeJoint {
80 jointParameters HingeJointParameters {
81 anchor 0 0 0.5
82 dampingConstant 0.1
83 }
84 device [
85 PositionSensor {
86 name "joint2"
87 }
88 RotationalMotor {
89 name "motor2"
90 controlPID 1 0 0
91 maxVelocity 3.14
92 minPosition -10
93 maxPosition 10
94 maxTorque 20
95 }
96 ]
97 endPoint Solid {
98 translation 0.13 0 0.5
99 rotation 1 0 0 0
100 children [
101 DEF link2AShape Pose {
102 translation 0.07 0 0.2
103 children [
104 Shape {
105 appearance PaintedWood {
106 colorOverride 0.413001 1 0.33489
107 }
108 geometry Box {
109 size 0.1 0.1 0.6
110 }
111 }
112 ]
113 }
114 ]
115 name "link2"
116 boundingObject USE link2AShape
117 physics Physics {
118 density -1
119 mass 0.5
120 }
121 }
122 }
123 Pose {
124 translation 0.18 0 0.5
125 rotation 0 1 0 1.5708
126 children [
127 Shape {
128 appearance USE link1Color
129 geometry Cylinder {
130 height 0.2
131 radius 0.01
132 }
133 }
134 ]
135 }
136 ]
137 name "link1"
138 boundingObject USE link1Shape
139 physics Physics {
140 density -1
141 mass 0.5
142 }
143 }
144 }
145 Pose {
146 translation 0.2 0 1
147 rotation 0 1 0 1.5708
148 children [
149 Shape {
150 appearance USE baseColor
151 geometry Cylinder {
152 height 0.2
153 radius 0.01
154 }
155 }
156 ]
157 }
158 ]
159 name "left"
160 boundingObject USE baseObject
161 physics Physics {
162 density -1
163 mass 20
164 }
165 controller "dbl_pend"
166}
167Robot {
168 translation 0 0.5 0
169 children [
170 DEF baseObject Pose {
171 translation 0 0 0.6
172 children [
173 Shape {
174 appearance DEF baseColor PaintedWood {
175 colorOverride 0.21529 0.543008 0.99855
176 }
177 geometry Box {
178 size 0.3 0.5 1.2
179 }
180 }
181 ]
182 }
183 HingeJoint {
184 jointParameters HingeJointParameters {
185 anchor 0 0 1
186 }
187 device [
188 PositionSensor {
189 name "joint1"
190 }
191 RotationalMotor {
192 name "motor1"
193 controlPID 1 0 0
194 maxVelocity 3.14
195 minPosition -10
196 maxPosition 10
197 maxTorque 20
198 }
199 ]
200 endPoint Solid {
201 translation 0.15 0 1
202 rotation 1 0 0 0
203 children [
204 DEF link1Shape Pose {
205 translation 0.08 0 0.1
206 children [
207 Shape {
208 appearance DEF link1Color PaintedWood {
209 colorOverride 0.990494 0.516915 0.468254
210 }
211 geometry Box {
212 size 0.1 0.3 1
213 }
214 }
215 ]
216 }
217 HingeJoint {
218 jointParameters HingeJointParameters {
219 anchor 0 0 0.5
220 dampingConstant 0.1
221 }
222 device [
223 PositionSensor {
224 name "joint2"
225 }
226 RotationalMotor {
227 name "motor2"
228 controlPID 1 0 0
229 maxVelocity 3.14
230 minPosition -10
231 maxPosition 10
232 maxTorque 20
233 }
234 ]
235 endPoint Solid {
236 translation 0.13 0 0.5
237 rotation 1 0 0 0
238 children [
239 DEF link2AShape Pose {
240 translation 0.07 0 0.2
241 children [
242 Shape {
243 appearance PaintedWood {
244 colorOverride 0.413001 1 0.33489
245 }
246 geometry Box {
247 size 0.1 0.1 0.6
248 }
249 }
250 ]
251 }
252 ]
253 name "link2"
254 boundingObject USE link2AShape
255 physics Physics {
256 density -1
257 mass 0.5
258 }
259 }
260 }
261 Pose {
262 translation 0.18 0 0.5
263 rotation 0 1 0 1.5708
264 children [
265 Shape {
266 appearance USE link1Color
267 geometry Cylinder {
268 height 0.2
269 radius 0.01
270 }
271 }
272 ]
273 }
274 ]
275 name "link1"
276 boundingObject USE link1Shape
277 physics Physics {
278 density -1
279 mass 0.5
280 }
281 }
282 }
283 Pose {
284 translation 0.2 0 1
285 rotation 0 1 0 1.5708
286 children [
287 Shape {
288 appearance USE baseColor
289 geometry Cylinder {
290 height 0.2
291 radius 0.01
292 }
293 }
294 ]
295 }
296 ]
297 name "right"
298 boundingObject USE baseObject
299 physics Physics {
300 density -1
301 mass 20
302 }
303 controller "dbl_pend"
304}