petercorke · Samuel-Drew · Jun 21, 2020 · Jun 10, 2020 · Jun 15, 2020 · Jun 15, 2020
diff --git a/roboticstoolbox/robot/Link.py b/roboticstoolbox/robot/Link.py
@@ -1,10 +1,6 @@
 """
 Link object.
-Python implementation by: Luis Fernando Lara Tobar and Peter Corke.
-Based on original Robotics Toolbox for Matlab code by Peter Corke.
-Permission to use and copy is granted provided that acknowledgement of
-the authors is made.
-@author: Luis Fernando Lara Tobar and Peter Corke
+Python implementation by Samuel Drew
 """
 
 from numpy import *
@@ -19,8 +15,8 @@ class Link(list):
 
     def __init__(self, *argv):
         """
-        %Link Create robot link object
-        %
+        Link Create robot link object
+
         % This the class constructor which has several call signatures.
         %
         % L = Link() is a Link object with default parameters.
@@ -165,7 +161,7 @@ def __init__(self, *argv):
             # format input into argparse
             argstr = ""
             known = ['theta', 'a', 'd', 'alpha', 'G', 'B', 'Tc', 'Jm', 'I', 'm', 'r',
-                     'offset', 'qlim', 'type', 'convention', 'sym', 'flip']
+                     'offset', 'qlim', 'type', 'convention', 'sym', 'flip', 'help']
             for arg in argv:
                 if arg in known:
                     argstr += "--" + arg + " "
@@ -226,15 +222,15 @@ def __init__(self, *argv):
 
                 if opt.theta != 0:
                     # constant value of theta means it must be prismatic
-                    self.theta = opt.theta
                     self.jointtype = 'P'
                     print('Prismatic joint, theta =', opt.theta)
                 if opt.d != 0:
                     # constant value of d means it must be revolute
-                    self.d = opt.d
                     self.jointtype = 'R'
                     print('Revolute joint, d =', opt.d)
 
+                self.theta = opt.theta
+                self.d = opt.d
                 self.a = opt.a
                 self.alpha = opt.alpha
 
@@ -291,10 +287,11 @@ def __init__(self, *argv):
                     self.B = 0
                     self.Tc = [0, 0]
                     self.qlim = 0
-                if argv[1] == 'modified':
-                    self.mdh = 1
-                else:
-                    self.mdh = 0
+
+                self.mdh = 0
+                if len(argv) == 2:
+                    if argv[1] == 'modified':
+                        self.mdh == 1
 
     def __repr__(self):
 

diff --git a/roboticstoolbox/robot/serial_link.py b/roboticstoolbox/robot/serial_link.py
@@ -4,6 +4,7 @@
 from roboticstoolbox.robot.Link import *
 from spatialmath.pose3d import *
 from scipy.optimize import minimize
+from graphics.graphics_test_features import *
 
 class SerialLink:
 
@@ -95,7 +96,7 @@ def fkine(self, jointconfig, unit='rad', alltout=False):
                 t[k] = tt * SE3(self.tool)
 
         if alltout:
-            return t, allt
+            return allt
         else:
             return t
 
@@ -108,6 +109,8 @@ def ikine(self, T, q0=None, unit='rad'):
         :return: a list of 6 joint angles.
         """
         assert T.shape == (4, 4)
+        if type(T) == SE3:
+            T = T.A
         bounds = [(link.qlim[0], link.qlim[1]) for link in self]
         reach = 0
         for link in self:
@@ -125,3 +128,32 @@ def objective(x):
             return sol.x * 180 / pi
         else:
             return sol.x
+
+    def plot(self, jointconfig, unit='rad'):
+        """
+        Creates a 3D plot of the robot in your web browser
+        :param jointconfig: takes an array or list of joint angles
+        :param unit: unit of angles. radians if not defined
+        :return: a vpython robot object.
+        """
+        if type(jointconfig) == list:
+            jointconfig = argcheck.getvector(jointconfig)
+        if unit == 'deg':
+            jointconfig = jointconfig * pi / 180
+        if jointconfig.size == self.length:
+            poses = self.fkine(jointconfig, unit, alltout=True)
+            t = list(range(len(poses)))
+            for i in range(len(poses)):
+                t[i] = poses[i].t
+            # add the base
+            t.insert(0, SE3(self.base).t)
+            grjoints = list(range(len(t)-1))
+
+            canvas_grid = init_canvas()
+
+            for i in range(1, len(t)):
+                grjoints[i-1] = RotationalJoint(vector(t[i-1][0], t[i-1][1], t[i-1][2]), vector(t[i][0], t[i][1], t[i][2]))
+            print(grjoints)
+            x = GraphicalRobot(grjoints)
+
+            return x
diff --git a/roboticstoolbox/robot/threelink.py b/roboticstoolbox/robot/threelink.py
@@ -0,0 +1,24 @@
+"""
+Defines the object 'tl' in the current workspace
+
+Also define the vector qz = [0 0 0] which corresponds to the zero joint
+angle configuration.
+
+@author: Luis Fernando Lara Tobar and Peter Corke
+
+Edited June 2020 by Samuel Drew
+"""
+
+from roboticstoolbox.robot.serial_link import *
+
+L = []
+
+L.append(Link('a', 1, 'type', 'revolute'))
+L.append(Link('a', 1, 'type', 'revolute'))
+L.append(Link('a', 1, 'type', 'revolute'))
+
+
+qz = [0,0,0]
+
+tl = SerialLink(L, name='Simple three link')
+
diff --git a/roboticstoolbox/robot/uprighttl.py b/roboticstoolbox/robot/uprighttl.py
@@ -0,0 +1,19 @@
+"""
+Defines the object 'arm' in the current workspace
+
+Also define the vector qz = [pi/4,0,-pi/3]
+"""
+
+from roboticstoolbox.robot.serial_link import *
+
+L = []
+
+L.append(Link('a', 0.1, 'd', 1, 'alpha', pi/2, 'type', 'revolute'))
+L.append(Link('a', 1, 'type', 'revolute'))
+L.append(Link('a', 0.5, 'type', 'revolute'))
+
+
+qz = [pi/4,0,-pi/3]
+
+arm = SerialLink(L, name='Upright Arm')
+
diff --git a/tests/Sam1.py b/tests/Sam1.py
@@ -0,0 +1,32 @@
+"""
+NOT A UNIT TEST
+Simple Example for Serial_link plot function
+Starts by creating a robot arm.
+uncomment and copy code to console as needed
+"""
+
+from roboticstoolbox.robot.serial_link import *
+
+L = []
+
+# create links
+L.append(Link('a', 0.1, 'd', 1, 'alpha', pi/2, 'type', 'revolute'))
+L.append(Link('a', 1, 'type', 'revolute'))
+L.append(Link('a', 0.5, 'type', 'revolute'))
+
+# create initial joint array to be passed into plot as joint configuration
+qz = [pi/4,0,-pi/3]
+
+# create serial link robot
+arm = SerialLink(L, name='Upright Arm')
+
+# plot robot
+plotbot = arm.plot(qz)
+
+## Use this code to change the joint angles
+## lift arm
+# plotbot.set_joint_angle(1, -pi/2)
+
+## rotate base:
+# plotbot.set_joint_angle(0, pi/2)
+