44from roboticstoolbox .robot .ERobot import ERobot
55from math import pi
66
7-
87class AL5D (ERobot ):
98 """
10- Class that imports a Puma 560 URDF model
9+ Class that imports a AL5D URDF model
1110
12- ``Puma560 ()`` is a class which imports a Unimation Puma560 robot definition
11+ ``AL5D ()`` is a class which imports a Lynxmotion AL5D robot definition
1312 from a URDF file. The model describes its kinematic and graphical
1413 characteristics.
1514
@@ -22,16 +21,7 @@ class AL5D(ERobot):
2221 Defined joint configurations are:
2322
2423 - qz, zero joint angle configuration, 'L' shaped configuration
25- - qr, vertical 'READY' configuration
26- - qs, arm is stretched out in the x-direction
27- - qn, arm is at a nominal non-singular configuration
28-
29- .. warning:: This file has been modified so that the zero-angle pose is the
30- same as the DH model in the toolbox. ``j3`` rotation is changed from
31- -𝜋/2 to 𝜋/2. Dimensions are also slightly different. Both models
32- include the pedestal height.
33-
34- .. note:: The original file is from https://github.com/nimasarli/puma560_description/blob/master/urdf/puma560_robot.urdf.xacro
24+ - up, robot poiting upwards
3525
3626 .. codeauthor:: Tassos Natsakis
3727 """
@@ -50,13 +40,12 @@ def __init__(self):
5040 )
5141
5242 self .manufacturer = "Lynxmotion"
53- # self.ee_link = self.ets[9]
5443
55- # zero angles, upper arm horizontal , lower up straight ahead
44+ # zero angles, upper arm pointing up , lower arm straight ahead
5645 self .addconfiguration ("qz" , np .array ([0 , 0 , 0 , 0 ]))
5746
58- # reference pose, arm to the right, elbow up
59- self .addconfiguration ("ru " , np .array ([0.0000 , 0.0000 , 1.5707 , 0.0000 ]))
47+ # reference pose robot pointing upwards
48+ self .addconfiguration ("up " , np .array ([0.0000 , 0.0000 , 1.5707 , 0.0000 ]))
6049
6150if __name__ == "__main__" : # pragma nocover
6251
0 commit comments