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 index c1bba591..12c1da7f 100644 --- a/examples/drone/fly_route.py +++ b/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,65 +41,42 @@ 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 ##### @@ -100,16 +84,13 @@ def main(): 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() \ No newline at end of file + main() diff --git a/examples/drone/quadcopter_controller.py b/examples/drone/quadcopter_controller.py new file mode 100644 index 00000000..8940faf8 --- /dev/null +++ b/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 diff --git a/genesis/engine/entities/drone_entity.py b/genesis/engine/entities/drone_entity.py index c5492416..842fb7ed 100644 --- a/genesis/engine/entities/drone_entity.py +++ b/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 \ No newline at end of file