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.

../_images/dobot.png

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

../_images/Dobot-home-pose.jpg

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

../_images/Dobot-schematic-axes.png

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.

../_images/Dobot-schematic-forward-pose.png

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.

../_images/Dobot-schematic-dims.png

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:

  1. a solution for J1 depending only on the (X,Y) projection of the TCP

  2. a solution for J2 and J3 depending only on the radial distance of the TCP from the base and the height: (R, Z)

  3. a solution for J4 depending only J1 and the desired TCP rotation: (J1, theta)

../_images/Dobot-IK.png

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):

  1. compute the TCP polar coordinates labeled (radius, gamma)

  2. apply the law of cosines to calculate the angle beta

  3. apply the law of sines to calculate the angle alpha

  4. 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}