Merge branch 'master' of github.com:petercorke/robotics-toolbox-python

Author: jhavl

README.md

@@ -110,8 +110,8 @@ orientation parallel to y-axis (O=+Y)).
 from spatialmath import SE3
 
 T = SE3(0.8, 0.2, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1])
-sol = robot.ikine_unc(T)         # solve IK
-print(sol.q)                    # display joint angles
+sol = robot.ikine_min(T)         # solve IK
+print(sol.q)                     # display joint angles
 
 	[-0.01044    7.876    1.557    -6.81    1.571    4.686   0.5169]
 

examples/baur.py

@@ -32,17 +32,17 @@
 n = 7
 
 # Set the desired end-effector pose
-Tep = panda.fkine() * sm.SE3(0.3, 0.2, 0.3)
+Tep = panda.fkine(panda.q) * sm.SE3(0.3, 0.2, 0.3)
 
 arrived = False
 
 while not arrived:
 
     # The pose of the Panda's end-effector
-    Te = panda.fkine()
+    Te = panda.fkine(panda.q)
 
     # The manipulator Jacobian in the end-effecotr frame
-    Je = panda.jacobe()
+    Je = panda.jacobe(panda.q)
 
     # Calulate the required end-effector spatial velocity for the robot
     # to approach the goal. Gain is set to 1.0
@@ -52,7 +52,7 @@
     Y = 0.01
 
     # The manipulability Jacobian
-    Jm = panda.jacobm()
+    Jm = panda.jacobm(panda.q)
 
     # The minimum angle (in radians) in which the joint is allowed to approach
     # to its limit

examples/mmc.py

@@ -31,14 +31,14 @@
 n = 7
 
 # Set the desired end-effector pose
-Tep = panda.fkine() * sm.SE3(0.3, 0.2, 0.3)
+Tep = panda.fkine(panda.q) * sm.SE3(0.3, 0.2, 0.3)
 
 arrived = False
 
 while not arrived:
 
     # The pose of the Panda's end-effector
-    Te = panda.fkine()
+    Te = panda.fkine(panda.q)
 
     # Transform from the end-effector to desired pose
     eTep = Te.inv() * Tep
@@ -63,7 +63,7 @@
     Q[n:, n:] = (1 / e) * np.eye(6)
 
     # The equality contraints
-    Aeq = np.c_[panda.jacobe(), np.eye(6)]
+    Aeq = np.c_[panda.jacobe(panda.q), np.eye(6)]
     beq = v.reshape((6,))
 
     # The inequality constraints for joint limit avoidance

examples/neo.py

@@ -49,7 +49,7 @@
 env.add(target)
 
 # Set the desired end-effector pose to the location of target
-Tep = panda.fkine()
+Tep = panda.fkine(panda.q)
 Tep.A[:3, 3] = target.base.t
 Tep.A[2, 3] += 0.1
 
@@ -58,7 +58,7 @@
 while not arrived:
 
     # The pose of the Panda's end-effector
-    Te = panda.fkine()
+    Te = panda.fkine(panda.q)
 
     # Transform from the end-effector to desired pose
     eTep = Te.inv() * Tep
@@ -83,7 +83,7 @@
     Q[n:, n:] = (1 / e) * np.eye(6)
 
     # The equality contraints
-    Aeq = np.c_[panda.jacobe(), np.eye(6)]
+    Aeq = np.c_[panda.jacobe(panda.q), np.eye(6)]
     beq = v.reshape((6,))
 
     # The inequality constraints for joint limit avoidance
@@ -108,7 +108,8 @@
         # object on the robot to the collision in the scene
         c_Ain, c_bin = panda.link_collision_damper(
             collision, panda.q[:n], 0.3, 0.05, 1.0,
-            panda.link_dict['panda_link1'], panda.link_dict['panda_hand'])
+            startlink=panda.link_dict['panda_link1'],
+            endlink=panda.link_dict['panda_hand'])
 
         # If there are any parts of the robot within the influence distance
         # to the collision in the scene
@@ -120,7 +121,7 @@
             bin = np.r_[bin, c_bin]
 
     # Linear component of objective function: the manipulability Jacobian
-    c = np.r_[-panda.jacobm().reshape((n,)), np.zeros(6)]
+    c = np.r_[-panda.jacobm(panda.q).reshape((n,)), np.zeros(6)]
 
     # The lower and upper bounds on the joint velocity and slack variable
     lb = -np.r_[panda.qdlim[:n], 10 * np.ones(6)]

examples/rtb.py

@@ -31,13 +31,23 @@
     help='specify script to run')
 parser.add_argument('--backend', '-b', default=None,
     help='specify graphics frontend')
+parser.add_argument('--color', '-c', default='neutral',
+    help='specify terminal color scheme (neutral, lightbg, nocolor, linux), linux is for dark mode')
+parser.add_argument('--confirmexit', '-x', default=False,
+    help='confirm exit')
+parser.add_argument('--prompt', '-p', default='>>> ',
+    help='input prompt')
+parser.add_argument('--resultprefix', '-r', default=None,
+    help='execution result prefix, include {} for execution count number')
+parser.add_argument('--showassign', '-a', default=False,
+    help='display the result of assignments')
 args = parser.parse_args()
 
 if args.backend is not None:
     print(f"Using matplotlub backend {args.backend}")
     plt.use(args.backend)
 
-# load some models
+# load some robot models
 puma = models.DH.Puma560()
 panda = models.DH.Panda()
 
@@ -68,12 +78,32 @@
 ## drop into IPython
 import IPython
 from traitlets.config import Config
+from IPython.terminal.prompts import ClassicPrompts
+from IPython.terminal.prompts import Prompts
+from pygments.token import Token
+
+class MyPrompt(Prompts):
+    def in_prompt_tokens(self, cli=None):
+        return [(Token.Prompt, args.prompt)]
+    def out_prompt_tokens(self, cli=None):
+        if args.resultprefix is None:
+            # traditional behaviour
+            return [
+                (Token.OutPrompt, 'Out['),
+                (Token.OutPromptNum, str(self.shell.execution_count)),
+                (Token.OutPrompt, ']: '),
+        ]
+        else:
+            return [(Token.Prompt, args.resultprefix.format(self.shell.execution_count))]
 
 # set configuration options, there are lots, see
 # https://ipython.readthedocs.io/en/stable/config/options/terminal.html
 c = Config()
-c.InteractiveShellEmbed.colors = "Linux"
-c.InteractiveShell.colors = 'Neutral'
-c.InteractiveShell.confirm_exit = False
+c.InteractiveShellEmbed.colors = args.color
+c.InteractiveShell.confirm_exit = args.confirmexit
+# c.InteractiveShell.prompts_class = ClassicPrompts
+c.InteractiveShell.prompts_class = MyPrompt
+if args.showassign:
+    c.InteractiveShell.ast_node_interactivity = 'last_expr_or_assign'
 
 IPython.embed(config=c)

examples/swifty.py

@@ -209,11 +209,16 @@ def rand_v():
 
 x = []
 y = []
+rands = 100
+v = np.zeros((rands, 6))
+
+for i in range(rands):
+    v[i, :] = rand_v()
 
 for i in range(10000000):
 
     q = rand_q()
-    v = rand_v()
+    # v = rand_v()
 
     J = r.jacob0(q)
     Jt = J[:3, :]
@@ -222,15 +227,16 @@ def rand_v():
 
     q_n = [10000, 0]
     q_m = [10000, 0]
+    q_mn = 0
 
     # cond = np.linalg.cond(J[:3, :])
     m = r.manipulability(J=J, axes='trans')
     # infn = np.linalg.norm(Jt, 2)
     psi = (np.cbrt(np.linalg.det(Jt @ np.transpose(Jt)))) / \
         (np.trace(Jt @ np.transpose(Jt)) / 3)
 
-    for j in range(1000):
-        qd = np.linalg.pinv(J) @ v
+    for j in range(rands):
+        qd = np.linalg.pinv(J) @ v[j, :]
 
         if np.max(qd) > q_m[1]:
             q_m[1] = np.max(qd)
@@ -243,12 +249,16 @@ def rand_v():
         elif np.linalg.norm(qd) < q_n[0]:
             q_n[0] = np.linalg.norm(qd)
 
+        q_mn += np.linalg.norm(qd)
+
+    q_mn /= rands
+
     # # ax.plot(m, np.log10(cond), 'o', color='black')
     # ax.plot(m, infn, 'o', color='black')
     # # ax.plot(1, 0.002, 'o', color='black')
 
-    x.append(psi)
-    y.append(np.log10(q_m[1]))
+    x.append(m)
+    y.append(np.log10(q_n[0]))
     # y.append(psi)
 
     # ax.plot(m, np.log10(q_m[1]), 'o', color='black')

examples/test.py

@@ -14,27 +14,22 @@
 env.launch()
 
 # Create a Panda robot object
-puma = rtb.models.Puma560()
+r = rtb.models.Frankie()
 
 # Set joint angles to ready configuration
-puma.q = puma.qr
+r.q = r.qr
 
 # Add the puma to the simulator
-env.add(puma)
+env.add(r)
+time.sleep(1)
 
-# Tep = puma.fkine() * sm.SE3.Tz(0.1)
+Tep = r.fkine(r.q) * sm.SE3.Tx(1.0) * sm.SE3.Ty(1.0)
 
-# arrived = False
+arrived = False
 
-# dt = 0.05
+dt = 0.05
 
-# while not arrived:
-
-#     start = time.time()
-#     v, arrived = rtb.p_servo(puma.fkine(), Tep, 0.1)
-#     puma.qd = np.linalg.pinv(puma.jacobe()) @ v
-#     env.step(50)
-#     stop = time.time()
-
-#     if stop - start < dt:
-#         time.sleep(dt - (stop - start))
+while not arrived:
+    v, arrived = rtb.p_servo(r.fkine(r.q), Tep, 0.1)
+    r.qd = np.linalg.pinv(r.jacobe(r.q)) @ v
+    env.step(dt)

roboticstoolbox/backends/PyPlot/PyPlot.py

@@ -125,7 +125,7 @@ def launch(self, name=None, limits=None):
             plt.ion()
             plt.show()
 
-        self.t = 0
+        self.sim_time = 0
 
         # # Set the signal handler and a 0.1 second plot updater
         # signal.signal(signal.SIGALRM, self._plot_handler)
@@ -165,9 +165,9 @@ def step(self, dt=0.05):
         self._set_axes_equal()
 
         # update time and display it on plot
-        if self.t > 0:
-            self.timer.set_text(f"t = {self.t:.2f}")
-        self.t += dt
+        if self.sim_time > 0:
+            self.timer.set_text(f"t = {self.sim_time:.2f}")
+        self.sim_time += dt
 
         # plt.ion()
 

roboticstoolbox/backends/PyPlot/RobotPlot.py

@@ -144,8 +144,12 @@ def draw(self):
         # Remove oldjoint z coordinates
         if self.jointaxes:
             j = 0
-            # for joint in self.joints:
-            #     joint.remove()
+
+            for joint in self.joints:
+                self.ax.collections.remove(joint)
+
+            del self.joints
+            self.joints = []
 
             # Plot joint z coordinates
             for i in range(len(self.robot.links)):

roboticstoolbox/backends/ROS/ROS.py

@@ -11,8 +11,22 @@
 import time
 import subprocess
 import os
-import rospy
-from std_msgs.msg import Float32MultiArray
+# import rospy
+# from std_msgs.msg import Float32MultiArray
+
+_ros = None
+rospy = None
+
+
+def _import_ros():     # pragma nocover
+    import importlib
+    global rospy
+    try:
+        ros = importlib.import_module('rospy')
+    except ImportError:
+        print(
+            '\nYou must have ROS installed')
+        raise
 
 
 class ROS(Connector):  # pragma nocover
@@ -112,14 +126,14 @@ def __init__(self, robot):
         self.robot = robot
         self.v = np.zeros(robot.n)
 
-        self.velocity_publisher = rospy.Publisher(
-            '/joint_velocity',
-            Float32MultiArray, queue_size=1)
+        # self.velocity_publisher = rospy.Publisher(
+        #     '/joint_velocity',
+        #     Float32MultiArray, queue_size=1)
 
         self.relay()
 
-    def relay(self):
+    # def relay(self):
 
-        while True:
-            data = Float32MultiArray(data=self.robot.q)
-            self.velocity_publisher.publish(data)
+    #     while True:
+    #         data = Float32MultiArray(data=self.robot.q)
+    #         self.velocity_publisher.publish(data)