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