Commit cf843db1 authored by Ibrahim's avatar Ibrahim
Browse files

Current calculations;

Added Motor.current_average() and Multirotor.current_average() methods for duty-cycle adjusted currents.
Refactored plotting function for DataLog in visualize.py
parent 966e85d6
%% 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
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, angular_to_euler_rate
from multirotor.env import SpeedsMultirotorEnv as LocalOctorotor
from multirotor.trajectories import Trajectory, GuidedTrajectory
```
%% 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:1ba2a2fe tags:
``` python
# Tarot T18 params
bp = BatteryParams(max_voltage=22.2)
mp = MotorParams(
moment_of_inertia=5e-5,
# resistance=0.27,
# resistance=0.27,
resistance=0.081,
k_emf=0.0265,
k_motor=0.0932,
speed_voltage_scaling=0.0347
# 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, -2*np.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.001, g=9.81)
```
%% Cell type:markdown id:17f3b34d tags:
### Multirotor
%% 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 signal in [2, 4, 6, 8, 10, 12, 14, 16, 18, 20]:
speeds = []
motor.reset()
for i in range(200):
speeds.append(motor.step(signal))
plt.plot(speeds, label='%dV' % signal)
plt.legend(ncol=2)
plt.ylabel('Speed rad/s')
plt.xlabel('Time /ms')
```
%% 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:
#### Propeller
%% 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: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(
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:2c6535f4 tags:
### PID Controller
%% Cell type:code id:33984887 tags:
``` python
# From PID parameters file
def get_controller(m: Multirotor):
pos = PosController(
1.0, 0., 0., 1., dt=1e-3, vehicle=m)
1.0, 0., 0., 1., dt=m.simulation.dt, vehicle=m)
vel = VelController(
2.0, 1.0, 0.5, 1000., dt=1e-3, vehicle=m)
2.0, 1.0, 0.5, 1000., dt=m.simulation.dt, vehicle=m)
att = AttController(
[2.6875, 4.5, 4.5],
0, 0.,
1., dt=1e-3, vehicle=m)
1., dt=m.simulation.dt, vehicle=m)
rat = RateController(
[0.1655, 0.1655, 0.5],
[0.135, 0.135, 0.018],
[0.01234, 0.01234, 0.],
[0.5,0.5,0.5], dt=1e-3, vehicle=m)
[0.5,0.5,0.5], dt=m.simulation.dt, vehicle=m)
alt = AltController(
1, 0, 0,
1, dt=1e-3, vehicle=m)
1, dt=m.simulation.dt, vehicle=m)
alt_rate = AltRateController(
5, 0, 0,
1, dt=1e-3, vehicle=m)
1, dt=m.simulation.dt, vehicle=m)
ctrl = Controller(
pos, vel, att, rat, alt, alt_rate
)
return ctrl
```
%% Cell type:code id:d21d73fe tags:
``` python
%matplotlib inline
m = Multirotor(vp, sp)
ctrl = get_controller(m)
log = DataLog(controller=ctrl)
for i in range(100):
action = ctrl.step((1,1,1,0))
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',))
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 = 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',))
for i in range(5000):
ref = np.asarray([np.pi/18, 0, 0])
rate = att.step(ref, m.orientation)
action = rat.step(rate, m.euler_rate)
action = np.clip(action, 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)
plt.twinx()
plt.plot(log.actions[:,0], ls=':')
```
%% 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, controller=alt, other_vars=('thrust',))
for i in range(5000):
ref = np.asarray([1.])
rate = alt.step(ref, m.position[2:])
action = alt_rate.step(rate, m.world_velocity[2:])
action = np.clip(action, a_min=-2*m.weight, a_max=2*m.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.actions.squeeze())
plt.twinx()
plt.plot(log.z, ls=':')
```
%% 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, controller=pos, other_vars=('err', 'att_actions'))
for i in range(5000):
ref = np.asarray([1.,0.])
velocity = pos.step(ref, m.position[:2])
angles = vel.step(velocity, m.velocity[:2])[::-1]
rate = att.step(np.asarray([*angles, 0]), m.orientation)
action = rat.step(rate, m.euler_rate)
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.done_logging()
plt.plot(log.position[:,0])
plt.plot(log.err)
# plt.plot(log.position[:,1])
plt.twinx()
plt.plot(log.actions[:,0] * 180 / np.pi, ls=':')
plt.plot(log.pitch * 180 / np.pi, ls='-.')
# plt.plot(log.actions[:,0] * 180 / np.pi, ls=':')
```
%% Cell type:markdown id:9eb34672 tags:
### Simulation
%% Cell type:code id:f5f52df4 tags:
``` python
# 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)[:2]
wind = lambda t, m: [0, 0]
def wind(t, m, nominal=False):
if nominal:
return np.zeros(3)
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:d60f1ca5 tags:
``` python
from pyscurve import plot_trajectory
```
%% Cell type:code id:a98de44f tags:
``` python
m = Multirotor(vp, sp)
env = LocalOctorotor(vehicle=m)
# for _ in range(5000):
# state, *_ = env.step(np.asarray([0., 0., m.weight * 1.01, 0., 0., 0.]))
env = LocalOctorotor(vehicle=Multirotor(vp, sp))
waypoints = [[0,50,2], [50,50,2], [50,0,2], [25,-25,2]]
traj = GuidedTrajectory(env.vehicle, waypoints, proximity=2)
# traj = Trajectory(env.vehicle, waypoints, proximity=2, resolution=None)
ctrl = get_controller(m)
ctrl = get_controller(env.vehicle)
errs = SimpleNamespace()
errs.pos = SimpleNamespace()
errs.pos.p, errs.pos.i, errs.pos.d = [], [], []
errs.att = SimpleNamespace()
errs.att.p, errs.att.i, errs.att.d = [], [], []
log = DataLog(env.vehicle, ctrl,
other_vars=('speeds','target', 'alloc_errs',
'ctrl_p', 'leash', 'err_p', 'ff_vel',
'currents', 'voltages'))
for i, (pos, feed_forward_vel) in tqdm(enumerate(traj), leave=False, total=60000):
if i==30000: break
if i==1000: 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 = m.allocate_control(thrust, torques)
action = env.vehicle.allocate_control(thrust, torques)
# get any disturbances
wind_force = wind(i, env.vehicle, nominal=False)
# Send speeds to environment
state, *_ = env.step(
action, disturb_forces=0, disturb_torques=0
action, disturb_forces=wind_force, disturb_torques=0
)
for (c, e) in zip((ctrl.ctrl_p, ctrl.ctrl_a), (errs.pos, errs.att)):
e.p.append(c.err_p)
e.i.append(c.err_i)
e.d.append(c.err_d)
alloc_errs = np.asarray([thrust, *torques]) - m.alloc @ action**2
alloc_errs = np.asarray([thrust, *torques]) - env.vehicle.alloc @ action**2
log.log(speeds=action, target=pos, alloc_errs=alloc_errs,
ctrl_p=ctrl.ctrl_p.action, leash=ctrl.ctrl_p.leash,
err_p=ctrl.ctrl_p.err, ff_vel=feed_forward_velocity,
currents=[p.motor.current for p in m.propellers],
voltages=[p.motor.voltage for p in m.propellers])
err_p=ctrl.ctrl_p.err, ff_vel=feed_forward_vel,
currents=[p.motor.current_average(env.vehicle.battery.params.max_voltage) for p in env.vehicle.propellers],
voltages=[p.motor.voltage for p in env.vehicle.propellers])
if np.any(np.abs(m.orientation[:2]) > np.pi/6): break
if np.any(np.abs(env.vehicle.orientation[:2]) > np.pi/6): break
log.done_logging()
currents = np.asarray(currents)
```
%% Cell type:code id:2d1b0058 tags:
``` python
plt.plot(log.currents, ls=':')
plt.ylim(0, 30)
plt.ylabel('Motor current /A')
plt.xlabel('Time /ms')
plt.title('Rectangular trajectory')
plt.title('Individual motor currents')
```
%% Cell type:code id:f0aa49e1 tags:
``` python
plt.plot(log.voltages, ls=':')
plt.ylim(0, 30)
plt.ylabel('Motor voltage /A')
plt.xlabel('Time /ms')
plt.title('Rectangular trajectory')
plt.title('Voltages')
```
%% Cell type:code id:5d64dbce tags:
``` python
plot_trajectory(traj.trajs[-1], dt=0.1)
```
%% Cell type:code id:81a8043e tags:
``` python
%matplotlib inline
plt.figure(figsize=(21,10.5))
plot_grid = (3,3)
plt.subplot(*plot_grid,1)
n = len(log)
plt.plot(log.x, label='x', c='r')
plt.plot(log.target[:, 0], c='r', ls=':')
plt.plot(log.y, label='y', c='g')
plt.plot(log.target[:, 1], c='g', ls=':')
plt.plot(log.z, label='z', c='b')
lines = plt.gca().lines[::2]
plt.ylabel('Position /m')
plt.twinx()
plt.plot(log.roll * (180 / np.pi), label='roll', c='c', ls=':')
plt.plot(log.pitch * (180 / np.pi), label='pitch', c='m', ls=':')
plt.plot(log.yaw * (180 / np.pi), label='yaw', c='y', ls=':')
plt.ylabel('Orientation /deg')
plt.legend(handles=plt.gca().lines + lines, ncol=2)
plt.title('Position and Orientation')
plt.subplot(*plot_grid,2)
for i in range(log.speeds.shape[1]):
l, = plt.plot(log.speeds[:,i], label='prop %d' % i)
# plt.plot(speeds[:,i], c=l.get_c())
lines = plt.gca().lines
plt.legend(handles=lines, ncol=2)
plt.title('Motor speeds /RPM')
plt.subplot(*plot_grid,3)
v_world = np.zeros_like(log.velocity)
for i, (v, o) in enumerate(zip(log.velocity, log.orientation)):
dcm = direction_cosine_matrix(*o)
v_world[i] = body_to_inertial(v, dcm)
for i, c, a in zip(range(3), 'rgb', 'xyz'):
plt.plot(v_world[:,i], label='Velocity %s' % a, c=c)
# plt.plot(velocities[:,i], label='Velocity %s' % a, c=c)
plt.legend()
plt.title('Velocities')
plt.subplot(*plot_grid,4)
plt.title('Controller allocated dynamics')
l = plt.plot(log.actions[:,0], label='Ctrl Thrust')
plt.ylabel('Force /N')
plt.twinx()
for i, c, a in zip(range(3), 'rgb', 'xyz'):
plt.plot(log.actions[:,1+i], label='Ctrl Torque %s' % a, c=c)
plt.ylabel('Torque /Nm')
plt.legend(handles=plt.gca().lines + l, ncol=2)
plt.subplot(*plot_grid,5)
lines = plt.plot(log.alloc_errs[:, 0], label='Thrust err', c='b')
plt.ylabel('Thrust /N')
plt.twinx()
plt.plot(log.alloc_errs[:, 1], label='Torque x err', ls=':')
plt.plot(log.alloc_errs[:, 2], label='Torque y err', ls=':')
plt.plot(log.alloc_errs[:, 3], label='Torque z err', ls=':')
plt.legend(handles = plt.gca().lines + lines, ncol=2)
plt.ylabel('Torque /Nm')
plt.title('Allocation Errors')
plt.subplot(*plot_grid,6)
plt.plot(log.target[:,0], log.target[:,1], label='Prescribed traj')
plt.plot(log.x, log.y, label='Actual traj', ls=':')
plt.gca().set_aspect('equal', 'box')
plt.title('XY positions /m')
plt.legend()
plt.tight_layout()
```
%% Cell type:code id:c17d8b15 tags:
``` python
%matplotlib inline
for e in (errs.pos, errs.att):
e.p = np.asarray(e.p)
e.i = np.asarray(e.i)
e.d = np.asarray(e.d)
lines = []
plt.figure(figsize=(21,24))
plt.subplot(6,1,1)
plt.plot(errs.pos.p[:,0], label='Pos-x P', c='r', ls='-')
plt.plot(errs.pos.p[:,1], label='Pos-y P', c='g', ls='-')
lines += plt.gca().lines
plt.title('Position P errors')
plt.subplot(6,1, 2)
plt.plot(errs.pos.i[:, 0], label='Pos-x I', c='r', ls=':')
plt.plot(errs.pos.i[:, 1], label='Pos-y I', c='g', ls=':')
lines += plt.gca().lines
plt.title('Position I errors')
plt.subplot(6,1, 3)
plt.plot(errs.pos.d[:, 0], label='Pos-x D', c='r', ls='-.')
plt.plot(errs.pos.d[:, 1], label='Pos-y D', c='g', ls='-.')
lines += plt.gca().lines
plt.legend(handles=lines)
plt.title('Position D errors')
lines = []
plt.subplot(6,1,4)
plt.plot(errs.att.p[:,0], label='Att-x P', c='r', ls='-')
plt.plot(errs.att.p[:,1], label='Att-y P', c='g', ls='-')
lines += plt.gca().lines
plt.title('Attitude P errors')
plt.subplot(6,1, 5)
plt.plot(errs.att.i[:, 0], label='Att-x I', c='r', ls=':')
plt.plot(errs.att.i[:, 1], label='Att-y I', c='g', ls=':')
lines += plt.gca().lines
plt.title('Attitude I errors')
plt.subplot(6,1, 6)
plt.plot(errs.att.d[:, 0], label='Att-x D', c='r', ls='-.')
plt.plot(errs.att.d[:, 1], label='Att-y D', c='g', ls='-.')
lines += plt.gca().lines
plt.legend(handles=lines)
plt.title('Attitude D errors')
```
%% Cell type:code id:d9d99572 tags:
``` python
%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)
```
......
"""
Guided mode control. Generates x,y,z,yaw references for PID controllers.
"""
import numpy as np
class GuidedTrajectory:
def __init__(self, vehicle, controller) -> None:
self.vehicle = vehicle
self.controller = controller
self.waypoints = []
def add_waypoint(self, waypoint: np.ndarray):
self.waypoints.append(np.asarray(waypoint))
def __iter__(self):
pass
\ No newline at end of file
......@@ -147,7 +147,7 @@ class PosController(PIDController):
"Maximum acceleration in m/s/s"
max_jerk: float = 100.0
"Maximum jerk in m/s/s/s"
square_root_scaling: bool = True
square_root_scaling: bool = False
"Whether to scale P-gain with the square root of the error"
leashing: bool = False
"Whether to limit proportional position error"
......@@ -158,35 +158,43 @@ class PosController(PIDController):
# Att angle controller is strictly a P controller
self.k_i = np.zeros(2) * np.asarray(self.k_i)
self.k_d = np.zeros(2) * np.asarray(self.k_d)
self.leash = 0 if self.leashing else np.inf
if self.leashing or self.square_root_scaling:
self.k_p[:] = 0.5 * self.max_jerk / self.max_acceleration
super().__post_init__()
@property
def leash(self) -> float:
if not self.leashing:
return np.inf
# https://nrotella.github.io/journal/arducopter-flight-controllers.html
acc = self.vehicle.inertial_acceleration[:2]
acc_mag = np.linalg.norm(acc)
vel = self.vehicle.inertial_velocity[:2]
vel_mag = np.linalg.norm(vel)
leash = np.abs(acc_mag / (2 * self.k_p[0]**2) + vel_mag**2 / (2 * acc_mag + 1e-6))
leash = np.inf if leash==0 else leash
return leash
def step(self, reference, measurement):
# inertial frame velocity
err = reference - measurement
err_len = np.linalg.norm(err)
k_p = 0.5 * self.max_jerk / self.max_acceleration
if self.leashing:
# https://nrotella.github.io/journal/arducopter-flight-controllers.html
acc = self.vehicle.inertial_acceleration[:2]
acc_mag = np.linalg.norm(acc)
vel = self.vehicle.inertial_velocity[:2]
vel_mag = np.linalg.norm(vel)
self.leash = np.abs(acc_mag / (2 * k_p**2) + vel_mag**2 / (2 * acc_mag + 1e-6))
self.leash = np.inf if self.leash==0 else self.leash
err_unit = err / (err_len + 1e-6)
err_len = min(err_len, self.leash)
err = err_unit * err_len
self.err = err
velocity = k_p * err
velocity = self.err_p = self.k_p * err
if err_len > 0. and self.square_root_scaling:
velocity = np.zeros_like(self.k_p)
velocity[0] = sqrt_control(err[0], k_p, self.max_acceleration, self.dt)
velocity[1] = sqrt_control(err[1], k_p, self.max_acceleration, self.dt)
velocity[0] = sqrt_control(err[0], self.k_p[0], self.max_acceleration, self.dt)
velocity[1] = sqrt_control(err[1], self.k_p[1], self.max_acceleration, self.dt)
# scale velocity correction by error size
# velocity = (np.abs(err) / err_len) * velocity
self.err = err
self.err_p = velocity
else:
velocity = super().step(reference, measurement)
# convert to body-frame velocity
......
......@@ -129,7 +129,7 @@ class SpeedsMultirotorEnv(BaseMultirotorEnv):
"""
def __init__(self, vehicle) -> None:
def __init__(self, vehicle: Multirotor) -> None:
"""
Parameters
......
......@@ -133,6 +133,24 @@ class Motor:
self._last_angular_acc = 0.
def current_average(self, max_voltage: float) -> float:
"""
Average current consumption given the duty cycle of the speed controller.
Duty cycle depends on max voltage which causes 100% duty cycle.
Parameters
----------
max_voltage : float
The peak voltage corresponding to 100% duty cycle.
Returns
-------
float
Current consumption
"""
duty_cycle = self.voltage / max_voltage
return self.current * duty_cycle