diff --git a/examples/drone/README.md b/examples/drone/README.md index 39eb37c1..29451a3e 100644 --- a/examples/drone/README.md +++ b/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`) diff --git a/examples/drone/fly_route.py b/examples/drone/fly_route.py new file mode 100644 index 00000000..12c1da7f --- /dev/null +++ b/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() diff --git a/examples/drone/quadcopter_controller.py b/examples/drone/quadcopter_controller.py new file mode 100644 index 00000000..a7a851e4 --- /dev/null +++ b/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 diff --git a/genesis/engine/entities/drone_entity.py b/genesis/engine/entities/drone_entity.py index ccdf5bdf..842fb7ed 100644 --- a/genesis/engine/entities/drone_entity.py +++ b/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)