Dobot Magician Lite¶
The Dobot model approximates the commercially available Dobot Magician Lite educational robot arm. The arm features three DOF and modular end effectors, several of which add a fourth DOF for Z axis rotation. IDeATe owns one of these robots including the following end effectors: Pen Holder, Suction Cup, and Soft Gripper. The system also includes the optional “Magic Box” controller
This model is demonstrated in the dobot.wbt world available within the
Webots.zip archive.
Screenshot of Webots model of the Dobot Magician Lite. For scale, each arm link is 150 mm long, and the red sample object is a 15 mm square, 10 mm tall.¶
Physical Robot¶
Photo of the Dobot Magician Lite in the lab, shown in the home pose and with the suction cup end effector installed. Each of the arm links is 150 mm from axis to axis. The grid on the table pad has a minor spacing of 20 mm and a major spacing of 100 mm.¶
Dobot Kinematics¶
Schematic of the Dobot simulation model with labeled joints. The +X axis points toward the right, the +Y axis is into the page, and +Z points up.¶
Schematic of the Dobot simulation model posed with the suction cup origin located at table height at XYZ Cartesian coordinates (250, 0, 0). The J2 and J3 angles are labeled as dimensions.¶
The core arm has three DOF driven by stepper motors:
J1: Z axis rotation in the base
J2: Y axis rotation in the “shoulder”, measured with respect to the Z axis.
J3: Y axis rotation in the “elbow”, measured with respect to the horizontal plane.
The key detail of the kinematics is that the arm is implemented as two four-bar linkages which act to keep the end effector vertical at all times. The design also keeps both the J2 and J3 motors located on the rotating part right above the base. The J2 and J3 motor mass is supported by the J1 bearings and presents minimal inertial load to the J1 motor. The elbow structure located between the two arm linkages also maintains the same angular orientation at all times.
Several of the end effectors include a J4 actuator for Z axis (yaw) rotation. Only J1 and J4 affect the Z axis rotation of the end effector. The pitch and roll rotations are always zero.
The Tool Center Point (TCP) is the traditional term for the origin of the tool coordinate system used for kinematic calculations. For the following, it is assumed to lie at the end of the suction cup manipulator.
Schematic of the Dobot simulation model with dimensions matching the Webots model.¶
Forward Kinematics¶
TODO: derivation of the forward kinematics.
Inverse Kinematics¶
The inverse kinematics of the robots calculates the joint angles to reach a specific Cartesian end effector location and rotation. The machine is four DOF, so the workspace includes a volume of 3D points and the rotation of the effector around the vertical, specifed as a tuple of (X,Y,Z,theta).
The problem can be decomposed into three parts:
a solution for J1 depending only on the (X,Y) projection of the TCP
a solution for J2 and J3 depending only on the radial distance of the TCP from the base and the height: (R, Z)
a solution for J4 depending only J1 and the desired TCP rotation: (J1, theta)
Diagram representing the kinematics of J2 and J3 within a vertical plane assumed to lie between the center of the base and the TCP. dr is the radial location of the TCP within that plane, dz is the height. L1 and L2 are the lengths of the arm links. alpha, beta, gamma, and radius are intermediate values used in the calculation.¶
The gist of the calculation given a endpoint location at (dr, dz):
compute the TCP polar coordinates labeled (radius, gamma)
apply the law of cosines to calculate the angle beta
apply the law of sines to calculate the angle alpha
apply algebra to get the actual joint angles, since gamma+alpha+J2 is 90 degrees, and J3+beta+(90-J2) is 180 degrees
TODO: full derivation of the inverse kinematics.
Sample Control Code¶
The control demo includes an implementation of forward and inverse kinematic calculations to support Cartesian-space programming. It does not implement any trajectory generation or attempt to emulate the specific command language of the physical robot.
1# dobot.py
2#
3# Sample Webots controller file for driving the Dobot robot
4# arm model.
5
6# No copyright, 2022, Garth Zeglin. This file is
7# explicitly placed in the public domain.
8
9print("loading dobot.py...")
10
11# Import standard Python modules.
12import math
13
14# Import the Webots simulator API.
15from controller import Robot
16
17# Define the time step in milliseconds between controller updates.
18EVENT_LOOP_DT = 200
19
20def deg2rad(deg):
21 return deg*(math.pi/180.0)
22
23def rad2deg(rad):
24 return rad*(180.0/math.pi)
25
26################################################################
27# The sample controller is defined as an class which is then instanced as a
28# singleton control object. This is conventional Python practice and also
29# simplifies the implementation of the Arduino interface which connects this
30# code to physical hardware.
31
32class Dobot(Robot):
33 def __init__(self):
34
35 # Initialize the superclass which implements the Robot API.
36 super().__init__()
37
38 robot_name = self.getName()
39 print("%s: controller connected." % (robot_name))
40
41 # Define kinematic properties of arm. This should be kept in sync with
42 # the simulation geometry. The following numbers are close to those derived from
43 # observations of the Dobot controller.
44 self.L1 = 150.0 # 149.8
45 self.L2 = 150.0 # 148.0
46 self.X0 = 90.0 # 92.8
47 self.Z0 = 0.0 # 0.1
48
49 # Fetch handles for the four axis motors.
50 j1 = self.getDevice('motor1')
51 j2 = self.getDevice('motor2')
52 j3 = self.getDevice('motor3')
53 j4 = self.getDevice('motor4')
54
55 # Convenience list of all actuators.
56 self.motors = [j1, j2, j3, j4]
57
58 # Fetch handle for suction cup 'connector'.
59 self.suction = self.getDevice('suction')
60 self.suction.enablePresence(EVENT_LOOP_DT)
61
62 # Initialize pose generators.
63 # self.position = self.position_gen()
64 self.position = self.cycle_gen()
65 return
66
67 #================================================================
68 def forward_kinematics(self, j1, j2, j3, j4):
69 """Compute the tool center point (TCP) position and rotation of the
70 Dobot, accepting four joint angles in radians and returning a (x, y, z,
71 r) tuple with the TCP position in millimeters and rotation in radians.
72 """
73
74 # calculate the radial distance of the TCP along the ground plane
75 rad = self.X0 + self.L1 * math.sin(j2) + self.L2 * math.cos(j3)
76
77 # calculate the height of the TCP above the ground plane
78 z = self.Z0 + self.L1 * math.cos(j2) - self.L2 * math.sin(j3)
79
80 # calculate the XY position of the TCP by rotating the radial distance using J1
81 x = rad * math.cos(j1)
82 y = rad * math.sin(j1)
83
84 # The TCP frame rotation is determined entirely by J1 and J4
85 rot = j1 + j4
86
87 return x, y, z, rot
88
89 #================================================================
90 def inverse_kinematics(self, x, y, z, r):
91 """Compute the joint angles of the robot, accepting a TCP location and
92 rotation in millimeters and radians and returning a (j1, j2, j3, j4) tuple in radians."""
93
94 # calculate the radial distance and orientation of the TCP projection on the ground plane
95 rad = math.sqrt(x*x + y*y)
96 j1 = math.atan2(y, x)
97
98 # the end effector rotation transforms the j1 rotation to the TCP frame rotation
99 j4 = r - j1
100
101 # subtract the offsets to solve the two-link IK problem
102 dr = rad - self.X0
103 dz = z - self.Z0
104
105 # solve in polar XZ coordinates
106 radiussq = dr*dr + dz*dz
107 radius = math.sqrt(radiussq)
108 gamma = math.atan2(dz, dr)
109
110 # use the law of cosines to compute the upper internal angle
111 # R**2 = l1**2 + l2**2 - 2*l1*l2*cos(beta)
112 acosarg = (radiussq - self.L1**2 - self.L2**2) / (-2 * self.L1 * self.L2)
113 if acosarg < -1.0: beta = math.pi
114 elif acosarg > 1.0: beta = 0.0
115 else: beta = math.acos(acosarg)
116
117 # use the law of sines to find the lower internal angle
118 # radius / sin(beta) = l2 / sin(alpha)
119 if radius > 0.0:
120 alpha = math.asin(self.L2 * math.sin(beta) / radius)
121 else:
122 alpha = 0.0
123
124 # calculate the joint angles
125 j2 = math.pi/2 - alpha - gamma
126 j3 = math.pi - beta - alpha - gamma
127
128 return j1, j2, j3, j4
129
130 #================================================================
131 # cycle of poses to move a block back and forth
132 def cycle_gen(self):
133 home_pose = [0.0, 0.0, 0.0, 0.0]
134 grasp1_pose = self.inverse_kinematics(250.0, 0.0, -46.0, 0.0) # xyzr
135 print("IK grasp1 solution radians:", grasp1_pose)
136 print("IK grasp1 solution degrees:", [rad2deg(a) for a in grasp1_pose])
137
138 lift1_pose = self.inverse_kinematics(250.0, 0.0, -26.0, 0.0) # xyzr
139
140 x2 = 300.0
141 y2 = 0.0
142 grasp2_pose = self.inverse_kinematics(x2, y2, -46.0, 0.0) # xyzr
143 lift2_pose = self.inverse_kinematics(x2, y2, -26.0, 0.0) # xyzr
144
145 while True:
146 yield home_pose
147 yield lift1_pose
148 yield grasp1_pose
149 self.suction.lock()
150 yield lift1_pose
151 yield lift2_pose
152 yield grasp2_pose
153 self.suction.unlock()
154 yield lift2_pose
155
156 yield home_pose
157 yield lift2_pose
158 yield grasp2_pose
159 self.suction.lock()
160 yield lift2_pose
161 yield lift1_pose
162 yield grasp1_pose
163 self.suction.unlock()
164 yield lift1_pose
165
166 #================================================================
167 def set_targets(self, pose):
168 """Convenience function to set all joint targets."""
169 for joint, angle in zip(self.motors, pose):
170 joint.setPosition(angle)
171
172 def run(self):
173 iteration_count = 5
174 cycle_count = iteration_count
175
176 while robot.step(EVENT_LOOP_DT) != -1:
177
178 # Read simulator clock time.
179 t = robot.getTime()
180 # print("Suction presence:", self.suction.getPresence())
181
182 # Change the target position in a cycle with a two-second period.
183 cycle_count -= 1
184 if cycle_count <= 0:
185 cycle_count = iteration_count
186 angles = next(self.position)
187 print(f"Moving to {angles}")
188 self.set_targets(angles)
189
190################################################################
191# If running directly from Webots as a script, the following will begin execution.
192# This will have no effect when this file is imported as a module.
193if __name__ == "__main__":
194 robot = Dobot()
195 robot.run()
World File¶
The robot is directly defined within the world file. N.B. this may later be converted to a proto file.
The complexity of the model comes from defining the closed kinematics loops of the four-bar linkages. Each of the two arm sections includes two slender links and four hinge joints. The loops are closed in Webots using SolidReference nodes within the endPoint field of HingeJoint nodes to close a loop by connecting the last pivot back to a previously defined and named solid body.
1#VRML_SIM R2022a utf8
2WorldInfo {
3 basicTimeStep 5
4}
5Viewpoint {
6 orientation -0.18474475956838535 0.11228893425794732 0.9763506383749815 1.9974740127447057
7 position 0.1937657109787663 -0.5036934708616577 1.1348369642534586
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 floorSize 2 2
21}
22Solid {
23 children [
24 DEF pedestal pedestal {
25 width 1
26 depth 0.5
27 height 0.8
28 }
29 ]
30 name "pedestal"
31 boundingObject USE pedestal
32}
33Robot {
34 translation -0.2 0 0.8
35 children [
36 DEF base Transform {
37 translation 0 0 0.0385
38 children [
39 Shape {
40 appearance GlossyPaint {
41 baseColor 0.180392 0 1
42 }
43 geometry Box {
44 size 0.15 0.15 0.077
45 }
46 }
47 ]
48 }
49 HingeJoint {
50 jointParameters HingeJointParameters {
51 axis 0 0 1
52 }
53 device [
54 PositionSensor {
55 name "joint1"
56 }
57 RotationalMotor {
58 name "motor1"
59 minPosition -2.35
60 maxPosition 2.35
61 }
62 ]
63 endPoint Solid {
64 translation 0 0 0.077
65 children [
66 DEF link1 Transform {
67 translation 0 0 0.03
68 children [
69 Shape {
70 appearance PBRAppearance {
71 baseColor 0.397253 0.535546 1
72 baseColorMap ImageTexture {
73 url [
74 "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/matte_car_paint/matte_car_paint_base_color.jpg"
75 ]
76 filtering 5
77 }
78 transparency 0.15
79 roughnessMap ImageTexture {
80 url [
81 "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/glossy_car_paint/glossy_car_paint_roughness.jpg"
82 ]
83 filtering 5
84 }
85 metalness 0
86 normalMap ImageTexture {
87 url [
88 "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/glossy_car_paint/glossy_car_paint_normal.jpg"
89 ]
90 filtering 5
91 }
92 normalMapFactor 0.5
93 textureTransform TextureTransform {
94 scale 10 10
95 }
96 }
97 geometry Box {
98 size 0.1 0.1 0.06
99 }
100 }
101 ]
102 }
103 HingeJoint {
104 jointParameters HingeJointParameters {
105 axis 0 1 0
106 anchor 0 0 0.03
107 }
108 device [
109 PositionSensor {
110 name "joint2"
111 }
112 RotationalMotor {
113 name "motor2"
114 minPosition -0.08
115 maxPosition 1.4
116 }
117 ]
118 endPoint Solid {
119 translation 0 0 0.03
120 rotation 0 1 0 0
121 children [
122 DEF link2 Transform {
123 translation 0 0 0.075
124 children [
125 Shape {
126 appearance GlossyPaint {
127 baseColor 0.736309 0.796796 1
128 }
129 geometry Box {
130 size 0.012 0.05 0.15
131 }
132 }
133 ]
134 }
135 HingeJoint {
136 jointParameters HingeJointParameters {
137 axis 0 1 0
138 anchor 0 0 0.15
139 }
140 endPoint Solid {
141 translation 0 0 0.15
142 rotation 0 1 0 0
143 children [
144 DEF elbow Transform {
145 translation 0.025 0 0
146 rotation 1 0 0 1.5708
147 children [
148 Shape {
149 appearance PBRAppearance {
150 baseColor 0.736309 0.796796 1
151 baseColorMap ImageTexture {
152 url [
153 "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/matte_car_paint/matte_car_paint_base_color.jpg"
154 ]
155 filtering 5
156 }
157 transparency 0.15
158 roughnessMap ImageTexture {
159 url [
160 "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/glossy_car_paint/glossy_car_paint_roughness.jpg"
161 ]
162 filtering 5
163 }
164 metalness 0
165 normalMap ImageTexture {
166 url [
167 "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/glossy_car_paint/glossy_car_paint_normal.jpg"
168 ]
169 filtering 5
170 }
171 normalMapFactor 0.5
172 textureTransform TextureTransform {
173 scale 10 10
174 }
175 }
176 geometry Cylinder {
177 height 0.04
178 radius 0.0425
179 }
180 }
181 ]
182 }
183 HingeJoint {
184 jointParameters HingeJointParameters {
185 axis 0 1 0
186 anchor 0.05 0 0.025
187 }
188 device [
189 PositionSensor {
190 name "joint3"
191 }
192 RotationalMotor {
193 name "motor3"
194 minPosition -0.17
195 maxPosition 1.48
196 }
197 ]
198 endPoint Solid {
199 translation 0.05 0 0.025
200 rotation 0 1 0 0
201 children [
202 DEF link3 Transform {
203 translation 0.075 0 0
204 children [
205 Shape {
206 appearance GlossyPaint {
207 baseColor 0.869856 0.899718 1
208 }
209 geometry Box {
210 size 0.15 0.03 0.012
211 }
212 }
213 ]
214 }
215 HingeJoint {
216 jointParameters HingeJointParameters {
217 axis 0 1 0
218 anchor 0.15 0 0
219 }
220 endPoint Solid {
221 translation 0.15 0 0
222 rotation 0 1 0 0
223 children [
224 DEF linkEnd Transform {
225 translation 0.01 0 -0.0125
226 children [
227 Shape {
228 appearance PBRAppearance {
229 baseColor 0.869856 0.899718 1
230 baseColorMap ImageTexture {
231 url [
232 "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/matte_car_paint/matte_car_paint_base_color.jpg"
233 ]
234 filtering 5
235 }
236 transparency 0.15
237 roughnessMap ImageTexture {
238 url [
239 "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/glossy_car_paint/glossy_car_paint_roughness.jpg"
240 ]
241 filtering 5
242 }
243 metalness 0
244 normalMap ImageTexture {
245 url [
246 "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/glossy_car_paint/glossy_car_paint_normal.jpg"
247 ]
248 filtering 5
249 }
250 normalMapFactor 0.5
251 textureTransform TextureTransform {
252 scale 10 10
253 }
254 }
255 geometry Box {
256 size 0.09 0.03 0.045
257 }
258 }
259 ]
260 }
261 HingeJoint {
262 jointParameters HingeJointParameters {
263 axis 0 0 1
264 anchor 0.04 0 0
265 }
266 device [
267 PositionSensor {
268 name "joint4"
269 }
270 RotationalMotor {
271 name "motor4"
272 }
273 ]
274 endPoint Solid {
275 translation 0.04 0 -0.035
276 children [
277 DEF suction Transform {
278 translation 0 0 -0.0205
279 children [
280 Shape {
281 appearance GlossyPaint {
282 }
283 geometry Cylinder {
284 height 0.041
285 radius 0.005
286 }
287 }
288 ]
289 }
290 Connector {
291 translation 0 0 -0.041
292 rotation 0 1 0 1.5708
293 name "suction"
294 model "suction"
295 type "active"
296 autoLock TRUE
297 rotationTolerance 3.14
298 numberOfRotations 0
299 }
300 ]
301 boundingObject USE suction
302 physics Physics {
303 }
304 }
305 }
306 ]
307 name "end"
308 boundingObject USE linkEnd
309 physics Physics {
310 density 1500
311 }
312 }
313 }
314 ]
315 name "link3A"
316 boundingObject USE link3
317 physics Physics {
318 }
319 }
320 }
321 HingeJoint {
322 jointParameters HingeJointParameters {
323 axis 0 1 0
324 anchor 0.025 0 0
325 }
326 endPoint Solid {
327 translation 0.025 0 0
328 rotation 0 1 0 0
329 children [
330 DEF link3B Transform {
331 translation 0.075 0 0
332 children [
333 Shape {
334 appearance GlossyPaint {
335 baseColor 0.869856 0.899718 1
336 }
337 geometry Box {
338 size 0.15 0.02 0.01
339 }
340 }
341 ]
342 }
343 HingeJoint {
344 jointParameters HingeJointParameters {
345 axis 0 1 0
346 anchor 0.15 0 0
347 }
348 endPoint SolidReference {
349 solidName "end"
350 }
351 }
352 ]
353 name "link3B"
354 boundingObject USE link3B
355 physics Physics {
356 }
357 }
358 }
359 ]
360 name "elbow"
361 boundingObject USE elbow
362 physics Physics {
363 density 1500
364 }
365 }
366 }
367 ]
368 name "link2A"
369 boundingObject USE link2
370 physics Physics {
371 }
372 }
373 }
374 HingeJoint {
375 jointParameters HingeJointParameters {
376 axis 0 1 0
377 anchor 0.025 0 0.03
378 }
379 endPoint Solid {
380 translation 0.025 0 0.03
381 rotation 0 1 0 0
382 children [
383 DEF link2B Transform {
384 translation 0 0 0.075
385 children [
386 Shape {
387 appearance GlossyPaint {
388 baseColor 0.736309 0.796796 1
389 }
390 geometry Box {
391 size 0.012 0.05 0.15
392 }
393 }
394 ]
395 }
396 HingeJoint {
397 jointParameters HingeJointParameters {
398 axis 0 1 0
399 anchor 0 0 0.15
400 }
401 endPoint SolidReference {
402 solidName "elbow"
403 }
404 }
405 ]
406 name "link2B"
407 boundingObject USE link2B
408 physics Physics {
409 }
410 }
411 }
412 ]
413 name "link1"
414 boundingObject USE link1
415 physics Physics {
416 }
417 }
418 }
419 ]
420 name "Dobot"
421 boundingObject USE base
422 controller "dobot"
423}
424Solid {
425 translation 0.05 0 0.805
426 children [
427 Connector {
428 translation 0 0 0.005
429 rotation 0.70710528118436 -3.3905113482557537e-09 0.707108281185553 3.14159
430 model "suction"
431 type "passive"
432 autoLock TRUE
433 rotationTolerance 3.14
434 numberOfRotations 0
435 }
436 DEF box Shape {
437 appearance GlossyPaint {
438 baseColor 1 0 0.0166629
439 }
440 geometry Box {
441 size 0.015 0.015 0.01
442 }
443 }
444 ]
445 boundingObject USE box
446 physics Physics {
447 }
448}