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()")
