Merge branch 'multi_drone'

Author: ucsdsaslab

.gitmodules

@@ -1,3 +1,6 @@
 [submodule "crazyflie/deps/crazyflie_tools"]
 	path = crazyflie/deps/crazyflie_tools
 	url = https://github.com/whoenig/crazyflie_tools.git
+[submodule "third_party/CrazySim"]
+	path = third_party/CrazySim
+	url = https://github.com/gtfactslab/CrazySim.git

crazyflie/config/crazyflies.yaml

@@ -2,25 +2,41 @@
 fileversion: 3
 
 robots:
-  cf231:
+  cf_1:
+      enabled: true
+      uri: udp://0.0.0.0:19850
+      initial_position: [0.0, 0.0, 0.0]
+      type: cf_sim
+
+  cf_2:
     enabled: true
-    uri: radio://0/80/2M/E7E7E7E7E1
-    initial_position: [-4.5, 0.0, 0.]
-    # initial_position: [0., 0.3, 0.0]
-    type: cf21_single_PID  # see robot_types
-    # firmware_params:
-    #   kalman:
-    #     pNAcc_xy: 1.0 # default 0.5
-    # firmware_logging:
-    #   enabled: true
-    #   custom_topics:
-    #     topic_name3: 
-    #       frequency: 1
-    #       vars: ["acc.x", "acc.y"]
-    # reference_frame: "odom_cf231" # use a custom reference frame if no global coordinate system is available
+    uri: udp://0.0.0.0:19851
+    initial_position: [1.0, 4.0, 0.0]
+    type: cf_sim
+
+  # cf_3:
+  #   enabled: false
+  #   uri: udp://0.0.0.0:19852
+  #   initial_position: [0.0, 1.0, 0.0]
+  #   type: cf_sim 
+
+  # cf_4:
+  #   enabled: false
+  #   uri: udp://0.0.0.0:19853
+  #   initial_position: [1.0, 0.0, 0.0]
+  #   type: cf_sim
 
 # Definition of the various robot types
 robot_types:
+  cf_sim:
+    motion_capture:
+      enabled: false
+    big_quad: false
+    firmware_logging:
+      enabled: true
+      default_topics:
+        pose:
+          frequency: 10
   cf21_single:
     motion_capture:
       tracking: "librigidbodytracker"  # one of "vendor", "librigidbodytracker"

crazyflie/config/server.yaml

@@ -14,8 +14,10 @@
       query_all_values_on_connect: False
     # simulation related
     sim:
-      max_dt: 0.001 #0.1              # artificially limit the step() function (set to 0 to disable)
-      backend: np                 # see backend folder for a list 
+      max_dt: 0.001               # artificially limit the step() function (set to 0 to disable) TODO: Rephrase
+      backend: np                 # see backend folder for a list (dynobench, neuralswarm, none, np, pinocchio, custom)
+      backend_dt: 0.0005           # time step of the backend (~ backend_dt / max_dt dictates sim-to-real time ratio)
+      backend_time_for_stamp: true # if true, the backend will use the time from the stamp, otherwise rclpy get time now
       visualizations:             # see visualization folder for a list
         rviz:
           enabled: true
@@ -40,4 +42,4 @@
               calibration:
                 tvec: [0,0,0]
                 rvec: [ 0.61394313, -0.61394313,  1.48218982]   # 45 deg tilt
-      controller: pid # none, pid, mellinger
+      controller: pid # none, pid, mellinger, custom

crazyflie_sim/README.md

@@ -0,0 +1,7 @@
+## Simulation backends for crazyswarm2
+
+The following simulation backends are compatible with SASLab crazyswarm2
+- pinnochio: https://stack-of-tasks.github.io/pinocchio/download.html and pip3 install pinocchio
+- dynobench: pip3 install dynobench
+- np: Pure numpy based implementation
+- None: Next state is desired state (high level quick check of how things work)

crazyflie_sim/crazyflie_sim/backend/custom.py

@@ -0,0 +1,188 @@
+from __future__ import annotations
+
+import numpy as np
+from rclpy.node import Node
+from rclpy.time import Time
+from rosgraph_msgs.msg import Clock
+import rowan
+
+from ..sim_data_types import Action, State
+
+
+class Backend:
+    """Backend that uses a SASlab custom dynamics model."""
+
+    def __init__(self, node: Node, names: list[str], states: list[State], dt: float):
+        self.node = node
+        self.names = names
+        self.clock_publisher = node.create_publisher(Clock, "clock", 10)
+        self.t = 0
+        self.dt = dt
+
+        self.uavs = []
+        self.uavs_takeoff = []
+        for state in states:
+            uav = Quadrotor(state)
+            self.uavs.append(uav)
+            uav_backup = QuadrotorNumpy(state)
+            self.uavs_takeoff.append(uav_backup)
+
+    def time(self) -> float:
+        return self.t
+
+    def step(
+        self,
+        states_desired: list[State],
+        actions: list[Action],
+        disturbances: list[State],
+    ) -> list[State]:
+        # advance the time
+        self.t += self.dt
+        next_states = []
+        for i, (uav, action, disturbance) in enumerate(zip(self.uavs, actions, disturbances)):
+            backup_uav = self.uavs_takeoff[i]
+            if isinstance(action, np.ndarray):
+                uav.step(action, self.dt, disturbance=disturbance)
+                next_states.append(uav.state)
+                backup_uav.state = uav.state
+            else:
+                backup_uav.step(action, self.dt, disturbance=disturbance)
+                next_states.append(backup_uav.state)
+                uav.state = backup_uav.state
+
+        # print(states_desired, actions, next_states)
+        # publish the current clock
+        clock_message = Clock()
+        clock_message.clock = Time(seconds=self.time()).to_msg()
+        self.clock_publisher.publish(clock_message)
+
+        return next_states
+
+    def shutdown(self):
+        pass
+
+
+class QuadrotorNumpy:
+    """Basic rigid body quadrotor model (no drag) using numpy and rowan.
+    Assumes control is forces."""
+
+    def __init__(self, state):
+        # parameters (Crazyflie 2.0 quadrotor)
+        self.mass = 0.034  # kg
+        # self.J = np.array([
+        # 	[16.56,0.83,0.71],
+        # 	[0.83,16.66,1.8],
+        # 	[0.72,1.8,29.26]
+        # 	]) * 1e-6  # kg m^2
+        self.J = np.array([16.571710e-6, 16.655602e-6, 29.261652e-6])
+
+        # Note: we assume here that our control is forces
+        arm_length = 0.046  # m
+        arm = 0.707106781 * arm_length
+        t2t = 0.006  # thrust-to-torque ratio
+        self.B0 = np.array([
+            [1, 1, 1, 1],
+            [-arm, -arm, arm, arm],
+            [-arm, arm, arm, -arm],
+            [-t2t, t2t, -t2t, t2t]
+            ])
+        self.g = 9.81  # not signed
+
+        if self.J.shape == (3, 3):
+            self.inv_J = np.linalg.pinv(self.J)  # full matrix -> pseudo inverse
+        else:
+            self.inv_J = 1 / self.J  # diagonal matrix -> division
+
+        self.state = state
+
+    def step(self, action, dt, disturbance, f_a=np.zeros(3)):
+
+        # convert RPM -> Force
+        def rpm_to_force(rpm):
+            # polyfit using data and scripts from https://github.com/IMRCLab/crazyflie-system-id
+            p = [2.55077341e-08, -4.92422570e-05, -1.51910248e-01]
+            force_in_grams = np.polyval(p, rpm)
+            force_in_newton = force_in_grams * 9.81 / 1000.0
+            return np.maximum(force_in_newton, 0)
+
+        force = rpm_to_force(action.rpm)
+
+        # compute next state
+        eta = np.dot(self.B0, force)
+        f_u = np.array([0, 0, eta[0]])
+        tau_u = np.array([eta[1], eta[2], eta[3]])
+        # dynamics
+        # dot{p} = v
+        pos_next = self.state.pos + self.state.vel * dt + disturbance.pos * dt
+        # mv = mg + R f_u + f_a
+        vel_next = self.state.vel + (
+            np.array([0, 0, -self.g]) +
+            (rowan.rotate(self.state.quat, f_u) + f_a) / self.mass) * dt + (
+            disturbance.vel * dt
+            )
+
+        # dot{R} = R S(w)
+        # to integrate the dynamics, see
+        # https://www.ashwinnarayan.com/post/how-to-integrate-quaternions/, and
+        # https://arxiv.org/pdf/1604.08139.pdf
+        # Sec 4.5, https://arxiv.org/pdf/1711.02508.pdf
+        omega_global = rowan.rotate(self.state.quat, self.state.omega)
+        q_next = rowan.normalize(
+            rowan.calculus.integrate(
+                self.state.quat, omega_global, dt))
+
+        # mJ = Jw x w + tau_u
+        omega_next = self.state.omega + (
+            self.inv_J * (np.cross(self.J * self.state.omega, self.state.omega) + tau_u)) * dt
+
+        self.state.pos = pos_next
+        self.state.vel = vel_next
+        self.state.quat = q_next
+        self.state.omega = omega_next
+
+        # if we fall below the ground, set velocities to 0
+        if self.state.pos[2] < 0:
+            self.state.pos[2] = 0
+            self.state.vel = [0, 0, 0]
+            self.state.omega = [0, 0, 0]
+
+
+class Quadrotor:
+    """Basic quadrotor model assuming control is attitude + thrust"""
+
+    def __init__(self, state):
+        self.g = 9.81  # not signed
+        self.state = state
+        self.internal_state = np.zeros(7)
+        self.i = 0
+
+    def step(self, action, dt, disturbance, f_a=np.zeros(3)):
+        # dot{p} = v
+        if isinstance(action, np.ndarray):
+            pos_next = self.state.pos + self.state.vel * dt + disturbance.pos * dt
+            # mv = mg + R f_u + f_a
+            vel_next = (
+                self.state.vel
+                + dt
+                * np.array([self.g * action[1], -self.g * action[0], action[3] - self.g])
+                + dt * disturbance.vel
+            )
+            ypr = rowan.to_euler(self.state.quat)
+            yaw_next = ypr[0] + action[2] * dt
+            eulerangles = np.array([yaw_next, action[1], action[0]])
+            q_next = rowan.from_euler(*eulerangles)
+            # omega_next = np.array([0, 0, action[2]])
+            omega_next = np.zeros(3)  # TODO: check if sufficient
+
+            self.state.pos = pos_next
+            self.state.vel = vel_next
+            self.state.quat = q_next
+            self.state.omega = omega_next
+
+            # if we fall below the ground, set velocities to 0
+            if self.state.pos[2] < 0:
+                self.state.pos[2] = 0
+                self.state.vel = [0, 0, 0]
+                self.state.omega = [0, 0, 0]
+        else:
+            self.state = self.state

crazyflie_sim/crazyflie_sim/backend/dynobench.py

@@ -3,7 +3,7 @@
 import numpy as np
 from rclpy.node import Node
 from rclpy.time import Time
-import robot_python
+import dynobench
 from rosgraph_msgs.msg import Clock
 
 # import sys
@@ -15,12 +15,12 @@
 class Backend:
     """Backend that uses newton-euler rigid-body dynamics implemented in numpy."""
 
-    def __init__(self, node: Node, names: list[str], states: list[State]):
+    def __init__(self, node: Node, names: list[str], states: list[State], dt: float):
         self.node = node
         self.names = names
         self.clock_publisher = node.create_publisher(Clock, 'clock', 10)
         self.t = 0
-        self.dt = 0.0005
+        self.dt = dt
 
         self.uavs = []
         for state in states:
@@ -30,14 +30,14 @@ def __init__(self, node: Node, names: list[str], states: list[State]):
     def time(self) -> float:
         return self.t
 
-    def step(self, states_desired: list[State], actions: list[Action]) -> list[State]:
+    def step(self, states_desired: list[State], actions: list[Action], disturbances: list[State]) -> list[State]:
         # advance the time
         self.t += self.dt
 
         next_states = []
 
-        for uav, action in zip(self.uavs, actions):
-            uav.step(action, self.dt)
+        for uav, action, disturbance in zip(self.uavs, actions, disturbances):
+            uav.step(action, disturbance, self.dt)
             next_states.append(uav.state)
 
         # print(states_desired, actions, next_states)
@@ -84,11 +84,11 @@ class Quadrotor:
     """Basic rigid body quadrotor model (no drag) using numpy and rowan."""
 
     def __init__(self, state):
-        self.uav = robot_python.robot_factory(
+        self.uav = dynobench.robot_factory(
             str((Path(__file__).parent / 'data/dynobench/crazyflie2.yaml').resolve()), [], [])
         self.state = state
 
-    def step(self, action, dt):
+    def step(self, action, disturbance, dt):
 
         # m: 0.034
         # max_f: 1.3
@@ -97,6 +97,8 @@ def step(self, action, dt):
 
         xnext = np.zeros(13)
         self.uav.step(xnext, sim_state2dynobench_state(self.state), normalized_force, dt)
+        xnext[0:3] += disturbance.pos * dt
+        xnext[7:10] += disturbance.vel * dt
         self.state = dynobench_state2sim_state(xnext)
 
         # if we fall below the ground, set velocities to 0

crazyflie_sim/crazyflie_sim/backend/neuralswarm.py

@@ -103,12 +103,12 @@ def compute_Fa(self, data_self, data_neighbors):
 class Backend:
     """Backend that is based on the one defined in np.py."""
 
-    def __init__(self, node: Node, names: list[str], states: list[State]):
+    def __init__(self, node: Node, names: list[str], states: list[State], dt: float):
         self.node = node
         self.names = names
         self.clock_publisher = node.create_publisher(Clock, 'clock', 10)
         self.t = 0
-        self.dt = 0.0005
+        self.dt = dt
 
         self.uavs = []
         for state in states:

crazyflie_sim/crazyflie_sim/backend/none.py

@@ -10,17 +10,17 @@
 class Backend:
     """Tracks the desired state perfectly (no physics simulation)."""
 
-    def __init__(self, node: Node, names: list[str], states: list[State]):
+    def __init__(self, node: Node, names: list[str], states: list[State], dt: float):
         self.node = node
         self.names = names
         self.clock_publisher = node.create_publisher(Clock, 'clock', 10)
         self.t = 0
-        self.dt = 0.1
+        self.dt = dt
 
     def time(self) -> float:
         return self.t
 
-    def step(self, states_desired: list[State], actions: list[Action]) -> list[State]:
+    def step(self, states_desired: list[State], actions: list[Action], disturbances: list[State]) -> list[State]:
         # advance the time
         self.t += self.dt