moved quadcopter controller to examples, updated README

examples/drone/README.md

@@ -71,6 +71,17 @@ Run with:
 python hover_eval.py -e drone-hovering --ckpt 500 --record
 ```
 
+### 4. Quadcopter Controller (`quadcopter_controller.py` & `fly_route.py`)
+A PID-based controller to provide stable point-to-point flight without controlling each rotor. Parameters
+have been tuned for controlled flight but not optimized. Stored in `fly_route.py`. Run `fly_route.py` to
+test.
+
+Run with:
+
+```bash
+python fly_route.py
+```
+
 ## Technical Details
 
 - The drone model used is the Crazyflie 2.X (`urdf/drones/cf2x.urdf`)

examples/drone/fly_route.py

@@ -1,18 +1,22 @@
 import genesis as gs
 import math
-from genesis.engine.entities.drone_entity import DroneEntity, DronePIDController
+from quadcopter_controller import DronePIDController
+from genesis.engine.entities.drone_entity import DroneEntity
 from genesis.vis.camera import Camera
 
 base_rpm = 14468.429183500699
 min_rpm = 0.9 * base_rpm
 max_rpm = 1.5 * base_rpm
 
+
 def hover(drone: DroneEntity):
     drone.set_propellels_rpm([base_rpm, base_rpm, base_rpm, base_rpm])
 
+
 def clamp(rpm):
     return max(min_rpm, min(int(rpm), max_rpm))
-
+
+
 def fly_to_point(target, controller: DronePIDController, scene: gs.Scene, cam: Camera):
     drone = controller.drone
     step = 0
@@ -21,10 +25,13 @@ def fly_to_point(target, controller: DronePIDController, scene: gs.Scene, cam: C
     z = target[2] - drone.get_pos()[2]
 
     distance = math.sqrt(x**2 + y**2 + z**2)
-    
-    while (distance > 0.1 and step < 1000):
+
+    while distance > 0.1 and step < 1000:
         [M1, M2, M3, M4] = controller.update(target)
-        M1=clamp(M1); M2=clamp(M2); M3=clamp(M3); M4=clamp(M4)
+        M1 = clamp(M1)
+        M2 = clamp(M2)
+        M3 = clamp(M3)
+        M4 = clamp(M4)
         drone.set_propellels_rpm([M1, M2, M3, M4])
         scene.step()
         cam.render()
@@ -34,82 +41,56 @@ def fly_to_point(target, controller: DronePIDController, scene: gs.Scene, cam: C
         x = drone_pos[0]
         y = drone_pos[1]
         z = drone_pos[2]
-        cam.set_pose(lookat=(x,y,z))
+        cam.set_pose(lookat=(x, y, z))
         x = target[0] - x
         y = target[0] - y
         z = target[0] - z
         distance = math.sqrt(x**2 + y**2 + z**2)
         step += 1
 
+
 def main():
     gs.init(backend=gs.gpu)
-
-    ##### scene #####
-    scene = gs.Scene(
-        show_viewer=False,
-        sim_options=gs.options.SimOptions(
-            dt=0.01
-        )
 
-    )
+    ##### scene #####
+    scene = gs.Scene(show_viewer=False, sim_options=gs.options.SimOptions(dt=0.01))
 
     ##### entities #####
-    plane = scene.add_entity(
-        morph=gs.morphs.Plane()
-    )
-
-    drone =  scene.add_entity(
-        morph = gs.morphs.Drone(
-            file="urdf/drones/cf2x.urdf",
-            pos=(0,0,0.2)
-        )
-    )
-
-    # parameters are tuned such that the 
+    plane = scene.add_entity(morph=gs.morphs.Plane())
+
+    drone = scene.add_entity(morph=gs.morphs.Drone(file="urdf/drones/cf2x.urdf", pos=(0, 0, 0.2)))
+
+    # parameters are tuned such that the
     # drone can fly, not optimized
     pid_params = [
-        [2.,0.,0.],
-        [2.,0.,0.],
-        [2.,0.,0.],
-        [20.,0.,20.],
-        [20.,0.,20.],
-        [25.,0.,20.],
-        [10.,0.,1.],
-        [10.,0.,1.],
-        [2.,0.,0.2],
+        [2.0, 0.0, 0.0],
+        [2.0, 0.0, 0.0],
+        [2.0, 0.0, 0.0],
+        [20.0, 0.0, 20.0],
+        [20.0, 0.0, 20.0],
+        [25.0, 0.0, 20.0],
+        [10.0, 0.0, 1.0],
+        [10.0, 0.0, 1.0],
+        [2.0, 0.0, 0.2],
     ]
 
-    controller = DronePIDController(
-        drone=drone, 
-        dt=0.01,
-        base_rpm=base_rpm,
-        pid_params=pid_params
-        )
-
-    cam = scene.add_camera(
-        pos=(1,1,1),
-        lookat=drone.morph.pos,
-        GUI=False,
-        res = (640,480),
-        fov=30
-    )
+    controller = DronePIDController(drone=drone, dt=0.01, base_rpm=base_rpm, pid_params=pid_params)
+
+    cam = scene.add_camera(pos=(1, 1, 1), lookat=drone.morph.pos, GUI=False, res=(640, 480), fov=30)
 
     ##### build #####
 
     scene.build()
 
     cam.start_recording()
 
-    points = [
-        (1,1,2),
-        (-1,2,1),
-        (0,0,0.5)
-    ]
+    points = [(1, 1, 2), (-1, 2, 1), (0, 0, 0.5)]
 
     for point in points:
         fly_to_point(point, controller, scene, cam)
 
     cam.stop_recording(save_to_filename="../../videos/fly_route.mp4")
 
+
 if __name__ == "__main__":
-    main()
+    main()

examples/drone/quadcopter_controller.py

@@ -0,0 +1,80 @@
+import torch
+import numpy as np
+from genesis.engine.entities.drone_entity import DroneEntity
+from genesis.utils.geom import quat_to_xyz
+from genesis.utils.pid import PIDController
+
+
+class DronePIDController:
+    def __init__(self, drone: DroneEntity, dt, base_rpm, pid_params):
+        self.__pid_pos_x = PIDController(kp=pid_params[0][0], ki=pid_params[0][1], kd=pid_params[0][2])
+        self.__pid_pos_y = PIDController(kp=pid_params[1][0], ki=pid_params[1][1], kd=pid_params[1][2])
+        self.__pid_pos_z = PIDController(kp=pid_params[2][0], ki=pid_params[2][1], kd=pid_params[2][2])
+
+        self.__pid_vel_x = PIDController(kp=pid_params[3][0], ki=pid_params[3][1], kd=pid_params[3][2])
+        self.__pid_vel_y = PIDController(kp=pid_params[4][0], ki=pid_params[4][1], kd=pid_params[4][2])
+        self.__pid_vel_z = PIDController(kp=pid_params[5][0], ki=pid_params[5][1], kd=pid_params[5][2])
+
+        self.__pid_att_roll = PIDController(kp=pid_params[6][0], ki=pid_params[6][1], kd=pid_params[6][2])
+        self.__pid_att_pitch = PIDController(kp=pid_params[7][0], ki=pid_params[7][1], kd=pid_params[7][2])
+        self.__pid_att_yaw = PIDController(kp=pid_params[8][0], ki=pid_params[8][1], kd=pid_params[8][2])
+
+        self.drone = drone
+        self.__dt = dt
+        self.__base_rpm = base_rpm
+
+    def __get_drone_pos(self) -> torch.Tensor:
+        return self.drone.get_pos()
+
+    def __get_drone_vel(self) -> torch.Tensor:
+        return self.drone.get_vel()
+
+    def __get_drone_att(self) -> torch.Tensor:
+        quat = self.drone.get_quat()
+        # print(quat_to_xyz(quat))
+        return quat_to_xyz(quat)
+
+    def __mixer(self, thrust, roll, pitch, yaw, x_vel, y_vel) -> torch.Tensor:
+        M1 = self.__base_rpm + (thrust - roll - pitch - yaw - x_vel + y_vel)
+        M2 = self.__base_rpm + (thrust - roll + pitch + yaw + x_vel + y_vel)
+        M3 = self.__base_rpm + (thrust + roll + pitch - yaw + x_vel - y_vel)
+        M4 = self.__base_rpm + (thrust + roll - pitch + yaw - x_vel - y_vel)
+        # print("pitch =", pitch)
+        # print("roll =", roll)
+
+        return torch.Tensor([M1, M2, M3, M4])
+
+    def update(self, target) -> np.ndarray:
+        curr_pos = self.__get_drone_pos()
+        curr_vel = self.__get_drone_vel()
+        curr_att = self.__get_drone_att()
+
+        err_pos_x = target[0] - curr_pos[0]
+        err_pos_y = target[1] - curr_pos[1]
+        err_pos_z = target[2] - curr_pos[2]
+
+        vel_des_x = self.__pid_pos_x.update(err_pos_x, self.__dt)
+        vel_des_y = self.__pid_pos_y.update(err_pos_y, self.__dt)
+        vel_des_z = self.__pid_pos_z.update(err_pos_z, self.__dt)
+
+        error_vel_x = vel_des_x - curr_vel[0]
+        error_vel_y = vel_des_y - curr_vel[1]
+        error_vel_z = vel_des_z - curr_vel[2]
+
+        x_vel_del = self.__pid_vel_x.update(error_vel_x, self.__dt)
+        y_vel_del = self.__pid_vel_y.update(error_vel_y, self.__dt)
+        thrust_des = self.__pid_vel_z.update(error_vel_z, self.__dt)
+
+        err_roll = 0.0 - curr_att[0]
+        err_pitch = 0.0 - curr_att[1]
+        err_yaw = 0.0 - curr_att[2]
+
+        roll_del = self.__pid_att_roll.update(err_roll, self.__dt)
+        pitch_del = self.__pid_att_pitch.update(err_pitch, self.__dt)
+        yaw_del = self.__pid_att_yaw.update(err_yaw, self.__dt)
+
+        prop_rpms = self.__mixer(thrust_des, roll_del, pitch_del, yaw_del, x_vel_del, y_vel_del)
+        prop_rpms = prop_rpms.cpu()
+        prop_rpms - prop_rpms.numpy()
+
+        return prop_rpms

genesis/engine/entities/drone_entity.py

@@ -3,19 +3,15 @@
 
 import numpy as np
 import taichi as ti
-import torch
 
 import genesis as gs
 import genesis.utils.misc as mu
-from genesis.utils.geom import quat_to_xyz
-from genesis.utils.pid import PIDController
 
 from .rigid_entity import RigidEntity
 
 
 @ti.data_oriented
 class DroneEntity(RigidEntity):
-
     def _load_URDF(self, morph, surface):
         super()._load_URDF(morph, surface)
 
@@ -101,96 +97,3 @@ def propellers_idx(self):
     @property
     def propellers_spin(self):
         return self._propellers_spin
-
-class DronePIDController():
-    def __init__(self, drone: DroneEntity, dt, base_rpm, pid_params):
-        self.__pid_pos_x = PIDController(
-            kp=pid_params[0][0], 
-            ki=pid_params[0][1], 
-            kd=pid_params[0][2])
-        self.__pid_pos_y = PIDController(kp=pid_params[1][0],
-             ki=pid_params[1][1], 
-             kd=pid_params[1][2])
-        self.__pid_pos_z = PIDController(kp=pid_params[2][0],
-             ki=pid_params[2][1], 
-             kd=pid_params[2][2])
-
-        self.__pid_vel_x = PIDController(kp=pid_params[3][0],
-             ki=pid_params[3][1], 
-             kd=pid_params[3][2])
-        self.__pid_vel_y = PIDController(kp=pid_params[4][0],
-             ki=pid_params[4][1], 
-             kd=pid_params[4][2])
-        self.__pid_vel_z = PIDController(kp=pid_params[5][0],
-             ki=pid_params[5][1], 
-             kd=pid_params[5][2])
-
-        self.__pid_att_roll  = PIDController(kp=pid_params[6][0],
-             ki=pid_params[6][1], 
-             kd=pid_params[6][2])
-        self.__pid_att_pitch = PIDController(kp=pid_params[7][0],
-             ki=pid_params[7][1], 
-             kd=pid_params[7][2])
-        self.__pid_att_yaw   = PIDController(kp=pid_params[8][0],
-             ki=pid_params[8][1], 
-             kd=pid_params[8][2])
-
-        self.drone = drone
-        self.__dt = dt
-        self.__base_rpm = base_rpm
-
-    def __get_drone_pos(self) -> torch.Tensor:
-        return self.drone.get_pos()
-
-    def __get_drone_vel(self) -> torch.Tensor:
-        return self.drone.get_vel()
-
-    def __get_drone_att(self) -> torch.Tensor:
-        quat = self.drone.get_quat()
-        # print(quat_to_xyz(quat))
-        return quat_to_xyz(quat)
-
-    def __mixer(self, thrust, roll, pitch, yaw, x_vel, y_vel) -> torch.Tensor:
-        M1 = self.__base_rpm + (thrust - roll - pitch - yaw - x_vel + y_vel)
-        M2 = self.__base_rpm + (thrust - roll + pitch + yaw + x_vel + y_vel)
-        M3 = self.__base_rpm + (thrust + roll + pitch - yaw + x_vel - y_vel)
-        M4 = self.__base_rpm + (thrust + roll - pitch + yaw - x_vel - y_vel)
-        # print("pitch =", pitch)
-        # print("roll =", roll)
-
-        return torch.Tensor([M1, M2, M3, M4])
-
-    def update(self, target) -> np.ndarray:
-        curr_pos = self.__get_drone_pos()
-        curr_vel = self.__get_drone_vel()
-        curr_att = self.__get_drone_att()
-
-        err_pos_x = target[0] - curr_pos[0]
-        err_pos_y = target[1] - curr_pos[1]
-        err_pos_z = target[2] - curr_pos[2]
-
-        vel_des_x = self.__pid_pos_x.update(err_pos_x, self.__dt)
-        vel_des_y = self.__pid_pos_y.update(err_pos_y, self.__dt)
-        vel_des_z = self.__pid_pos_z.update(err_pos_z, self.__dt)
-
-        error_vel_x = vel_des_x - curr_vel[0]
-        error_vel_y = vel_des_y - curr_vel[1]
-        error_vel_z = vel_des_z - curr_vel[2]
-
-        x_vel_del   = self.__pid_vel_x.update(error_vel_x, self.__dt)
-        y_vel_del  = self.__pid_vel_y.update(error_vel_y, self.__dt)
-        thrust_des = self.__pid_vel_z.update(error_vel_z, self.__dt)
-
-        err_roll  = 0. - curr_att[0]
-        err_pitch = 0. - curr_att[1]
-        err_yaw   = 0. - curr_att[2]
-
-        roll_del  = self.__pid_att_roll.update(err_roll, self.__dt)
-        pitch_del = self.__pid_att_pitch.update(err_pitch, self.__dt)
-        yaw_del   = self.__pid_att_yaw.update(err_yaw, self.__dt)
-
-        prop_rpms = self.__mixer(thrust_des, roll_del, pitch_del, yaw_del, x_vel_del, y_vel_del)
-        prop_rpms = prop_rpms.cpu()
-        prop_rpms - prop_rpms.numpy()
-
-        return prop_rpms