added optimize module; (3a213c4e) · Commits · Ibrahim Ahmed / multirotor

Demo.ipynb→Detailed Demo.ipynb

+124 −81

Changes for Detailed Demo.ipynb: 124 added lines, 81 removed lines.
Original line number	Diff line number	Diff line
		%% Cell type:markdown id:d001f534 tags:

		#### Notebook setup

		%% Cell type:code id:be76bd53 tags:

		``` python
		%pip install -e .
		```

		%% Cell type:code id:5a776232 tags:

		``` python
		%reload_ext autoreload
		%autoreload 2
		%matplotlib inline
		import time
		import warnings
		import os, sys
		from copy import deepcopy
		from types import SimpleNamespace
		from pprint import pprint as print

		import matplotlib.pyplot as plt
		import gym
		import numpy as np
		from tqdm.auto import tqdm, trange

		from multirotor.helpers import control_allocation_matrix, DataLog
		from multirotor.vehicle import MotorParams, VehicleParams, PropellerParams, SimulationParams, BatteryParams
		from multirotor.controller import (
		PosController, VelController,
		AttController, RateController,
		AltController, AltRateController,
		Controller
		)
		from multirotor.simulation import Multirotor, Propeller, Motor, Battery
		from multirotor.coords import body_to_inertial, inertial_to_body, direction_cosine_matrix, euler_to_angular_rate
		from multirotor.env import SpeedsMultirotorEnv, DynamicsMultirotorEnv
		from multirotor.trajectories import Trajectory
		from multirotor.visualize import plot_datalog
		```

		%% Cell type:code id:421b284b tags:

		``` python
		# Plotting/display parameters
		# https://stackoverflow.com/a/21009774/4591810
		float_formatter = "{:.3f}".format
		np.set_printoptions(formatter={'float_kind':float_formatter})

		SMALL_SIZE = 16
		MEDIUM_SIZE = 16
		BIGGER_SIZE = 20

		plt.rc('font', size=SMALL_SIZE) # controls default text sizes
		plt.rc('axes', titlesize=MEDIUM_SIZE) # fontsize of the axes title
		plt.rc('axes', labelsize=BIGGER_SIZE, titlesize=BIGGER_SIZE) # fontsize of the x and y labels
		plt.rc('xtick', labelsize=MEDIUM_SIZE) # fontsize of the tick labels
		plt.rc('ytick', labelsize=MEDIUM_SIZE) # fontsize of the tick labels
		plt.rc('legend', fontsize=SMALL_SIZE) # legend fontsize
		plt.rc('figure', titlesize=BIGGER_SIZE) # fontsize of the figure title
		```

		%% Cell type:markdown id:9a31149f tags:

		### Parameters

		%% Cell type:code id:3c2c1785 tags:

		``` python
		22.2 / 0.0347
		```

		%% Cell type:code id:1ba2a2fe tags:

		``` python
		# Tarot T18 params
		bp = BatteryParams(max_voltage=22.2)
		mp = MotorParams(
		moment_of_inertia=5e-5,
		# resistance=0.27,
		resistance=0.081,
		k_emf=0.0265,
		# k_motor=0.0932,
		speed_voltage_scaling= 0.0347,
		max_current=38.
		)
		pp = PropellerParams(
		moment_of_inertia=1.86e-6,
		use_thrust_constant=True,
		k_thrust=9.8419e-05, # 18-inch propeller
		# k_thrust=5.28847e-05, # 15 inch propeller
		k_drag=1.8503e-06, # 18-inch propeller
		# k_drag=1.34545e-06, # 15-inch propeller
		motor=mp
		)
		vp = VehicleParams(
		propellers=[pp] * 8,
		battery=bp,
		# angles in 45 deg increments, rotated to align with
		# model setup in gazebo sim (not part of this repo)
		angles=np.linspace(0, -2np.pi, num=8, endpoint=False) + 0.375 np.pi,
		distances=np.ones(8) * 0.635,
		clockwise=[-1,1,-1,1,-1,1,-1,1],
		mass=10.66,
		inertia_matrix=np.asarray([
		[0.2206, 0, 0],
		[0, 0.2206, 0.],
		[0, 0, 0.4238]
		])
		)
		sp = SimulationParams(dt=0.01, g=9.81)
		```

		%% Cell type:markdown id:17f3b34d tags:
		%% Cell type:markdown id:48866609 tags:

		### Multirotor

		%% Cell type:markdown id:17f3b34d tags:

		Simulating individual components of the multirotor. These make up the final `Multirotor` object.

		%% Cell type:markdown id:0505e4f6 tags:

		#### Motor

		%% Cell type:code id:b3128c10 tags:

		``` python
		%matplotlib inline
		# Plot motor speeds as a function of time and input voltage signal
		plt.figure(figsize=(8,8))
		motor = Motor(mp, sp)
		for vsignal in [2, 4, 6, 8, 10, 12, 14, 16, 18, 20]:
		speeds = []
		motor.reset()
		speed = vsignal / mp.speed_voltage_scaling
		for i in range(200):
		speeds.append(motor.step(speed))
		plt.plot(speeds, label='%d rad/s' % speed)
		plt.legend(ncol=2)
		plt.ylabel('Speed rad/s')
		plt.xlabel('Time /ms')
		```

		%% Cell type:markdown id:f3ddb119 tags:

		Learning a linear relationship for the equation $V = k_{scaling} * speed$ for motors. This is useful for `SpeedsMultirotorEnv` which takes speed signals as the input. This constant converts speeds to applied voltages. The default value in`MotorParams` is 1, meaning the actions are voltage signals.

		%% Cell type:code id:42ebfb97 tags:

		``` python
		from multirotor.helpers import learn_speed_voltage_scaling

		def make_motor_fn(params, sp):
		from copy import deepcopy
		params = deepcopy(params)
		params.speed_voltage_scaling = 1.
		def motor_step(signal):
		m = Motor(params, sp)
		for i in range(100):
		s = m.step(signal)
		return s
		return motor_step

		print('Voltage = %.5f * speed' % (learn_speed_voltage_scaling(make_motor_fn(mp, sp))))
		```

		%% Cell type:markdown id:6fb2f5c9 tags:
		%% Cell type:markdown id:8d42fff7 tags:

		#### Propeller

		%% Cell type:markdown id:6fb2f5c9 tags:

		The propeller can use a numerically solved thrust relationship, where thrust depends on airspeed. Or the easier option of using thrust coefficient is available.

		%% Cell type:code id:1669687b tags:

		``` python
		%matplotlib inline
		# Plot propeller speed by numerically solving the thrust equation,
		# if accurate propeller measurements are given in params
		pp_ = deepcopy(pp)
		pp_.use_thrust_constant = False # Set to true to just use k_thrust
		prop = Propeller(pp_, sp)
		plt.figure(figsize=(8,8))
		speeds = np.linspace(0, 600, num=100)
		for a in np.linspace(0, 10, 10, endpoint=False):
		thrusts = []
		for s in speeds:
		thrusts.append(prop.thrust(s, np.asarray([0, 0, a])))
		plt.plot(speeds, thrusts, label='%.1f m/s' % a)
		plt.xlabel('Speed rad/s')
		plt.ylabel('Thrust /N')
		plt.title('Thrust with airspeed')
		plt.legend(ncol=2)
		```

		%% Cell type:markdown id:e7bb489d tags:

		#### Vehicle

		%% Cell type:markdown id:2936761d tags:

		Create a `Multirotor` object, given `VehicleParams` and `SimulationParams`

		%% Cell type:code id:ecc23e14 tags:

		``` python
		# Combine propeller/motor/vehicle to get vehicle.
		# Take off simulation
		m = Multirotor(vp, sp)
		log = DataLog(vehicle=m) # convenient logging class
		m.reset()
		m.state *= 0 # set to zero, reset() sets random values
		action = m.allocate_control(
		action = m.allocate_control( # In this case action is allocated speed signals
		thrust=m.weight * 1.1,
		torques=np.asarray([0, 0, 0])
		)
		for i in range(500):
		m.step_speeds(action)
		log.log()
		log.done_logging()
		plt.plot(log.z)
		```

		%% Cell type:markdown id:90475ed5 tags:

		### Gym Environment

		%% Cell type:code id:857201b9 tags:

		``` python
		# this env takes the vector of [force_x, force_y, force_z, torque_x, torque_y, torque_z] to move
		# the multirotor
		env = DynamicsMultirotorEnv(Multirotor(vp, sp), max_rads=600)
		env.reset()
		log = DataLog(vehicle=env.vehicle)
		for _ in range(100):
		env.step(np.asarray([0,0,env.vehicle.weight * 1.2, 0,0,0]))
		log.log()
		log.done_logging()
		plt.plot(log.z)
		```

		%% Cell type:code id:014b7077 tags:

		``` python
		# this env takes the vector of speed signals to move
		# the multirotor
		env = SpeedsMultirotorEnv(Multirotor(vp, sp))
		env.reset()
		log = DataLog(vehicle=env.vehicle)
		for _ in range(100):
		env.step(np.ones(8) * 400)
		log.log()
		log.done_logging()
		plt.plot(log.z)
		```

		%% Cell type:markdown id:2c6535f4 tags:

		### PID Controller

		%% Cell type:markdown id:37d265bb tags:

		This section explains how a PID controller is constructed. This is a cascaded PID architecture. See `Controller` docs
		for more details.

		%% Cell type:code id:33984887 tags:

		``` python
		# From PID parameters file
		def get_controller(m: Multirotor, max_velocity=5., max_acceleration=3.):
		assert m.simulation.dt <= 0.1, 'Simulation time step too large.'
		pos = PosController(
		1.0, 0., 0., 1., vehicle=m,
		max_velocity=max_velocity, max_acceleration=max_acceleration,
		square_root_scaling=True, leashing=True
		square_root_scaling=False, leashing=False
		)
		vel = VelController(
		2.0, 1.0, 0.5,
		max_err_i=max_acceleration,
		max_tilt=np.pi/12,
		vehicle=m)
		att = AttController(
		[2.6875, 4.5, 4.5],
		0, 0.,
		max_err_i=1.,
		vehicle=m)
		rat = RateController(
		[4., 4., 4.],
		0, 0,
		max_err_i=0.5,
		max_acceleration=1.,
		vehicle=m)
		alt = AltController(
		1, 0, 0,
		max_err_i=1, vehicle=m,
		max_velocity=max_velocity)
		alt_rate = AltRateController(
		5, 0, 0,
		max_err_i=1, vehicle=m)
		ctrl = Controller(
		pos, vel, att, rat, alt, alt_rate,
		period_p=0.1, period_a=0.01, period_z=0.1
		)
		return ctrl
		```

		%% Cell type:code id:d21d73fe tags:

		``` python
		%matplotlib inline
		m = Multirotor(vp, sp)
		ctrl = get_controller(m)
		log = DataLog(vehicle=m, controller=ctrl)
		for i in range(500):
		action = ctrl.step((0.01,0.1,1,0))
		# no allocation or motor simulation, for which we first need to
		# m.step_speeds(m.allocate_control(action[0], action[3:])
		# Instead, requested dynamics are fulfilled:
		dynamics = np.zeros(6, m.dtype)
		dynamics[2] = action[0]
		dynamics[3:] = action[1:]
		m.step_dynamics(dynamics)
		log.log()
		log.done_logging()

		plt.plot(log.actions[:,0], ls=':', label='thrust')
		lines = plt.gca().lines
		plt.twinx()
		for s, axis in zip(log.actions.T[1:], ('x','y','z')):
		plt.plot(s, label=axis + '-torque')
		plt.legend(handles=plt.gca().lines + lines)
		```

		%% Cell type:markdown id:cfbc9c25 tags:

		#### Attitude Angle Controller

		%% Cell type:code id:f012e1f8 tags:

		``` python
		m = Multirotor(vp, sp)
		fz = m.weight
		att = get_controller(m).ctrl_a
		log = DataLog(vehicle=m, controller=att, other_vars=('err',))
		ctrl = get_controller(m)
		att = ctrl.ctrl_a
		log = DataLog(vehicle=m, controller=ctrl, other_vars=('err',))
		for i in range(5000):
		ref = np.asarray([np.pi/18, 0, 0])
		# action is prescribed euler rate
		action = att.step(ref, m.orientation)
		action = att.step(ref, m.orientation, dt=sp.dt)
		# action = np.clip(action, a_min=-0.1, a_max=0.1)
		m.step_dynamics(np.asarray([0, 0, 0, *action]))
		log.log(err=att.err_p[0])
		log._actions[-1] = action
		log.done_logging()

		plt.plot(log.roll * 180 / np.pi)
		plt.twinx()
		plt.plot(log.actions[:,0], ls=':', label='Rate rad/s')
		```

		%% Cell type:markdown id:fdbd88c1 tags:

		#### Attitude Rate Controller

		%% Cell type:code id:9cc3b317 tags:

		``` python
		m = Multirotor(vp, sp)
		fz = m.weight
		ctrl = get_controller(m)
		rat = ctrl.ctrl_r
		att = ctrl.ctrl_a
		log = DataLog(vehicle=m, controller=rat, other_vars=('err',))
		log = DataLog(vehicle=m, controller=ctrl, other_vars=('err',))
		for i in range(200):
		ref = np.asarray([np.pi/18, np.pi/12, 0])
		rate = att.step(ref, m.orientation, m.simulation.dt)
		torque = rat.step(rate, m.euler_rate, m.simulation.dt)
		action = np.clip(torque, a_min=-0.1, a_max=0.1)
		m.step_dynamics(np.asarray([0, 0, 0, *action]))
		log.log(err=rat.err_p[0])
		log._actions[-1] = action
		log.done_logging()

		plt.plot(log.roll * 180 / np.pi, c='r', label='roll')
		plt.plot(log.pitch * 180 / np.pi, c='g', label='pitch')
		plt.plot(log.yaw * 180 / np.pi, c='b', label='yaw')
		plt.ylabel('Orientation /deg')
		plt.legend()
		plt.twinx()
		plt.plot(log.actions[:,0], ls=':', c='r')
		plt.plot(log.actions[:,1], ls=':', c='g')
		plt.plot(log.actions[:,2], ls=':', c='b')
		plt.ylabel('Torque / Nm')
		plt.title('Ref orientation' + str(ref))
		```

		%% Cell type:code id:a7ad3618 tags:

		``` python
		m.params.inertia_matrix
		```

		%% Cell type:code id:f996d427 tags:

		``` python
		att.err
		```

		%% Cell type:code id:8b3b2181 tags:

		``` python
		rat.err
		```

		%% Cell type:code id:d5b77d12 tags:

		``` python
		action
		```

		%% Cell type:code id:3b9a3eba tags:

		``` python
		euler_to_angular_rate(
		m.euler_rate,
		m.orientation
		)
		```

		%% Cell type:markdown id:6370aa80 tags:

		#### Altitude Controller

		%% Cell type:code id:5fcd0d24 tags:

		``` python
		m = Multirotor(vp, sp)
		ctrl = get_controller(m)
		alt = ctrl.ctrl_z
		alt_rate = ctrl.ctrl_vz
		log = DataLog(vehicle=m, other_vars=('thrust',))
		for i in range(5000):
		ref = np.asarray([1.])
		rate = alt.step(ref, m.position[2:], dt=0.1)
		action = alt_rate.step(rate, m.inertial_velocity[2:], dt=0.1)
		action = np.clip(action, a_min=-2m.weight, a_max=2m.weight)
		m.step_dynamics(np.asarray([0, 0, action[0], 0,0,0]))
		log.log(thrust=action)
		#log._actions[-1] = action
		log.done_logging()

		plt.plot(log.thrust.squeeze())
		l = plt.plot(log.thrust.squeeze(), label='Thrust')
		plt.twinx()
		plt.plot(log.z, ls=':')
		plt.plot(log.z, ls=':', label='Altitude /m')
		plt.legend(handles=l+plt.gca().lines)
		```

		%% Cell type:markdown id:f4278c17 tags:

		#### Position Controller

		%% Cell type:code id:91ae8919 tags:

		``` python
		m = Multirotor(vp, sp)
		ctrl = get_controller(m)
		pos = ctrl.ctrl_p
		vel = ctrl.ctrl_v
		rat = ctrl.ctrl_r
		att = ctrl.ctrl_a
		log = DataLog(vehicle=m, other_vars=('err', 'att_actions'))
		for i in range(5000):
		log = DataLog(vehicle=m, other_vars=('err', 'torques'))
		for i in range(100):
		ref = np.asarray([1.,0.])

		# converting position -> velocity -> angles
		velocity = pos.step(ref, m.position[:2], dt=0.1)
		angles = vel.step(velocity, m.velocity[:2], dt=0.1)[::-1]
		# attitude controller operates at higher frequency
		rate = att.step(np.asarray([*angles, 0]), m.orientation, dt=0.01)
		action = rat.step(rate, m.euler_rate, dt=0.01)

		# clipping torques to prevent over-reactions
		action = np.clip(action, a_min=-0.1, a_max=0.1)
		m.step_dynamics(np.asarray([0, 0, m.weight, *action]))
		log.log(err=pos.err_p[0], att_actions=action)
		log.log(err=pos.err[0], torques=action)
		log.done_logging()

		plt.plot(log.position[:,0])
		plt.plot(log.err)
		# plt.plot(log.position[:,1])
		plt.plot(log.x, label='x')
		plt.plot(log.err, label='x-err')
		plt.ylabel('x /m')
		l = plt.gca().lines
		plt.twinx()
		plt.plot(log.att_actions[:,0] * 180 / np.pi, ls=':')
		plt.plot(log.pitch * 180 / np.pi, ls='-.')
		plt.plot(log.torques[:,1], ls=':', label='y-torque', c='c')
		plt.plot(log.pitch * 180 / np.pi, ls='-.', label='Pitch', c='m')
		plt.legend(handles=plt.gca().lines+l)
		# plt.plot(log.actions[:,0] * 180 / np.pi, ls=':')
		```

		%% Cell type:markdown id:565cf2c9 tags:

		#### Parameter search #TODO
		### Parameter search

		%% Cell type:markdown id:dc8f3d0d tags:

		Using `optuna` to search over the space of PID controller parameters.

		%% Cell type:code id:11013f70 tags:
		%% Cell type:code id:607c8b66 tags:

		``` python
		# TODO
		from multirotor.optimize import optimize, DEFAULTS
		print(DEFAULTS)
		```

		%% Cell type:code id:f3b82f8d tags:

		``` python
		# search over parameter space
		study = optimize(vp, sp, ntrials=100)
		```

		%% Cell type:code id:c1a374a6 tags:

		``` python
		# apply best parameters from study to controller, and run a simulation
		from multirotor.optimize import run_sim, apply_params

		env = DynamicsMultirotorEnv(Multirotor(vp, sp))
		traj = Trajectory(env.vehicle, [[0,0,0]], proximity=1)
		ctrl = get_controller(env.vehicle)
		ctrl.set_params(**apply_params(None, params=study.best_params))

		env.reset()
		ctrl.reset()
		log = run_sim(env, traj, ctrl)
		plot_datalog(log)
		```

		%% Cell type:markdown id:9eb34672 tags:

		### Simulation

		%% Cell type:markdown id:e17df085 tags:

		Combining `Multiotor` and `Controller` to run a simulation. First, defining waypoints:

		%% Cell type:code id:b60ef8d3 tags:

		``` python
		# NASA flight test
		# wp = np.asarray([
		# [0.0, 0.0, 30.0],
		# [164.0146725649829, -0.019177722744643688, 30.0],
		# [165.6418055187678, 111.5351051245816, 30.0],
		# [127.3337449710234, 165.73576059611514, 30.0],
		# [-187.28170707810204, 170.33217775914818, 45.0],
		# [-192.03130502498243, 106.30660058604553, 45.0],
		# [115.89920266153058, 100.8644210617058, 30.0],
		# [114.81859536317643, 26.80923518165946, 30.0],
		# [-21.459931490011513, 32.60508110653609, 30.0]
		# ])
		# wp = np.asarray([
		# [0.0, 0.0, 100.0],
		# [300, 0.0, 100.0],
		# [300, 200, 100.0],
		# [250, 300, 100.0],
		# [-350, 340, 120.0],
		# [-350, 250, 120.0],
		# [300, 250, 100.0],
		# [300, 150, 100.0],
		# [-50, 100, 100.0]
		# ])

		wp = np.asarray([
		[0,10,0],
		[10,10,0],
		[10,0,0],
		[0,0,0]
		])
		```

		%% Cell type:markdown id:b43dae3d tags:

		Then, defining a disturbance (for example, wind). The disturabance function takes time, `Multirotor`, and returns the forces in the body frame of the vehicle.

		%% Cell type:code id:f5f52df4 tags:

		``` python
		def wind(t, m, nominal=False):
		if nominal:
		return np.zeros(3)
		def wind(t, m):
		w_inertial = np.asarray([5 * np.sin(t * 2 * np.pi / 4000), 0, 0])
		dcm = direction_cosine_matrix(*m.orientation)
		return inertial_to_body(w_inertial, dcm)
		```

		%% Cell type:code id:a98de44f tags:

		``` python
		def run_sim(env, traj, steps=60_000, disturbance=None):
		ctrl = get_controller(env.vehicle, max_velocity=3.)

		log = DataLog(env.vehicle, ctrl,
		other_vars=('currents', 'voltages'))
		disturb_force, disturb_torque = 0., 0
		for i, (pos, feed_forward_vel) in tqdm(
		enumerate(traj), leave=False, total=steps
		):
		if i==steps: break
		# Generate reference for controller
		ref = np.asarray([*pos, 0.])
		# Get prescribed dynamics for system as thrust and torques
		dynamics = ctrl.step(ref, feed_forward_velocity=feed_forward_vel)
		thrust, torques = dynamics[0], dynamics[1:]
		# Allocate control: Convert dynamics into motor rad/s
		action = env.vehicle.allocate_control(thrust, torques)
		# get any disturbances
		if disturbance is not None:
		disturb_force, disturb_torque = disturbance(i, env.vehicle)
		# Send speeds to environment
		state, *_ = env.step(
		action, disturb_forces=disturb_force, disturb_torques=disturb_torque
		)
		alloc_errs = np.asarray([thrust, torques]) - env.vehicle.alloc @ action*2

		log.log(currents=[p.motor.current for p in env.vehicle.propellers],
		voltages=[p.motor.voltage for p in env.vehicle.propellers])

		if np.any(np.abs(env.vehicle.orientation[:2]) > np.pi/6): break

		log.done_logging()
		return log
		```

		%% Cell type:code id:62689b31 tags:

		``` python
		env = SpeedsMultirotorEnv(vehicle=Multirotor(vp, sp)) # step() takes speeds

		# waypoints = [[0,50,2], [50,50,2], [50,0,2], [0,0,2]]
		waypoints = wp
		traj = Trajectory(env.vehicle, waypoints, proximity=2, resolution=None)
		env = SpeedsMultirotorEnv(vehicle=Multirotor(vp, sp)) # step() takes speed signals
		traj = Trajectory(env.vehicle, wp, proximity=2, resolution=10)
		log = run_sim(env, traj, steps=60_000, disturbance=None)
		```

		%% Cell type:code id:2d1b0058 tags:

		``` python
		# Currents
		plt.plot(log.currents, ls=':')
		plt.ylabel('Motor current /A')
		plt.xlabel('Time /ms')
		plt.title('Individual motor currents')
		```

		%% Cell type:code id:f0aa49e1 tags:

		``` python
		# Voltages
		plt.plot(log.voltages, ls=':')
		plt.ylim(0, 30)
		plt.ylabel('Motor voltage /A')
		plt.xlabel('Time /ms')
		plt.title('Voltages')
		```

		%% Cell type:code id:665f4799 tags:

		``` python
		# PLot positions, velocities, prescribed dynamics
		plot_datalog(log)
		```

		%% Cell type:code id:d9d99572 tags:

		``` python
		# 3D plot of trajectory
		%matplotlib notebook
		fig = plt.figure()
		xlim = ylim = zlim = (np.min(log.position), np.max(log.position))
		ax = fig.add_subplot(projection='3d', xlim=xlim, ylim=ylim, zlim=zlim)
		ax.set_xlabel('x')
		ax.set_ylabel('y')
		ax.set_zlabel('z')
		ax.plot(log.x, log.y, log.z)
		```

multirotor/env.py

+87 −13

Changes for multirotor/env.py: 87 added lines, 13 removed lines.
Original line number	Diff line number	Diff line
		@@ -2,7 +2,7 @@
		This module defines OpenAI Gym compatible classes based on the Multirotor class.
		"""

		from typing import Tuple
		from typing import Tuple, List, Union
		import numpy as np
		import gym

		@@ -11,9 +11,23 @@ from .helpers import find_nominal_speed


		class BaseMultirotorEnv(gym.Env):
		"""
		The base environment class, defining the episode, and reward function.
		"""

		max_angle = np.pi/12
		"""The max tilt angle in radians."""
		proximity = 0.5
		"""Distance from the waypoint at which to consider it has been reached."""
		period = 10
		"""Maximum duration of the episode (seconds)."""
		bounding_box = 20
		"""Size of the cube in which the vehicle can fly, centered at origin."""
		motion_reward_scaling = bounding_box / 2
		bonus = bounding_box * 20

		def __init__(self, vehicle: Multirotor=None) -> None:

		def __init__(self, vehicle: Multirotor=None, seed: int=None) -> None:
		# pos, vel, att, ang vel
		self.observation_space = gym.spaces.Box(
		low=-np.inf,
		@@ -22,21 +36,77 @@ class BaseMultirotorEnv(gym.Env):
		shape=(12,)
		)
		self.vehicle = vehicle
		self.seed(seed=seed, _seed_with_none=True)


		def seed(self, seed: int=None, _seed_with_none: bool=False) -> List[Union[int,tuple]]:
		if isinstance(seed, (int, float)):
		self.random = np.random.RandomState(seed)
		self._seeds = (seed,)
		elif seed is None and _seed_with_none:
		self.random = np.random.RandomState()
		self._seeds = (self.random.get_state())
		elif isinstance(seed, tuple):
		self.random = np.random.RandomState()
		self.random.set_state(seed)
		self._seeds = (self.random.get_state(),)
		return list(self._seeds)


		@property
		def state(self) -> np.ndarray:
		return self.vehicle.state
		@state.setter
		def state(self, x: np.ndarray):
		self.vehicle.state = np.asarray(x, self.vehicle.dtype)


		def reset(self, x: np.ndarray=None) -> np.ndarray:
		"""
		Reset the vehicle to a random initial position.

		Parameters
		----------
		x : np.ndarray, optional
		A state to set the vehicle to, by default None

		def reset(self):
		Returns
		-------
		np.ndarray
		The state vector of the vehicle.
		"""
		if self.vehicle is not None:
		self.vehicle.reset()
		position = (self.random.rand(3) - 0.5) * self.bounding_box * 0.75
		position[(0<position) & (position<self.proximity)] = self.proximity
		position[(-self.proximity<position) & (position<0)] = -self.proximity
		self.vehicle.state[:3] = position
		if x is not None:
		self.vehicle.state = np.asarray(x, self.vehicle.dtype)
		# needed by reward() to calculate deviation from straight line
		self._des_unit_vec = - self.state[:3] / np.linalg.norm(self.state[:3])
		return self.state


		def reward(self, state, action, nstate):
		raise NotImplementedError
		def reward(self, state: np.ndarray, action: np.ndarray, nstate: np.ndarray) -> float:
		dist = np.linalg.norm(nstate[:3])
		self._reached = dist <= self.proximity
		self._outofbounds = np.any(np.abs(state[:3]) > self.bounding_box / 2)
		self._outoftime = self.vehicle.t >= self.period
		self._tipped = np.any(np.abs(state[6:9]) > self.max_angle)
		self._done = self._outoftime or self._outofbounds or self._reached or self._tipped
		delta_pos = (nstate[:3] - state[:3])
		advance = np.linalg.norm(delta_pos)
		cross = np.linalg.norm(np.cross(delta_pos, self._des_unit_vec))
		delta_turn = np.abs(nstate[8]) - np.abs(state[8])
		reward = ((advance - cross - delta_turn) * self.motion_reward_scaling) - self.vehicle.simulation.dt
		if self._reached:
		reward += self.bonus
		elif self._tipped or self._outofbounds:
		reward -= self.bonus
		elif self._outoftime:
		reward -= (dist / self.bounding_box) * self.bonus
		return reward


		def step(self, action: np.ndarray) -> tuple[np.ndarray, float, bool, dict]:
		@@ -78,7 +148,7 @@ class DynamicsMultirotorEnv(BaseMultirotorEnv):
		def step(
		self, action: np.ndarray, disturb_forces: np.ndarray=0.,
		disturb_torques: np.ndarray=0.
		) -> Tuple[np.ndarray, float, bool, bool, dict]:
		) -> Tuple[np.ndarray, float, bool, dict]:
		"""
		Step environment by providing dynamics acting in local frame.

		@@ -93,7 +163,7 @@ class DynamicsMultirotorEnv(BaseMultirotorEnv):

		Returns
		-------
		Tuple[np.ndarray, None, None, None]
		Tuple[np.ndarray, float, bool, dict]
		The state and other environment variables.
		"""
		if self.allocate:
		@@ -103,8 +173,10 @@ class DynamicsMultirotorEnv(BaseMultirotorEnv):
		action = np.concatenate((forces, torques))
		action[:3] += disturb_forces
		action[3:] += disturb_torques
		self.vehicle.step_dynamics(u=action)
		return self.state, 0., False, False, {}
		state = self.state
		nstate = self.vehicle.step_dynamics(u=action)
		reward = self.reward(state, action, nstate)
		return nstate, reward, self._done, {}



		@@ -150,7 +222,7 @@ class SpeedsMultirotorEnv(BaseMultirotorEnv):
		def step(
		self, action: np.ndarray, disturb_forces: np.ndarray=0.,
		disturb_torques: np.ndarray=0.
		) -> Tuple[np.ndarray, float, bool, bool, dict]:
		) -> Tuple[np.ndarray, float, bool, dict]:
		"""
		Step environment by providing speed signal.

		@@ -165,12 +237,14 @@ class SpeedsMultirotorEnv(BaseMultirotorEnv):

		Returns
		-------
		Tuple[np.ndarray, None, None, None]
		Tuple[np.ndarray, float, bool, dict]
		The state and other environment variables.
		"""
		self.vehicle.step_speeds(
		state = self.state
		nstate = self.vehicle.step_speeds(
		u=action,
		disturb_forces=disturb_forces,
		disturb_torques=disturb_torques
		)
		return self.state, 0., False, False, {}
		No newline at end of file
		reward = self.reward(state, action, nstate)
		return nstate, reward, self._done, {}
		No newline at end of file

multirotor/helpers.py

+3 −0

Changes for multirotor/helpers.py: 3 added lines, 0 removed lines.
Original line number	Diff line number	Diff line
		@@ -266,6 +266,9 @@ def get_vehicle_ability(
		# t = i a
		ang_acc = torques / I

		# TODO: max angular velocity such that can accelerate to and decelerate from it
		# to 0

		res = dict(
		max_acc_xy=max_acc_xy,
		max_acc_z=max_acc_z,

multirotor/optimize.py

0 → 100644

+423 −0

File added.

Preview size limit exceeded, changes collapsed.

setup.cfg

+2 −1

Changes for setup.cfg: 2 added lines, 1 removed line.
Original line number	Diff line number	Diff line
		# See: https://setuptools.pypa.io/en/latest/userguide/quickstart.html
		[metadata]
		name = multirotor
		version = 0.3.1
		version = 0.4.0
		description = Simulation testbed for multirotor vehicles.
		long_description = file: README.md
		long_description_content_type = text/markdown
		@@ -15,6 +15,7 @@ install_requires =
		numba
		matplotlib
		gym
		optuna
		# https://pip.pypa.io/en/latest/topics/vcs-support/
		# can't upload to pypi with a git link
		# pyscurve @ git+https://github.com/hazrmard/py-scurve.git@v1.0.2

Admin message