fixed q

jhavl · jhavl · commit e56bb94b2955 · 2021-02-01T16:40:16.000+10:00
diff --git a/examples/RRMC.py b/examples/RRMC.py
@@ -14,7 +14,7 @@
 panda = rp.models.DH.Panda()
 panda.q = panda.qr
 
-Tep = panda.fkine() * sm.SE3.Tx(-0.2) * sm.SE3.Ty(0.2) * sm.SE3.Tz(0.2)
+Tep = panda.fkine(panda.q) * sm.SE3.Tx(-0.2) * sm.SE3.Ty(0.2) * sm.SE3.Tz(0.2)
 
 arrived = False
 env.add(panda)
@@ -24,8 +24,8 @@
 while not arrived:
 
     start = time.time()
-    v, arrived = rp.p_servo(panda.fkine(), Tep, 1)
-    panda.qd = np.linalg.pinv(panda.jacobe()) @ v
+    v, arrived = rp.p_servo(panda.fkine(panda.q), Tep, 1)
+    panda.qd = np.linalg.pinv(panda.jacobe(panda.q)) @ v
     env.step(dt)
     stop = time.time()
 
diff --git a/examples/swift_recording.py b/examples/swift_recording.py
@@ -50,7 +50,7 @@
 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
 
@@ -82,7 +82,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
