Make example work again

Author: jhavl

roboticstoolbox/examples/branched_robot.py

@@ -4,15 +4,15 @@
 """
 
 import swift
-import roboticstoolbox as rp
+import roboticstoolbox as rtb
 import spatialmath as sm
 import spatialgeometry as sg
 import numpy as np
 
 env = swift.Swift()
 env.launch(realtime=True)
 
-r = rp.models.YuMi()
+r = rtb.models.YuMi()
 env.add(r)
 
 lTep = (
@@ -49,14 +49,10 @@
 env.add(l_frame)
 env.add(r_frame)
 
-l_path, l_n, _ = r.get_path(end=r.grippers[0])
-r_path, r_n, _ = r.get_path(end=r.grippers[1])
 
-# Inner list comprehension gets a list jindicies from the links in l_path
-# Outer list comprehension removes None's from the list (a None kindex means
-# the link is static)
-l_jindex = [i for i in [link.jindex for link in l_path] if i]
-r_jindex = [i for i in [link.jindex for link in r_path] if i is not None]
+# Construct an ETS for the left and right arms
+la = r.ets(end=r.grippers[0])
+ra = r.ets(end=r.grippers[1])
 
 arrivedl = False
 arrivedr = False
@@ -67,18 +63,14 @@
 
 while not arrivedl or not arrivedr:
 
-    vl, arrivedl = rp.p_servo(
-        r.fkine(r.q, end=r.grippers[0]), lTep, gain=gain, threshold=0.001
-    )
-    vr, arrivedr = rp.p_servo(
-        r.fkine(r.q, end=r.grippers[1]), rTep, gain=gain, threshold=0.001
-    )
+    vl, arrivedl = rtb.p_servo(la.fkine(r.q), lTep, gain=gain, threshold=0.001)
+    vr, arrivedr = rtb.p_servo(ra.fkine(r.q), rTep, gain=gain, threshold=0.001)
 
-    r.qd[l_jindex] = np.linalg.pinv(r.jacob0(r.q, end=r.grippers[0])) @ vl
-    r.qd[r_jindex] = np.linalg.pinv(r.jacob0(r.q, end=r.grippers[1])) @ vr
+    r.qd[la.jindices] = np.linalg.pinv(la.jacobe(r.q)) @ vl
+    r.qd[ra.jindices] = np.linalg.pinv(ra.jacobe(r.q)) @ vr
 
-    l_frame.base = r.fkine(r.q, end=r.grippers[0])
-    r_frame.base = r.fkine(r.q, end=r.grippers[1])
+    l_frame.T = la.fkine(r.q)
+    r_frame.T = ra.fkine(r.q)
 
     env.step(dt)