Fixed q

jhavl · jhavl · commit f332027148b3 · 2021-02-01T16:40:04.000+10:00
diff --git a/examples/RRMC_swift.py b/examples/RRMC_swift.py
@@ -13,7 +13,7 @@
 panda = rp.models.Panda()
 panda.q = panda.qr
 
-Tep = panda.fkine() * sm.SE3.Tx(0.2) * sm.SE3.Ty(0.2) * sm.SE3.Tz(0.45)
+Tep = panda.fkine(panda.q) * sm.SE3.Tx(0.2) * sm.SE3.Ty(0.2) * sm.SE3.Tz(0.45)
 
 arrived = False
 env.add(panda)
@@ -22,8 +22,8 @@
 
 while not arrived:
 
-    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)
 
 # Uncomment to stop the browser tab from closing
diff --git a/examples/park.py b/examples/park.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)
 
     # Project the gradient of manipulability into the null-space of the
     # differential kinematics
diff --git a/examples/ros.py b/examples/ros.py
@@ -8,11 +8,18 @@
 import numpy as np
 import os
 
-# Launch the simulator Swift
-env = rtb.backends.ROS()
-env.launch(ros_master_uri='http://localhost:11311')
+env = rtb.backends.Swift()
+env.launch()
 
-os.system('rostopic list')
+r = rtb.models.UR5()
+env.add(r)
+env.hold()
+
+# # Launch the simulator Swift
+# env = rtb.backends.ROS()
+# env.launch(ros_master_uri='http://localhost:11311')
+
+# os.system('rostopic list')
 
 # # Create a Panda robot object
 # panda = rtb.models.Panda()
