Merge pull request #160 from IMRCLab/feature_sim_controller

Add physics-based simulation

crazyflie/config/server.yaml

@@ -2,7 +2,9 @@
   ros__parameters:
     # simulation related
     sim:
-      max_dt: 0.1              # artificially limit the step() function (set to 0 to disable)
-      backend: none
-      visualizations:
+      max_dt: 0 #0.1              # artificially limit the step() function (set to 0 to disable)
+      backend: np                 # see backend folder for a list 
+      visualizations:             # see visualization folder for a list
         - rviz
+        # - pdf
+      controller: mellinger # none, pid, mellinger

crazyflie_sim/crazyflie_sim/backend/none.py

@@ -1,23 +1,25 @@
+from __future__ import annotations
+
 from rclpy.node import Node
 from rosgraph_msgs.msg import Clock
 from rclpy.time import Time
+from ..sim_data_types import State, Action
 
 
 class Backend:
+    """Tracks the desired state perfectly (no physics simulation)"""
 
-    def __init__(self, node: Node):
+    def __init__(self, node: Node, names: list[str], states: list[State]):
         self.node = node
+        self.names = names
         self.clock_publisher = node.create_publisher(Clock, 'clock', 10)
         self.t = 0
         self.dt = 0.1
 
-    def init(self, names, positions):
-        self.names = names
-
     def time(self) -> float:
         return self.t
 
-    def step(self, setpoints):
+    def step(self, states_desired: list[State], actions: list[Action]) -> list[State]:
         # advance the time
         self.t += self.dt
 
@@ -26,3 +28,9 @@ def step(self, setpoints):
         clock_message.clock = Time(seconds=self.time()).to_msg()
         self.clock_publisher.publish(clock_message)
 
+        # pretend we were able to follow desired states perfectly
+        return states_desired
+
+    def shutdown(self):
+        pass
+

crazyflie_sim/crazyflie_sim/backend/np.py

@@ -0,0 +1,125 @@
+from __future__ import annotations
+
+from rclpy.node import Node
+from rosgraph_msgs.msg import Clock
+from rclpy.time import Time
+from ..sim_data_types import State, Action
+
+
+import numpy as np
+import rowan
+
+class Backend:
+    """Backend that uses newton-euler rigid-body dynamics implemented in numpy"""
+
+    def __init__(self, node: Node, names: list[str], states: list[State]):
+        self.node = node
+        self.names = names
+        self.clock_publisher = node.create_publisher(Clock, 'clock', 10)
+        self.t = 0
+        self.dt = 0.0005
+
+        self.uavs = []
+        for state in states:
+            uav = Quadrotor(state)
+            self.uavs.append(uav)
+
+    def time(self) -> float:
+        return self.t
+
+    def step(self, states_desired: list[State], actions: list[Action]) -> list[State]:
+        # advance the time
+        self.t += self.dt
+
+        next_states = []
+
+        for uav, action in zip(self.uavs, actions):
+            uav.step(action, self.dt)
+            next_states.append(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 Quadrotor:
+    """Basic rigid body quadrotor model (no drag) using numpy and rowan"""
+
+    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):
+
+        # convert RPM -> Force
+        def rpm_to_force(rpm):
+            # polyfit using Tobias' data
+            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
+        # mv = mg + R f_u 
+        vel_next = self.state.vel + (np.array([0,0,-self.g]) + rowan.rotate(self.state.quat,f_u) / self.mass) * 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
+        q_next = rowan.normalize(rowan.calculus.integrate(self.state.quat, self.state.omega, 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]

crazyflie_sim/crazyflie_sim/crazyflie_server.py

@@ -25,6 +25,7 @@
 # from .backend import *
 # from .backend.none import BackendNone
 from .crazyflie_sil import CrazyflieSIL, TrajectoryPolynomialPiece
+from .sim_data_types import State, Action
 
 
 class CrazyflieServer(Node):
@@ -46,31 +47,42 @@ def __init__(self):
             pass
         robot_data = self._ros_parameters["robots"]
 
+        # Parse robots
+        names = []
+        initial_states = []
+        for cfname in robot_data:
+            if robot_data[cfname]["enabled"]:
+                type_cf = robot_data[cfname]["type"]
+                # do not include virtual objects
+                connection = self._ros_parameters['robot_types'][type_cf].get("connection", "crazyflie")
+                if connection == "crazyflie":
+                    names.append(cfname)
+                    pos = robot_data[cfname]["initial_position"]
+                    initial_states.append(State(pos))
+
         # initialize backend by dynamically loading the module
         backend_name = self._ros_parameters["sim"]["backend"]
         module = importlib.import_module(".backend." + backend_name, package="crazyflie_sim")
         class_ = getattr(module, "Backend")
-        self.backend = class_(self)
+        self.backend = class_(self, names, initial_states)
 
         # initialize visualizations by dynamically loading the modules
         self.visualizations = []
         for vis_name in self._ros_parameters["sim"]["visualizations"]:
             module = importlib.import_module(".visualization." + vis_name, package="crazyflie_sim")
             class_ = getattr(module, "Visualization")
-            vis = class_(self)
+            vis = class_(self, names, initial_states)
             self.visualizations.append(vis)
 
-        # Create robots
-        for cfname in robot_data:
-            if robot_data[cfname]["enabled"]:
-                type_cf = robot_data[cfname]["type"]
-                # do not include virtual objects
-                connection = self._ros_parameters['robot_types'][type_cf].get("connection", "crazyflie")
-                if connection == "crazyflie":
-                    self.cfs[cfname] = CrazyflieSIL(
-                        cfname,
-                        robot_data[cfname]["initial_position"],
-                        self.backend.time)
+        controller_name = backend_name = self._ros_parameters["sim"]["controller"]
+
+        # create robot SIL objects
+        for name, initial_state in zip(names, initial_states):
+            self.cfs[name] = CrazyflieSIL(
+                name,
+                initial_state.pos,
+                controller_name,
+                self.backend.time)
 
         # Create services for the entire swarm and each individual crazyflie
         self.create_service(Empty, "all/emergency", self._emergency_callback)
@@ -116,22 +128,35 @@ def __init__(self):
                 "/cmd_full_state", partial(self._cmd_full_state_changed, name=name), 10
             )
 
-        # Initialize backend
-        self.backend.init([name for name, _ in self.cfs.items()], [cf.initialPosition for _, cf in self.cfs.items()])
-
-        # Initialize Visualizations
-        for vis in self.visualizations:
-            vis.init([name for name, _ in self.cfs.items()], [cf.initialPosition for _, cf in self.cfs.items()])
-
         # step as fast as possible
         max_dt = 0.0 if "max_dt" not in self._ros_parameters["sim"] else self._ros_parameters["sim"]["max_dt"]
         self.timer = self.create_timer(max_dt, self._timer_callback)
+        self.is_shutdown = False
+
+    def on_shutdown_callback(self):
+        if not self.is_shutdown:
+            self.backend.shutdown()
+            for visualization in self.visualizations:
+                visualization.shutdown()
+
+            self.is_shutdown = True
 
     def _timer_callback(self):
-        states = [cf.getSetpoint() for _, cf in self.cfs.items()]
-        self.backend.step(states)
+        # update setpoint
+        states_desired = [cf.getSetpoint() for _, cf in self.cfs.items()]
+
+        # execute the control loop
+        actions = [cf.executeController()  for _, cf in self.cfs.items()]
+
+        # execute the physics simulator
+        states_next = self.backend.step(states_desired, actions)
+
+        # update the resulting state
+        for state, (_, cf) in zip(states_next, self.cfs.items()):
+            cf.setState(state)
+
         for vis in self.visualizations:
-            vis.step(states)
+            vis.step(self.backend.time(), states_next, states_desired, actions)
 
     def _param_to_dict(self, param_ros):
         """
@@ -286,11 +311,15 @@ def main(args=None):
 
     rclpy.init(args=args)
     crazyflie_server = CrazyflieServer()
-
-    rclpy.spin(crazyflie_server)
-
-    crazyflie_server.destroy_node()
-    rclpy.shutdown()
+    rclpy.get_default_context().on_shutdown(crazyflie_server.on_shutdown_callback)
+
+    try:
+        rclpy.spin(crazyflie_server)
+    except KeyboardInterrupt:
+        crazyflie_server.on_shutdown_callback()
+    finally:
+        rclpy.try_shutdown()
+        crazyflie_server.destroy_node()
 
 
 if __name__ == "__main__":