def _preprocessAction(self, action): rpm = np.zeros((self.NUM_DRONES, 4)) for k, v in action.items(): if self.ACT_TYPE == ActionType.RPM: rpm[int(k), :] = np.array(self.HOVER_RPM * (1 + 0.05 * v)) elif self.ACT_TYPE == ActionType.DYN: rpm[int(k), :] = nnlsRPM( thrust=(self.GRAVITY * (v[0] + 1)), x_torque=(0.05 * self.MAX_XY_TORQUE * v[1]), y_torque=(0.05 * self.MAX_XY_TORQUE * v[2]), z_torque=(0.05 * self.MAX_Z_TORQUE * v[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(int(k)) rpm, _, _ = self.ctrl[k].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 * v) rpm[int(k), :] = rpm elif self.ACT_TYPE == ActionType.ONE_D_RPM: rpm[int(k), :] = np.repeat(self.HOVER_RPM * (1 + 0.05 * v), 4) elif self.ACT_TYPE == ActionType.ONE_D_DYN: rpm[int(k), :] = nnlsRPM(thrust=(self.GRAVITY * (1 + 0.05 * v[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(int(k)) rpm, _, _ = self.ctrl[k].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, v[0]])) rpm[int(k), :] = rpm else: print("[ERROR] in BaseMultiagentAviary._preprocessAction()") exit() return rpm
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 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[0].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.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[0].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 _preprocessAction(self, action): """Pre-processes the action passed to `.step()` into motors' RPMs. Solves desired thrust and torques using NNLS and converts a dictionary into a 2D array. Parameters ---------- action : dict[str, ndarray] The input action each drone (desired thrust and torques), to be translated into RPMs. Returns ------- ndarray (NUM_DRONES, 4)-shaped array of ints containing to clipped RPMs commanded to the 4 motors of each drone. """ clipped_action = np.zeros((self.NUM_DRONES, 4)) for k, v in action.items(): clipped_action[int(k), :] = nnlsRPM( thrust=v[0], x_torque=v[1], y_torque=v[2], z_torque=v[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) return clipped_action
def _simplePIDAttitudeControl(self, control_timestep, thrust, cur_quat, target_rpy ): """Simple PID attitude control (with yaw fixed to 0). Parameters ---------- control_timestep : float The time step at which control is computed. thrust : float The target thrust along the drone z-axis. cur_quat : ndarray (4,1)-shaped array of floats containing the current orientation as a quaternion. target_rpy : ndarray (3,1)-shaped array of floats containing the computed the target roll, pitch, and yaw. Returns ------- ndarray (4,1)-shaped array of integers containing the RPMs to apply to each of the 4 motors. """ cur_rpy = p.getEulerFromQuaternion(cur_quat) rpy_e = target_rpy - np.array(cur_rpy).reshape(3,) if rpy_e[2] > np.pi: rpy_e[2] = rpy_e[2] - 2*np.pi if rpy_e[2] < -np.pi: rpy_e[2] = rpy_e[2] + 2*np.pi d_rpy_e = (rpy_e - self.last_rpy_e) / control_timestep self.last_rpy_e = rpy_e self.integral_rpy_e = self.integral_rpy_e + rpy_e*control_timestep #### PID target torques #################################### target_torques = np.multiply(self.P_COEFF_TOR, rpy_e) \ + np.multiply(self.I_COEFF_TOR, self.integral_rpy_e) \ + np.multiply(self.D_COEFF_TOR, d_rpy_e) return nnlsRPM(thrust=thrust, x_torque=target_torques[0], y_torque=target_torques[1], z_torque=target_torques[2], counter=self.control_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=True )
def _preprocessAction(self, action): clipped_action = np.zeros((self.NUM_DRONES, 4)) for k, v in action.items(): clipped_action[int(k), :] = nnlsRPM( thrust=v[0], x_torque=v[1], y_torque=v[2], z_torque=v[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) return clipped_action
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: the input to n-th drone, `action[n]` can be of length 1, 3, or 4, and represent RPMs, desired thrust and torques, or the next target position to reach using PID control. Parameter `action` is processed differenly for each of the different action types: `action` can be of length 1, 3, or 4 and represent RPMs, desired thrust and torques, the next target position to reach using PID control, a desired velocity vector, etc. Parameters ---------- action : dict[str, ndarray] The input action for each drone, to be translated into RPMs. Returns ------- ndarray (NUM_DRONES, 4)-shaped array of ints containing to clipped RPMs commanded to the 4 motors of each drone. """ rpm = np.zeros((self.NUM_DRONES,4)) for k, v in action.items(): if self.ACT_TYPE == ActionType.RPM: rpm[int(k),:] = np.array(self.HOVER_RPM * (1+0.05*v)) elif self.ACT_TYPE == ActionType.DYN: rpm[int(k),:] = nnlsRPM(thrust=(self.GRAVITY*(v[0]+1)), x_torque=(0.05*self.MAX_XY_TORQUE*v[1]), y_torque=(0.05*self.MAX_XY_TORQUE*v[2]), z_torque=(0.05*self.MAX_Z_TORQUE*v[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(int(k)) rpm_k, _, _ = self.ctrl[int(k)].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*v ) rpm[int(k),:] = rpm_k elif self.ACT_TYPE == ActionType.VEL: state = self._getDroneStateVector(int(k)) if np.linalg.norm(v[0:3]) != 0: v_unit_vector = v[0:3] / np.linalg.norm(v[0:3]) else: v_unit_vector = np.zeros(3) temp, _, _ = self.ctrl[int(k)].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(v[3]) * v_unit_vector # target the desired velocity vector ) rpm[int(k),:] = temp elif self.ACT_TYPE == ActionType.ONE_D_RPM: rpm[int(k),:] = np.repeat(self.HOVER_RPM * (1+0.05*v), 4) elif self.ACT_TYPE == ActionType.ONE_D_DYN: rpm[int(k),:] = nnlsRPM(thrust=(self.GRAVITY*(1+0.05*v[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(int(k)) rpm, _, _ = self.ctrl[k].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,v[0]]) ) rpm[int(k),:] = rpm else: print("[ERROR] in BaseMultiagentAviary._preprocessAction()") exit() return rpm
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 _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, or 4, and represent RPMs, desired thrust and torques, or the next target position to reach using PID control. 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.RPM: return np.array(self.HOVER_RPM * (1 + 0.2 * 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[0].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.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[0].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()")