Recording in swift with example

jhavl · jhavl · commit 79235f15a45d · 2020-11-06T10:59:40.000+10:00
diff --git a/examples/swift_recording.py b/examples/swift_recording.py
@@ -0,0 +1,108 @@
+#!/usr/bin/env python
+"""
+@author Jesse Haviland
+"""
+
+import roboticstoolbox as rtb
+import spatialmath as sm
+import numpy as np
+import qpsolvers as qp
+
+"""
+This is an implementation of the controller from:
+J. Haviland and P. Corke, “A purely-reactive manipulability-maximising
+motion controller,” arXiv preprint arXiv:2002.11901,2020.
+"""
+
+# Launch the simulator Swift
+env = rtb.backend.Swift()
+env.launch()
+
+# Create a Panda robot object
+panda = rtb.models.Panda()
+
+# Set joint angles to ready configuration
+panda.q = panda.qr
+
+# Add the Panda to the simulator
+env.add(panda)
+
+# The timestep for the simulation, 25ms
+dt = 0.025
+
+# Start recording with a framerate of 1/dt and
+# call the video panda_swift_recording
+env.start_recording('panda_swift_recording', 1 / dt)
+
+# Number of joint in the panda which we are controlling
+n = 7
+
+# Set the desired end-effector pose
+Tep = panda.fkine() * sm.SE3(0.3, 0.2, 0.3)
+
+arrived = False
+
+while not arrived:
+
+    # The pose of the Panda's end-effector
+    Te = panda.fkine()
+
+    # Transform from the end-effector to desired pose
+    eTep = Te.inv() * Tep
+
+    # Spatial error
+    e = np.sum(np.abs(np.r_[eTep.t, eTep.rpy() * np.pi/180]))
+
+    # Calulate the required end-effector spatial velocity for the robot
+    # to approach the goal. Gain is set to 1.0
+    v, arrived = rtb.p_servo(Te, Tep, 1.0)
+
+    # Gain term (lambda) for control minimisation
+    Y = 0.01
+
+    # Quadratic component of objective function
+    Q = np.eye(n + 6)
+
+    # Joint velocity component of Q
+    Q[:n, :n] *= Y
+
+    # Slack component of Q
+    Q[n:, n:] = (1 / e) * np.eye(6)
+
+    # The equality contraints
+    Aeq = np.c_[panda.jacobe(), np.eye(6)]
+    beq = v.reshape((6,))
+
+    # The inequality constraints for joint limit avoidance
+    Ain = np.zeros((n + 6, n + 6))
+    bin = np.zeros(n + 6)
+
+    # The minimum angle (in radians) in which the joint is allowed to approach
+    # to its limit
+    ps = 0.05
+
+    # The influence angle (in radians) in which the velocity damper
+    # becomes active
+    pi = 0.9
+
+    # Form the joint limit velocity damper
+    Ain[:n, :n], bin[:n] = panda.joint_velocity_damper(ps, pi, n)
+
+    # Linear component of objective function: the manipulability Jacobian
+    c = np.r_[-panda.jacobm().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)]
+    ub = np.r_[panda.qdlim[:n], 10 * np.ones(6)]
+
+    # Solve for the joint velocities dq
+    qd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq, lb=lb, ub=ub)
+
+    # Apply the joint velocities to the Panda
+    panda.qd[:n] = qd[:n]
+
+    # Step the simulator by 50 ms
+    env.step(dt)
+
+# Stop recording and save the video (to the downloads folder)
+env.stop_recording()
diff --git a/roboticstoolbox/backend/Swift/Swift.py b/roboticstoolbox/backend/Swift/Swift.py
@@ -74,6 +74,8 @@ def __init__(self, realtime=True, display=True):
         self.realtime = realtime
         self.display = display
 
+        self.recording = False
+
         if self.display and sw is None:
             _import_swift()
 
@@ -96,11 +98,11 @@ def launch(self):
             sw.start_servers(self.outq, self.inq)
             self.last_time = time.time()
 
-    def step(self, dt=50):
+    def step(self, dt=0.05):
         """
         Update the graphical scene
 
-        :param dt: time step in milliseconds, defaults to 50
+        :param dt: time step in seconds, defaults to 0.05
         :type dt: int, optional
 
         ``env.step(args)`` triggers an update of the 3D scene in the Swift
@@ -133,11 +135,11 @@ def step(self, dt=50):
 
             # If realtime is set, delay progress if we are running too quickly
             if self.realtime:
-                time_taken = (time.time() - self.last_time) * 1000
+                time_taken = (time.time() - self.last_time)
                 diff = dt - time_taken
 
                 if diff > 0:
-                    time.sleep(diff / 1000)
+                    time.sleep(diff)
 
                 self.last_time = time.time()
 
@@ -251,6 +253,46 @@ def remove(self):
 
         super().remove()
 
+    def start_recording(self, file_name, framerate):
+        """
+        Start recording the canvas in the Swift simulator
+
+        :param file_name: The file name for which the video will be saved as
+        :type file_name: string
+        :param framerate: The framerate of the video - to be timed correctly,
+            this should equalt 1 / dt where dt is the time supplied to the
+            step function
+        :type framerate: float
+
+        ``env.start_recording(file_name)`` starts recording the simulation
+            scene and will save it as file_name once
+            ``env.start_recording(file_name)`` is called
+        """
+
+        if not self.recording:
+            self._send_socket('start_recording', [framerate, file_name])
+            self.recording = True
+        else:
+            raise ValueError(
+                "You are already recording, you can only record one video"
+                " at a time")
+
+    def stop_recording(self):
+        """
+        Start recording the canvas in the Swift simulator. This is optional
+        as the video will be automatically saved when the python script exits
+
+        ``env.stop_recording()`` stops the recording of the simulation, can
+            only be called after ``env.start_recording(file_name)``
+        """
+
+        if self.recording:
+            self._send_socket('stop_recording')
+        else:
+            raise ValueError(
+                "You must call swift.start_recording(file_name) before trying"
+                " to stop the recording")
+
     def _step_robots(self, dt):
 
         for robot in self.robots:
@@ -261,7 +303,7 @@ def _step_robots(self, dt):
             if robot.control_type == 'v':
 
                 for i in range(robot.n):
-                    robot.q[i] += robot.qd[i] * (dt / 1000)
+                    robot.q[i] += robot.qd[i] * (dt)
 
                     if np.any(robot.qlim[:, i] != 0) and \
                             not np.any(np.isnan(robot.qlim[:, i])):
@@ -285,8 +327,8 @@ def _step_shapes(self, dt):
             t = T.t
             r = T.rpy('rad')
 
-            t += shape.v[:3] * (dt / 1000)
-            r += shape.v[3:] * (dt / 1000)
+            t += shape.v[:3] * (dt)
+            r += shape.v[3:] * (dt)
 
             shape.base = sm.SE3(t) * sm.SE3.RPY(r)
 
@@ -298,7 +340,7 @@ def _draw_all(self):
         for i in range(len(self.shapes)):
             self._send_socket('shape_poses', [i, self.shapes[i].fk_dict()])
 
-    def _send_socket(self, code, data):
+    def _send_socket(self, code, data=None):
         msg = [code, data]
 
         self.outq.put(msg)
