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 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