[MISC] Added PID controller util, quadcopter PID controller, flight examples

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

@@ -0,0 +1,96 @@
+import genesis as gs
+import math
+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
+    x = target[0] - drone.get_pos()[0]
+    y = target[1] - drone.get_pos()[1]
+    z = target[2] - drone.get_pos()[2]
+
+    distance = math.sqrt(x**2 + y**2 + z**2)
+
+    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)
+        drone.set_propellels_rpm([M1, M2, M3, M4])
+        scene.step()
+        cam.render()
+        # print("point =", drone.get_pos())
+        drone_pos = drone.get_pos()
+        drone_pos = drone_pos.cpu().numpy()
+        x = drone_pos[0]
+        y = drone_pos[1]
+        z = drone_pos[2]
+        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))
+
+    ##### 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
+    # drone can fly, not optimized
+    pid_params = [
+        [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)
+
+    ##### build #####
+
+    scene.build()
+
+    cam.start_recording()
+
+    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()

examples/drone/quadcopter_controller.py

@@ -0,0 +1,93 @@
+import torch
+import numpy as np
+from genesis.engine.entities.drone_entity import DroneEntity
+from genesis.utils.geom import quat_to_xyz
+
+class PIDController():
+    def __init__(self, kp, ki, kd):
+        self.kp = kp
+        self.ki = ki
+        self.kd = kd
+        self.integral = 0.
+        self.prev_error = 0.
+
+    def update(self, error, dt):
+        self.integral += error * dt
+        derivative = (error - self.prev_error) / dt
+        self.prev_error = error
+
+        return (self.kp*error) + (self.ki*self.integral) + (self.kd*derivative)
+
+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

@@ -12,7 +12,6 @@
 
 @ti.data_oriented
 class DroneEntity(RigidEntity):
-
     def _load_URDF(self, morph, surface):
         super()._load_URDF(morph, surface)