class BaseSingleAgentAviary(BaseAviary): """Base single drone environment class for reinforcement learning.""" ################################################################################ def __init__(self, drone_model: DroneModel=DroneModel.CF2X, initial_xyzs=None, initial_rpys=None, physics: Physics=Physics.PYB, freq: int=240, aggregate_phy_steps: int=1, gui=False, record=False, obs: ObservationType=ObservationType.KIN, act: ActionType=ActionType.RPM ): """Initialization of a generic single agent RL environment. Attribute `num_drones` is automatically set to 1; `vision_attributes` and `dynamics_attributes` are selected based on the choice of `obs` and `act`; `obstacles` is set to True and overridden with landmarks for vision applications; `user_debug_gui` is set to False for performance. Parameters ---------- drone_model : DroneModel, optional The desired drone type (detailed in an .urdf file in folder `assets`). initial_xyzs: ndarray | None, optional (NUM_DRONES, 3)-shaped array containing the initial XYZ position of the drones. initial_rpys: ndarray | None, optional (NUM_DRONES, 3)-shaped array containing the initial orientations of the drones (in radians). physics : Physics, optional The desired implementation of PyBullet physics/custom dynamics. freq : int, optional The frequency (Hz) at which the physics engine steps. aggregate_phy_steps : int, optional The number of physics steps within one call to `BaseAviary.step()`. gui : bool, optional Whether to use PyBullet's GUI. record : bool, optional Whether to save a video of the simulation in folder `files/videos/`. obs : ObservationType, optional The type of observation space (kinematic information or vision) act : ActionType, optional The type of action space (1 or 3D; RPMS, thurst and torques, waypoint or velocity with PID control; etc.) """ vision_attributes = True if obs == ObservationType.RGB else False dynamics_attributes = True if act in [ActionType.DYN, ActionType.ONE_D_DYN] else False self.OBS_TYPE = obs self.ACT_TYPE = act self.EPISODE_LEN_SEC = 5 #### Create integrated controllers ######################### if act in [ActionType.PID, ActionType.VEL, ActionType.TUN, ActionType.ONE_D_PID]: os.environ['KMP_DUPLICATE_LIB_OK']='True' if drone_model in [DroneModel.CF2X, DroneModel.CF2P]: self.ctrl = DSLPIDControl(drone_model=DroneModel.CF2X) if act == ActionType.TUN: self.TUNED_P_POS = np.array([.4, .4, 1.25]) self.TUNED_I_POS = np.array([.05, .05, .05]) self.TUNED_D_POS = np.array([.2, .2, .5]) self.TUNED_P_ATT = np.array([70000., 70000., 60000.]) self.TUNED_I_ATT = np.array([.0, .0, 500.]) self.TUNED_D_ATT = np.array([20000., 20000., 12000.]) elif drone_model in [DroneModel.HB, DroneModel.ARDRONE2]: self.ctrl = SimplePIDControl(drone_model=drone_model) if act == ActionType.TUN: self.TUNED_P_POS = np.array([.1, .1, .2]) self.TUNED_I_POS = np.array([.0001, .0001, .0001]) self.TUNED_D_POS = np.array([.3, .3, .4]) self.TUNED_P_ATT = np.array([.3, .3, .05]) self.TUNED_I_ATT = np.array([.0001, .0001, .0001]) self.TUNED_D_ATT = np.array([.3, .3, .5]) else: print("[ERROR] in BaseSingleAgentAviary.__init()__, no controller is available for the specified drone_model") super().__init__(drone_model=drone_model, num_drones=1, initial_xyzs=initial_xyzs, initial_rpys=initial_rpys, physics=physics, freq=freq, aggregate_phy_steps=aggregate_phy_steps, gui=gui, record=record, obstacles=True, # Add obstacles for RGB observations and/or FlyThruGate user_debug_gui=False, # Remove of RPM sliders from all single agent learning aviaries vision_attributes=vision_attributes, dynamics_attributes=dynamics_attributes ) #### Set a limit on the maximum target speed ############### if act == ActionType.VEL: self.SPEED_LIMIT = 0.03 * self.MAX_SPEED_KMH * (1000/3600) #### Try _trajectoryTrackingRPMs exists IFF ActionType.TUN # if act == ActionType.TUN and not (hasattr(self.__class__, '_trajectoryTrackingRPMs') and callable(getattr(self.__class__, '_trajectoryTrackingRPMs'))): print("[ERROR] in BaseSingleAgentAviary.__init__(), ActionType.TUN requires an implementation of _trajectoryTrackingRPMs in the instantiated subclass") exit() ################################################################################ def _addObstacles(self): """Add obstacles to the environment. Only if the observation is of type RGB, 4 landmarks are added. Overrides BaseAviary's method. """ if self.OBS_TYPE == ObservationType.RGB: p.loadURDF("block.urdf", [1, 0, .1], p.getQuaternionFromEuler([0, 0, 0]), physicsClientId=self.CLIENT ) p.loadURDF("cube_small.urdf", [0, 1, .1], p.getQuaternionFromEuler([0, 0, 0]), physicsClientId=self.CLIENT ) p.loadURDF("duck_vhacd.urdf", [-1, 0, .1], p.getQuaternionFromEuler([0, 0, 0]), physicsClientId=self.CLIENT ) p.loadURDF("teddy_vhacd.urdf", [0, -1, .1], p.getQuaternionFromEuler([0, 0, 0]), physicsClientId=self.CLIENT ) else: pass ################################################################################ def _actionSpace(self): """Returns the action space of the environment. Returns ------- ndarray A Box() of size 1, 3, 4, or 6 depending on the action type. """ if self.ACT_TYPE == ActionType.TUN: size = 6 elif self.ACT_TYPE in [ActionType.RPM, ActionType.DYN, ActionType.VEL]: size = 4 elif self.ACT_TYPE == ActionType.PID: size = 3 elif self.ACT_TYPE in [ActionType.ONE_D_RPM, ActionType.ONE_D_DYN, ActionType.ONE_D_PID]: size = 1 else: print("[ERROR] in BaseSingleAgentAviary._actionSpace()") exit() return spaces.Box(low=-1*np.ones(size), high=np.ones(size), dtype=np.float32 ) ################################################################################ def _preprocessAction(self, action ): """Pre-processes the action passed to `.step()` into motors' RPMs. Parameter `action` is processed differenly for each of the different action types: `action` can be of length 1, 3, 4, or 6 and represent RPMs, desired thrust and torques, the next target position to reach using PID control, a desired velocity vector, new PID coefficients, etc. Parameters ---------- action : ndarray The input action for each drone, to be translated into RPMs. Returns ------- ndarray (4,)-shaped array of ints containing to clipped RPMs commanded to the 4 motors of each drone. """ if self.ACT_TYPE == ActionType.TUN: self.ctrl.setPIDCoefficients(p_coeff_pos=(action[0]+1)*self.TUNED_P_POS, i_coeff_pos=(action[1]+1)*self.TUNED_I_POS, d_coeff_pos=(action[2]+1)*self.TUNED_D_POS, p_coeff_att=(action[3]+1)*self.TUNED_P_ATT, i_coeff_att=(action[4]+1)*self.TUNED_I_ATT, d_coeff_att=(action[5]+1)*self.TUNED_D_ATT ) return self._trajectoryTrackingRPMs() elif self.ACT_TYPE == ActionType.RPM: return np.array(self.HOVER_RPM * (1+0.05*action)) elif self.ACT_TYPE == ActionType.DYN: return nnlsRPM(thrust=(self.GRAVITY*(action[0]+1)), x_torque=(0.05*self.MAX_XY_TORQUE*action[1]), y_torque=(0.05*self.MAX_XY_TORQUE*action[2]), z_torque=(0.05*self.MAX_Z_TORQUE*action[3]), counter=self.step_counter, max_thrust=self.MAX_THRUST, max_xy_torque=self.MAX_XY_TORQUE, max_z_torque=self.MAX_Z_TORQUE, a=self.A, inv_a=self.INV_A, b_coeff=self.B_COEFF, gui=self.GUI ) elif self.ACT_TYPE == ActionType.PID: state = self._getDroneStateVector(0) rpm, _, _ = self.ctrl.computeControl(control_timestep=self.AGGR_PHY_STEPS*self.TIMESTEP, cur_pos=state[0:3], cur_quat=state[3:7], cur_vel=state[10:13], cur_ang_vel=state[13:16], target_pos=state[0:3]+0.1*action ) return rpm elif self.ACT_TYPE == ActionType.VEL: state = self._getDroneStateVector(0) if np.linalg.norm(action[0:3]) != 0: v_unit_vector = action[0:3] / np.linalg.norm(action[0:3]) else: v_unit_vector = np.zeros(3) rpm, _, _ = self.ctrl.computeControl(control_timestep=self.AGGR_PHY_STEPS*self.TIMESTEP, cur_pos=state[0:3], cur_quat=state[3:7], cur_vel=state[10:13], cur_ang_vel=state[13:16], target_pos=state[0:3], # same as the current position target_rpy=np.array([0,0,state[9]]), # keep current yaw target_vel=self.SPEED_LIMIT * np.abs(action[3]) * v_unit_vector # target the desired velocity vector ) return rpm elif self.ACT_TYPE == ActionType.ONE_D_RPM: return np.repeat(self.HOVER_RPM * (1+0.05*action), 4) elif self.ACT_TYPE == ActionType.ONE_D_DYN: return nnlsRPM(thrust=(self.GRAVITY*(1+0.05*action[0])), x_torque=0, y_torque=0, z_torque=0, counter=self.step_counter, max_thrust=self.MAX_THRUST, max_xy_torque=self.MAX_XY_TORQUE, max_z_torque=self.MAX_Z_TORQUE, a=self.A, inv_a=self.INV_A, b_coeff=self.B_COEFF, gui=self.GUI ) elif self.ACT_TYPE == ActionType.ONE_D_PID: state = self._getDroneStateVector(0) rpm, _, _ = self.ctrl.computeControl(control_timestep=self.AGGR_PHY_STEPS*self.TIMESTEP, cur_pos=state[0:3], cur_quat=state[3:7], cur_vel=state[10:13], cur_ang_vel=state[13:16], target_pos=state[0:3]+0.1*np.array([0,0,action[0]]) ) return rpm else: print("[ERROR] in BaseSingleAgentAviary._preprocessAction()") ################################################################################ def _observationSpace(self): """Returns the observation space of the environment. Returns ------- ndarray A Box() of shape (H,W,4) or (12,) depending on the observation type. """ if self.OBS_TYPE == ObservationType.RGB: return spaces.Box(low=0, high=255, shape=(self.IMG_RES[1], self.IMG_RES[0], 4), dtype=np.uint8 ) elif self.OBS_TYPE == ObservationType.KIN: ############################################################ #### OBS OF SIZE 20 (WITH QUATERNION AND RPMS) #### Observation vector ### X Y Z Q1 Q2 Q3 Q4 R P Y VX VY VZ WX WY WZ P0 P1 P2 P3 # obs_lower_bound = np.array([-1, -1, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1]) # obs_upper_bound = np.array([1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]) # return spaces.Box( low=obs_lower_bound, high=obs_upper_bound, dtype=np.float32 ) ############################################################ #### OBS SPACE OF SIZE 12 return spaces.Box(low=np.array([-1,-1,0, -1,-1,-1, -1,-1,-1, -1,-1,-1]), high=np.array([1,1,1, 1,1,1, 1,1,1, 1,1,1]), dtype=np.float32 ) ############################################################ else: print("[ERROR] in BaseSingleAgentAviary._observationSpace()") ################################################################################ def _computeObs(self): """Returns the current observation of the environment. Returns ------- ndarray A Box() of shape (H,W,4) or (12,) depending on the observation type. """ if self.OBS_TYPE == ObservationType.RGB: if self.step_counter%self.IMG_CAPTURE_FREQ == 0: self.rgb[0], self.dep[0], self.seg[0] = self._getDroneImages(0, segmentation=False ) #### Printing observation to PNG frames example ############ if self.RECORD: self._exportImage(img_type=ImageType.RGB, img_input=self.rgb[0], path=self.ONBOARD_IMG_PATH, frame_num=int(self.step_counter/self.IMG_CAPTURE_FREQ) ) return self.rgb[0] elif self.OBS_TYPE == ObservationType.KIN: obs = self._clipAndNormalizeState(self._getDroneStateVector(0)) ############################################################ #### OBS OF SIZE 20 (WITH QUATERNION AND RPMS) # return obs ############################################################ #### OBS SPACE OF SIZE 12 return np.hstack([obs[0:3], obs[7:10], obs[10:13], obs[13:16]]).reshape(12,) ############################################################ else: print("[ERROR] in BaseSingleAgentAviary._computeObs()") ################################################################################ def _clipAndNormalizeState(self, state ): """Normalizes a drone's state to the [-1,1] range. Must be implemented in a subclass. Parameters ---------- state : ndarray Array containing the non-normalized state of a single drone. """ raise NotImplementedError
class BaseSingleAgentAviary(BaseAviary): #################################################################################################### #### Initialize the environment #################################################################### #################################################################################################### #### Arguments ##################################################################################### #### - drone_model (DroneModel) desired drone type (associated to an .urdf file) ########### #### - num_drones (int) desired number of drones in the aviary ##################### #### - neighbourhood_radius (float) used to compute the drones' adjacency matrix, in meters #### #### - initial_xyzs ((3,1) array) initial XYZ position of the drones ######################### #### - initial_rpys ((3,1) array) initial orientations of the drones (radians) ############### #### - physics (Physics) desired implementation of physics/dynamics ################# #### - freq (int) the frequency (Hz) at which the physics engine advances #### #### - aggregate_phy_steps (int) number of physics updates within one call of .step() ####### #### - gui (bool) whether to use PyBullet's GUI ############################## #### - record (bool) whether to save a video of the simulation ################## #### - obstacles (bool) whether to add obstacles to the simulation ################# #### - user_debug_gui (bool) whether to draw the drones' axes and the GUI sliders ####### #### - #### #################################################################################################### def __init__(self, drone_model: DroneModel=DroneModel.CF2X, initial_xyzs=None, initial_rpys=None, physics: Physics=Physics.PYB, freq: int=240, aggregate_phy_steps: int=1, gui=False, record=False, obs: ObservationType=ObservationType.KIN, act: ActionType=ActionType.RPM): self.EPISODE_LEN_SEC = 5 self.OBS_TYPE = obs; self.ACT_TYPE = act #### Before super().__init__ to initialize the proper action/obs spaces ############################ if self.OBS_TYPE==ObservationType.RGB: self.IMG_RES = np.array([64, 48]); self.IMG_FRAME_PER_SEC = 24; self.IMG_CAPTURE_FREQ = int(freq/self.IMG_FRAME_PER_SEC) self.rgb = np.zeros(((1, self.IMG_RES[1], self.IMG_RES[0], 4))); self.dep = np.ones(((1, self.IMG_RES[1], self.IMG_RES[0]))); self.seg = np.zeros(((1, self.IMG_RES[1], self.IMG_RES[0]))) if self.IMG_CAPTURE_FREQ%aggregate_phy_steps!=0: print("[ERROR] in BaseSingleAgentAviary.__init__(), aggregate_phy_steps incompatible with the desired video capture frame rate ({:f}Hz)".format(self.IMG_FRAME_PER_SEC)); exit() super().__init__(drone_model=drone_model, num_drones=1, initial_xyzs=initial_xyzs, initial_rpys=initial_rpys, physics=physics, freq=freq, aggregate_phy_steps=aggregate_phy_steps, gui=gui, record=record, obstacles=True, # Add obstacles for RGB observations and/or FlyThruGate user_debug_gui=False) # Remove of RPM sliders from all single agent learning aviaries #### After super().__init__ to use the proper constants ############################################ if self.ACT_TYPE==ActionType.DYN or self.ACT_TYPE==ActionType.ONE_D_DYN: if self.DRONE_MODEL==DroneModel.CF2X: self.A = np.array([ [1, 1, 1, 1], [.5, .5, -.5, -.5], [-.5, .5, .5, -.5], [-1, 1, -1, 1] ]) elif self.DRONE_MODEL in [DroneModel.CF2P, DroneModel.HB]: self.A = np.array([ [1, 1, 1, 1], [0, 1, 0, -1], [-1, 0, 1, 0], [-1, 1, -1, 1] ]) self.INV_A = np.linalg.inv(self.A); self.B_COEFF = np.array([1/self.KF, 1/(self.KF*self.L), 1/(self.KF*self.L), 1/self.KM]) if self.ACT_TYPE==ActionType.PID or self.ACT_TYPE==ActionType.ONE_D_PID: os.environ['KMP_DUPLICATE_LIB_OK']='True' if self.DRONE_MODEL in [DroneModel.CF2X, DroneModel.CF2P]: self.ctrl = DSLPIDControl(CtrlAviary(drone_model=DroneModel.CF2X)) elif self.DRONE_MODEL==DroneModel.HB: self.ctrl = SimplePIDControl(CtrlAviary(drone_model=DroneModel.HB)) if self.OBS_TYPE==ObservationType.RGB and self.RECORD: self.ONBOARD_IMG_PATH = os.path.dirname(os.path.abspath(__file__))+"/../../../files/videos/onboard-"+datetime.now().strftime("%m.%d.%Y_%H.%M.%S")+"/"; os.makedirs(os.path.dirname(self.ONBOARD_IMG_PATH), exist_ok=True) #################################################################################################### #### Add obstacles to the environment from .urdf files ############################################# #################################################################################################### def _addObstacles(self): if self.OBS_TYPE==ObservationType.RGB: p.loadURDF("block.urdf", [1,0,.1], p.getQuaternionFromEuler([0,0,0]), physicsClientId=self.CLIENT) p.loadURDF("cube_small.urdf", [0,1,.1], p.getQuaternionFromEuler([0,0,0]), physicsClientId=self.CLIENT) p.loadURDF("duck_vhacd.urdf", [-1,0,.1], p.getQuaternionFromEuler([0,0,0]), physicsClientId=self.CLIENT) p.loadURDF("teddy_vhacd.urdf", [0,-1,.1], p.getQuaternionFromEuler([0,0,0]), physicsClientId=self.CLIENT) else: pass #################################################################################################### #### Return the action space of the environment, a Box(4,) ######################################### #################################################################################################### def _actionSpace(self): #### Action vector ######## P0 P1 P2 P3 act_lower_bound = np.array([-1, -1, -1, -1]) act_upper_bound = np.array([1, 1, 1, 1]) if self.ACT_TYPE==ActionType.RPM or self.ACT_TYPE==ActionType.DYN: return spaces.Box( low=act_lower_bound, high=act_upper_bound, dtype=np.float32 ) elif self.ACT_TYPE==ActionType.PID: return spaces.Box( low=-1*np.ones(3), high=np.ones(3), dtype=np.float32 ) elif self.ACT_TYPE in [ActionType.ONE_D_RPM, ActionType.ONE_D_DYN, ActionType.ONE_D_PID]: return spaces.Box( low=np.array([-1]), high=np.array([1]), dtype=np.float32 ) else: print("[ERROR] in BaseSingleAgentAviary._actionSpace()") #################################################################################################### #### Preprocess the action passed to step() ######################################################## #################################################################################################### #### Arguments ##################################################################################### #### - action ((4,1) array) unclipped RPMs commanded to the 4 motors of the drone ###### #################################################################################################### #### Returns ####################################################################################### #### - clipped_action ((4,1) array) clipped RPMs commanded to the 4 motors of the drone ######## #################################################################################################### def _preprocessAction(self, action): if self.ACT_TYPE==ActionType.RPM: return np.array(self.HOVER_RPM * (1+0.05*action)) elif self.ACT_TYPE==ActionType.DYN: return self._nnlsRPM(thrust=(self.GRAVITY*(action[0]+1)), x_torque=(0.05*self.MAX_XY_TORQUE*action[1]), y_torque=(0.05*self.MAX_XY_TORQUE*action[2]), z_torque=(0.05*self.MAX_Z_TORQUE*action[3])) elif self.ACT_TYPE==ActionType.PID: state = self._getDroneStateVector(0) rpm, _, _ = self.ctrl.computeControl(control_timestep=self.AGGR_PHY_STEPS*self.TIMESTEP, cur_pos=state[0:3], cur_quat=state[3:7], cur_vel=state[10:13], cur_ang_vel=state[13:16], target_pos=state[0:3]+1*action ) return rpm elif self.ACT_TYPE==ActionType.ONE_D_RPM: return np.repeat(self.HOVER_RPM * (1+0.05*action), 4) elif self.ACT_TYPE==ActionType.ONE_D_DYN: return self._nnlsRPM(thrust=(self.GRAVITY*(1+0.05*action[0])), x_torque=0, y_torque=0, z_torque=0) elif self.ACT_TYPE==ActionType.ONE_D_PID: state = self._getDroneStateVector(0) rpm, _, _ = self.ctrl.computeControl(control_timestep=self.AGGR_PHY_STEPS*self.TIMESTEP, cur_pos=state[0:3], cur_quat=state[3:7], cur_vel=state[10:13], cur_ang_vel=state[13:16], target_pos=state[0:3]+1*np.array([0,0,action[0]]) ) return rpm else: print("[ERROR] in BaseSingleAgentAviary._preprocessAction()") #################################################################################################### #### Return the observation space of the environment, a Box(20,) ################################### #################################################################################################### def _observationSpace(self): if self.OBS_TYPE==ObservationType.RGB: return spaces.Box(low=0, high=255, shape=(self.IMG_RES[1], self.IMG_RES[0], 4), dtype=np.uint8) elif self.OBS_TYPE==ObservationType.KIN: ################################### #### OBS OF SIZE 20 (WITH QUATERNION AND RPMS) #### Observation vector ### X Y Z Q1 Q2 Q3 Q4 R P Y VX VY VZ WR WP WY P0 P1 P2 P3 # obs_lower_bound = np.array([-1, -1, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1]) # obs_upper_bound = np.array([1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]) # return spaces.Box( low=obs_lower_bound, high=obs_upper_bound, dtype=np.float32 ) ################################### #### OBS SPACE OF SIZE 12 return spaces.Box( low=np.array([-1,-1,0, -1,-1,-1, -1,-1,-1, -1,-1,-1]), high=np.array([1,1,1, 1,1,1, 1,1,1, 1,1,1]), dtype=np.float32 ) ################################### else: print("[ERROR] in BaseSingleAgentAviary._observationSpace()") #################################################################################################### #### Return the current observation of the environment ############################################# #################################################################################################### #### Returns ####################################################################################### #### - obs (20,) array for its content see _observationSpace() #################### #################################################################################################### def _computeObs(self): if self.OBS_TYPE==ObservationType.RGB: if self.step_counter%self.IMG_CAPTURE_FREQ==0: self.rgb[0], self.dep[0], self.seg[0] = self._getDroneImages(0, segmentation=False) #### Printing observation to PNG frames example #################################################### if self.RECORD: self._exportImage(img_type=ImageType.RGB, img_input=self.rgb[0], path=self.ONBOARD_IMG_PATH, frame_num=int(self.step_counter/self.IMG_CAPTURE_FREQ)) return self.rgb[0] elif self.OBS_TYPE==ObservationType.KIN: obs = self._clipAndNormalizeState(self._getDroneStateVector(0)) ################################### #### OBS OF SIZE 20 (WITH QUATERNION AND RPMS) # return obs ################################### #### OBS SPACE OF SIZE 12 return np.hstack([ obs[0:3], obs[7:10], obs[10:13], obs[13:16] ]).reshape(12,) ################################### else: print("[ERROR] in BaseSingleAgentAviary._observationSpace()") #################################################################################################### #### Normalize the 20 values in the simulation state to the [-1,1] range ########################### #################################################################################################### #### Arguments ##################################################################################### #### - state ((20,1) array) raw simulation state ####################################### #################################################################################################### #### Returns ####################################################################################### #### - ... #################################################################################################### def _clipAndNormalizeState(self, state): pass #################################################################################################### #### Non-negative Least Squares (NNLS) RPM from desired thrust and torques ######################## #################################################################################################### #### Arguments ##################################################################################### #### - thrust (float) desired thrust along the local z-axis ###################### #### - x_torque (float) desired x-axis torque ###################################### #### - y_torque (float) desired y-axis torque ###################################### #### - z_torque (float) desired z-axis torque ###################################### #################################################################################################### #### Returns ####################################################################################### #### - rpm ((4,1) array) RPM values to apply to the 4 motors ######################## #################################################################################################### def _nnlsRPM(self, thrust, x_torque, y_torque, z_torque): #### Check the feasibility of thrust and torques ################################################### if self.GUI and (thrust<0 or thrust>self.MAX_THRUST): print("[WARNING] it", self.step_counter, "in DynCtrlAviary._nnlsRPM(), unfeasible thrust {:.2f} outside range [0, {:.2f}]".format(thrust, self.MAX_THRUST)) if self.GUI and np.abs(x_torque)>self.MAX_XY_TORQUE: print("[WARNING] it", self.step_counter, "in DynCtrlAviary._nnlsRPM(), unfeasible roll torque {:.2f} outside range [{:.2f}, {:.2f}]".format(x_torque, -self.MAX_XY_TORQUE, self.MAX_XY_TORQUE)) if self.GUI and np.abs(y_torque)>self.MAX_XY_TORQUE: print("[WARNING] it", self.step_counter, "in DynCtrlAviary._nnlsRPM(), unfeasible pitch torque {:.2f} outside range [{:.2f}, {:.2f}]".format(y_torque, -self.MAX_XY_TORQUE, self.MAX_XY_TORQUE)) if self.GUI and np.abs(z_torque)>self.MAX_Z_TORQUE: print("[WARNING] it", self.step_counter, "in DynCtrlAviary._nnlsRPM(), unfeasible yaw torque {:.2f} outside range [{:.2f}, {:.2f}]".format(z_torque, -self.MAX_Z_TORQUE, self.MAX_Z_TORQUE)) B = np.multiply(np.array([thrust, x_torque, y_torque, z_torque]), self.B_COEFF) sq_rpm = np.dot(self.INV_A, B) #### Use NNLS if any of the desired angular velocities is negative ################################# if np.min(sq_rpm)<0: sol, res = nnls(self.A, B, maxiter=3*self.A.shape[1]) if self.GUI: print("[WARNING] it", self.step_counter, "in DynCtrlAviary._nnlsRPM(), unfeasible squared rotor speeds, using NNLS") print("Negative sq. rotor speeds:\t [{:.2f}, {:.2f}, {:.2f}, {:.2f}]".format(sq_rpm[0], sq_rpm[1], sq_rpm[2], sq_rpm[3]), "\t\tNormalized: [{:.2f}, {:.2f}, {:.2f}, {:.2f}]".format(sq_rpm[0]/np.linalg.norm(sq_rpm), sq_rpm[1]/np.linalg.norm(sq_rpm), sq_rpm[2]/np.linalg.norm(sq_rpm), sq_rpm[3]/np.linalg.norm(sq_rpm))) print("NNLS:\t\t\t\t [{:.2f}, {:.2f}, {:.2f}, {:.2f}]".format(sol[0], sol[1], sol[2], sol[3]), "\t\t\tNormalized: [{:.2f}, {:.2f}, {:.2f}, {:.2f}]".format(sol[0]/np.linalg.norm(sol), sol[1]/np.linalg.norm(sol), sol[2]/np.linalg.norm(sol), sol[3]/np.linalg.norm(sol)), "\t\tResidual: {:.2f}".format(res) ) sq_rpm = sol return np.sqrt(sq_rpm)