swift simulation V2

Mordween · Mordween · commit 786753fe3f32 · 2024-04-08T12:41:30.000+02:00
diff --git a/tests/SwiftTest.py b/tests/SwiftTest.py
@@ -0,0 +1,60 @@
+import swift
+import roboticstoolbox as rtb
+import spatialmath as sm 
+import numpy as np 
+import time
+
+
+class RobotControl():
+    def __init__(self, dt, env, robot):
+        self.dt = dt
+        self.env = env
+        self.robot = robot
+        self.env.add(self.robot)
+
+
+    def move(self, positionX, positionY, positionZ, numberOfSteps):
+        robotJointsStates = np.zeros(6) 
+        for joint in range(5):
+            robotJointsStates[joint] = self.robot.q[joint]
+
+        Tep = sm.SE3.Trans(positionX, positionY, positionZ) * sm.SE3.OA([1, 0,1], [0, 0, -1])
+        sol = self.robot.ik_LM(Tep)         # solve IK
+
+        qt = rtb.jtraj(robotJointsStates, sol[0], numberOfSteps)
+        #print(qt.q[numberOfSteps-1])
+        for steps in range(numberOfSteps):
+            self.robot.q = qt.q[steps]
+            self.env.step(self.dt)
+            time.sleep(self.dt)
+            
+    
+    def pause(self, second):
+        for i in range(int(second/(self.dt))):
+            self.env.step(self.dt)
+            time.sleep(self.dt)
+
+
+if __name__ == "__main__":  # pragma nocover
+
+    env = swift.Swift()
+    env.launch(realtime = True)
+
+    lite = rtb.models.Lite6()
+    lite.q = lite.qr
+
+    rob = RobotControl(0.1, env, lite)
+
+    positionX = [0.15, 0.15, 0.35]
+    positionY = [-0.2, -0.2, 0.2]
+    positionZ = [0.05, 0.4, 0.2]
+    step = [50, 50, 50]
+
+    for position in range(len(step)):
+        rob.move(positionX[position], positionY[position], positionZ[position], step[position])
+        rob.pause(1)
+        
+    rob.pause(10)
+
+
+
