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 R2023b utf8
2
3EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/floors/protos/RectangleArena.proto"
4EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/appearances/protos/GlossyPaint.proto"
5EXTERNPROTO "../protos/pedestal.proto"
6
7WorldInfo {
8 basicTimeStep 5
9}
10Viewpoint {
11 orientation -0.18474475956838535 0.11228893425794732 0.9763506383749815 1.9974740127447057
12 position 0.1937657109787663 -0.5036934708616577 1.1348369642534586
13}
14Background {
15 skyColor [
16 0.1 0.1 0.1
17 ]
18}
19DirectionalLight {
20 direction -0.4 -0.5 -1
21 intensity 3
22 castShadows TRUE
23}
24RectangleArena {
25 floorSize 2 2
26}
27Solid {
28 children [
29 DEF pedestal pedestal {
30 width 1
31 depth 0.5
32 height 0.8
33 }
34 ]
35 name "pedestal"
36 boundingObject USE pedestal
37}
38Robot {
39 translation -0.2 0 0.8
40 children [
41 DEF base Transform {
42 translation 0 0 0.0385
43 children [
44 Shape {
45 appearance GlossyPaint {
46 baseColor 0.180392 0 1
47 }
48 geometry Box {
49 size 0.15 0.15 0.077
50 }
51 }
52 ]
53 }
54 HingeJoint {
55 jointParameters HingeJointParameters {
56 axis 0 0 1
57 }
58 device [
59 PositionSensor {
60 name "joint1"
61 }
62 RotationalMotor {
63 name "motor1"
64 minPosition -2.35
65 maxPosition 2.35
66 }
67 ]
68 endPoint Solid {
69 translation 0 0 0.077
70 children [
71 DEF link1 Transform {
72 translation 0 0 0.03
73 children [
74 Shape {
75 appearance PBRAppearance {
76 baseColor 0.397253 0.535546 1
77 baseColorMap ImageTexture {
78 url [
79 "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/matte_car_paint/matte_car_paint_base_color.jpg"
80 ]
81 filtering 5
82 }
83 transparency 0.15
84 roughnessMap ImageTexture {
85 url [
86 "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/glossy_car_paint/glossy_car_paint_roughness.jpg"
87 ]
88 filtering 5
89 }
90 metalness 0
91 normalMap ImageTexture {
92 url [
93 "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/glossy_car_paint/glossy_car_paint_normal.jpg"
94 ]
95 filtering 5
96 }
97 normalMapFactor 0.5
98 textureTransform TextureTransform {
99 scale 10 10
100 }
101 }
102 geometry Box {
103 size 0.1 0.1 0.06
104 }
105 }
106 ]
107 }
108 HingeJoint {
109 jointParameters HingeJointParameters {
110 axis 0 1 0
111 anchor 0 0 0.03
112 }
113 device [
114 PositionSensor {
115 name "joint2"
116 }
117 RotationalMotor {
118 name "motor2"
119 minPosition -0.08
120 maxPosition 1.4
121 }
122 ]
123 endPoint Solid {
124 translation 0 0 0.03
125 rotation 0 1 0 0
126 children [
127 DEF link2 Transform {
128 translation 0 0 0.075
129 children [
130 Shape {
131 appearance GlossyPaint {
132 baseColor 0.736309 0.796796 1
133 }
134 geometry Box {
135 size 0.012 0.05 0.15
136 }
137 }
138 ]
139 }
140 HingeJoint {
141 jointParameters HingeJointParameters {
142 axis 0 1 0
143 anchor 0 0 0.15
144 }
145 endPoint Solid {
146 translation 0 0 0.15
147 rotation 0 1 0 0
148 children [
149 DEF elbow Transform {
150 translation 0.025 0 0
151 rotation 1 0 0 1.5708
152 children [
153 Shape {
154 appearance PBRAppearance {
155 baseColor 0.736309 0.796796 1
156 baseColorMap ImageTexture {
157 url [
158 "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/matte_car_paint/matte_car_paint_base_color.jpg"
159 ]
160 filtering 5
161 }
162 transparency 0.15
163 roughnessMap ImageTexture {
164 url [
165 "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/glossy_car_paint/glossy_car_paint_roughness.jpg"
166 ]
167 filtering 5
168 }
169 metalness 0
170 normalMap ImageTexture {
171 url [
172 "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/glossy_car_paint/glossy_car_paint_normal.jpg"
173 ]
174 filtering 5
175 }
176 normalMapFactor 0.5
177 textureTransform TextureTransform {
178 scale 10 10
179 }
180 }
181 geometry Cylinder {
182 height 0.04
183 radius 0.0425
184 }
185 }
186 ]
187 }
188 HingeJoint {
189 jointParameters HingeJointParameters {
190 axis 0 1 0
191 anchor 0.05 0 0.025
192 }
193 device [
194 PositionSensor {
195 name "joint3"
196 }
197 RotationalMotor {
198 name "motor3"
199 minPosition -0.17
200 maxPosition 1.48
201 }
202 ]
203 endPoint Solid {
204 translation 0.05 0 0.025
205 rotation 0 1 0 0
206 children [
207 DEF link3 Transform {
208 translation 0.075 0 0
209 children [
210 Shape {
211 appearance GlossyPaint {
212 baseColor 0.869856 0.899718 1
213 }
214 geometry Box {
215 size 0.15 0.03 0.012
216 }
217 }
218 ]
219 }
220 HingeJoint {
221 jointParameters HingeJointParameters {
222 axis 0 1 0
223 anchor 0.15 0 0
224 }
225 endPoint Solid {
226 translation 0.15 0 0
227 rotation 0 1 0 0
228 children [
229 DEF linkEnd Transform {
230 translation 0.01 0 -0.0125
231 children [
232 Shape {
233 appearance PBRAppearance {
234 baseColor 0.869856 0.899718 1
235 baseColorMap ImageTexture {
236 url [
237 "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/matte_car_paint/matte_car_paint_base_color.jpg"
238 ]
239 filtering 5
240 }
241 transparency 0.15
242 roughnessMap ImageTexture {
243 url [
244 "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/glossy_car_paint/glossy_car_paint_roughness.jpg"
245 ]
246 filtering 5
247 }
248 metalness 0
249 normalMap ImageTexture {
250 url [
251 "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/glossy_car_paint/glossy_car_paint_normal.jpg"
252 ]
253 filtering 5
254 }
255 normalMapFactor 0.5
256 textureTransform TextureTransform {
257 scale 10 10
258 }
259 }
260 geometry Box {
261 size 0.09 0.03 0.045
262 }
263 }
264 ]
265 }
266 HingeJoint {
267 jointParameters HingeJointParameters {
268 axis 0 0 1
269 anchor 0.04 0 0
270 }
271 device [
272 PositionSensor {
273 name "joint4"
274 }
275 RotationalMotor {
276 name "motor4"
277 }
278 ]
279 endPoint Solid {
280 translation 0.04 0 -0.035
281 children [
282 DEF suction Transform {
283 translation 0 0 -0.0205
284 children [
285 Shape {
286 appearance GlossyPaint {
287 }
288 geometry Cylinder {
289 height 0.041
290 radius 0.005
291 }
292 }
293 ]
294 }
295 Connector {
296 translation 0 0 -0.041
297 rotation 0 1 0 1.5708
298 name "suction"
299 model "suction"
300 type "active"
301 autoLock TRUE
302 rotationTolerance 3.14
303 numberOfRotations 0
304 }
305 ]
306 boundingObject USE suction
307 physics Physics {
308 }
309 }
310 }
311 ]
312 name "end"
313 boundingObject USE linkEnd
314 physics Physics {
315 density 1500
316 }
317 }
318 }
319 ]
320 name "link3A"
321 boundingObject USE link3
322 physics Physics {
323 }
324 }
325 }
326 HingeJoint {
327 jointParameters HingeJointParameters {
328 axis 0 1 0
329 anchor 0.025 0 0
330 }
331 endPoint Solid {
332 translation 0.025 0 0
333 rotation 0 1 0 0
334 children [
335 DEF link3B Transform {
336 translation 0.075 0 0
337 children [
338 Shape {
339 appearance GlossyPaint {
340 baseColor 0.869856 0.899718 1
341 }
342 geometry Box {
343 size 0.15 0.02 0.01
344 }
345 }
346 ]
347 }
348 HingeJoint {
349 jointParameters HingeJointParameters {
350 axis 0 1 0
351 anchor 0.15 0 0
352 }
353 endPoint SolidReference {
354 solidName "end"
355 }
356 }
357 ]
358 name "link3B"
359 boundingObject USE link3B
360 physics Physics {
361 }
362 }
363 }
364 ]
365 name "elbow"
366 boundingObject USE elbow
367 physics Physics {
368 density 1500
369 }
370 }
371 }
372 ]
373 name "link2A"
374 boundingObject USE link2
375 physics Physics {
376 }
377 }
378 }
379 HingeJoint {
380 jointParameters HingeJointParameters {
381 axis 0 1 0
382 anchor 0.025 0 0.03
383 }
384 endPoint Solid {
385 translation 0.025 0 0.03
386 rotation 0 1 0 0
387 children [
388 DEF link2B Transform {
389 translation 0 0 0.075
390 children [
391 Shape {
392 appearance GlossyPaint {
393 baseColor 0.736309 0.796796 1
394 }
395 geometry Box {
396 size 0.012 0.05 0.15
397 }
398 }
399 ]
400 }
401 HingeJoint {
402 jointParameters HingeJointParameters {
403 axis 0 1 0
404 anchor 0 0 0.15
405 }
406 endPoint SolidReference {
407 solidName "elbow"
408 }
409 }
410 ]
411 name "link2B"
412 boundingObject USE link2B
413 physics Physics {
414 }
415 }
416 }
417 ]
418 name "link1"
419 boundingObject USE link1
420 physics Physics {
421 }
422 }
423 }
424 ]
425 name "Dobot"
426 boundingObject USE base
427 controller "dobot"
428}
429Solid {
430 translation 0.05 0 0.805
431 children [
432 Connector {
433 translation 0 0 0.005
434 rotation 0.70710528118436 -3.3905113482557537e-09 0.707108281185553 3.14159
435 model "suction"
436 type "passive"
437 autoLock TRUE
438 rotationTolerance 3.14
439 numberOfRotations 0
440 }
441 DEF box Shape {
442 appearance GlossyPaint {
443 baseColor 1 0 0.0166629
444 }
445 geometry Box {
446 size 0.015 0.015 0.01
447 }
448 }
449 ]
450 boundingObject USE box
451 physics Physics {
452 }
453}