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 _dynamics(self, pos, rot, vel, cmd):
"""Reimplements the abstract method of the IrisSimulator class."""
thrust, omega = self._cmd_to_thrust_omega(cmd, rot)
acc = thrust / isp.quad_mass - isp.gravity * uf.e3
# derivatives
dpos = vel
dvel = acc
drot = uf.skew(omega).dot(rot)
return dpos, drot, dvel
