def dynamics(self, pos, rot, vel, cmd):
# unpacking the control input
roll_cmd, pitch_cmd, thrust_cmd, yaw_rate_cmd = cmd
ea = uf.rot_to_ea(rot)
# current yaw
yaw = ea[2]
# maximum yaw rate
max_yaw_rate_deg = sp.max_yaw_rate_deg
max_yaw_rate = max_yaw_rate_deg * numpy.pi / 180
# maximum tilt
max_tilt_deg = sp.max_tilt_deg
max_tilt = max_tilt_deg * numpy.pi / 180
# desired roll
roll_des = (roll_cmd - 1500) * max_tilt / 500
# desired pitch
pitch_des = -(pitch_cmd - 1500) * max_tilt / 500
# gain of the inner loop for attitude control
ktt = sp.inner_loop_gain
# desired thrust versor
e3 = numpy.array([0.0, 0.0, 1.0])
dtv = uf.ea_to_rot((roll_des, pitch_des, yaw)).dot(e3)
# actual thrust versor
atv = rot.dot(e3)
# angular velocity
aux = ktt * uf.skew(atv).dot(dtv)
rot_t = numpy.transpose(rot)
omega = rot_t.dot(aux)
# yaw rate
omega[2] = -(yaw_rate_cmd - 1500) * max_yaw_rate / 500
# neutral thrust
nt = sp.neutral_thrust
# thrust gain
kt = sp.quad_mass * sp.g / nt
# thrust command adjustment
# The thrust sent to the motors is automatically adjusted according to
# the tilt angle of the vehicle (bigger tilt leads to bigger thrust)
# to reduce the thrust compensation that the pilot must give.
thrust_cmd = thrust_cmd / numpy.dot(atv, e3)
# dynamics
dpos = vel
dvel = kt * thrust_cmd * atv / sp.quad_mass - sp.g * e3
drot = rot.dot(uf.skew(omega))
return dpos, drot, dvel
def _cmd_to_thrust_omega(self, cmd, rot):
"""Computes the thrust as a 3D numpy array from the RC command."""
# unpacking the control input
roll_cmd, pitch_cmd, throttle_cmd, yaw_rate_cmd = cmd
# getting the current yaw from rot
current_yaw = uf.rot_to_ea(rot)[2]
# maximum yaw rate
max_yaw_rate_deg = isp.max_yaw_rate_deg
max_yaw_rate = max_yaw_rate_deg * np.pi / 180.0
# angular speed
omega = np.zeros(3)
omega[2] = -(float(yaw_rate_cmd) - 1500.0) * max_yaw_rate / 500.0
# maximum tilt
max_tilt_deg = isp.max_tilt_deg
max_tilt = max_tilt_deg * np.pi / 180.0
# desired roll
roll_des = (float(roll_cmd) - 1500.0) * max_tilt / 500.0
# desired pitch
pitch_des = -(float(pitch_cmd) - 1500.0) * max_tilt / 500.0
# desired acceleration
ea = roll_des, pitch_des, current_yaw
rot = uf.ea_to_rot(ea)
throttle_gain = isp.quad_mass * isp.gravity / isp.neutral_throttle
thrust = throttle_gain * throttle_cmd * rot.dot(uf.e3)
return thrust, omega
def control_law(self, pos, rot, vel, rp, rv, ra, rj, rs, rc):
"""Returns the control input for the Iris quad in the stabilize mode."""
err_pos = rp[0:3] - pos
err_vel = rv[0:3] - vel
current_yaw = uf.rot_to_ea(rot)[2]
err_yaw = rp[3] - current_yaw
err_yaw = np.arctan2(np.sin(err_yaw), np.cos(err_yaw))
des_acc = icp.gravity * uf.e3 + icp.kp*(err_pos) + icp.kv*(err_vel)
des_thrust = des_acc * icp.quad_mass
des_throttle = np.linalg.norm(des_thrust)
des_yaw_rate = icp.k_yaw*(err_yaw)
if des_throttle < icp.min_throttle:
des_roll = 0.0
des_pitch = 0.0
else:
n_des = des_thrust/des_throttle
n_des_rot = uf.rot_z(-current_yaw).dot(n_des)
sin_roll = -n_des_rot[1]
des_roll = np.arcsin(sin_roll)
sin_pitch = n_des_rot[0]/np.cos(des_roll)
cos_pitch = n_des_rot[2]/np.cos(des_roll)
des_pitch = np.arctan2(sin_pitch,cos_pitch)
cmd_throttle = des_throttle/icp.quad_mass/icp.gravity*icp.neutral_throttle
cmd_throttle = int(self._saturation(cmd_throttle, 1000.0, 2000.0))
max_tilt = icp.max_tilt_deg * np.pi/180.0
cmd_roll = 1500.0 + des_roll*500.0/max_tilt
cmd_roll = int(self._saturation(cmd_roll, 1000.0, 2000.0))
cmd_pitch = int(1500.0 - des_pitch*500.0/max_tilt)
cmd_pitch = int(self._saturation(cmd_pitch, 1000.0, 2000.0))
max_yaw_rate = icp.max_yaw_rate_deg * np.pi / 180.0
cmd_yaw = int(1500.0 - des_yaw_rate*500.0/max_yaw_rate)
cmd_yaw = int(self._saturation(cmd_yaw, 1000.0, 2000.0))
cmd = (cmd_roll, cmd_pitch, cmd_throttle, cmd_yaw)
return cmd
