Skip to content

Change wls_alloc from a Python equivalent to creating a Python interface to the actual paparazzi C code #8

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
spacewolfXfr wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Change wls_alloc from a Python equivalent to creating a Python interface to the actual paparazzi C code #8

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
2b2a77b
Testing new allocation and schemes
Mar 16, 2026
8a9084b
Replaced the Python equivalent of WLS by a binding to the actual C code
Mar 18, 2026
19ba3c9
Cleaned up prints and changed install from pure `setup` to `pyproject…
Mar 18, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions .gitignore
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -117,6 +117,9 @@ venv.bak/
# Rope project settings
.ropeproject

# VSCode project settings
.vscode/**

# mkdocs documentation
/site

Expand Down
137 changes: 71 additions & 66 deletions dronesim/control/INDIControl.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@
import os
import pdb

# Active set library from : https://github.com/JimVaranelli/ActiveSet
import sys
import xml.etree.ElementTree as etxml

Expand All @@ -13,8 +12,8 @@

from dronesim.control.BaseControl import BaseControl

# from dronesim.control.ActiveSet import ActiveSet, ConstrainedLS
from dronesim.control.wls_alloc import wls_alloc
from dronesim.control.wls_alloc import wls_alloc as wls_alloc
# from dronesim.control.lnwls_alloc import indi_lsi_wrapper as wls_alloc
from dronesim.envs.BaseAviary import BaseAviary, DroneModel


Expand Down Expand Up @@ -95,15 +94,15 @@ def _parseURDFControlParameters(self):
# self.PWM2RPM_SCALE = float(pwm2rpm.attrib['scale'])
# self.PWM2RPM_CONST = float(pwm2rpm.attrib['const'])
vals = [str(k) for k in pwm2rpm.attrib.values()]
self.PWM2RPM_SCALE = [float(s) for s in vals[0].split(" ") if s != ""]
self.PWM2RPM_CONST = [float(s) for s in vals[1].split(" ") if s != ""]
self.PWM2RPM_SCALE = np.asarray([float(s) for s in vals[0].split(" ") if s != ""])
self.PWM2RPM_CONST = np.asarray([float(s) for s in vals[1].split(" ") if s != ""])

pwmlimit = URDF_TREE.find("control/pwm/limit")
# self.MIN_PWM = float(pwmlimit.attrib['min'])
# self.MAX_PWM = float(pwmlimit.attrib['max'])
vals = [str(k) for k in pwmlimit.attrib.values()]
self.MIN_PWM = [float(s) for s in vals[0].split(" ") if s != ""]
self.MAX_PWM = [float(s) for s in vals[1].split(" ") if s != ""]
self.MIN_PWM = np.asarray([float(s) for s in vals[0].split(" ") if s != ""])
self.MAX_PWM = np.asarray([float(s) for s in vals[1].split(" ") if s != ""])

################################################################################
def reset(self):
Expand All @@ -114,55 +113,56 @@ def reset(self):
"""
super().reset()
#### Store the last roll, pitch, and yaw ###################
self.last_rpy = np.zeros(3)
self.diffed_cur_ang_vel = np.zeros(3) # ERASE
self.last_rpy :np.ndarray = np.zeros(3)
self.diffed_cur_ang_vel :np.ndarray = np.zeros(3) # ERASE
#### Initialized PID control variables #####################
self.last_pos_e = np.zeros(3)
self.integral_pos_e = np.zeros(3)
self.last_rpy_e = np.zeros(3)
self.integral_rpy_e = np.zeros(3)
self.last_pos_e :np.ndarray = np.zeros(3)
self.integral_pos_e :np.ndarray = np.zeros(3)
self.last_rpy_e :np.ndarray = np.zeros(3)
self.integral_rpy_e :np.ndarray = np.zeros(3)

self.last_rates = np.zeros(3) # p,q,r
self.last_rates :np.ndarray = np.zeros(3) # p,q,r
# self.last_pwm = np.ones(self.indi_actuator_nr)*1. # initial pwm
self.last_thrust = 0.0
self.last_thrust:float = 0.0
# self.indi_increment = np.zeros(4)
self.cmd = np.ones(self.indi_actuator_nr) * 0.0
self.last_vel = np.zeros(3)
self.last_torque = np.zeros(3) # For SU2 controller

self.xax = -1
self.yax = -1
self.zax = -1
self.xax1 = -2
self.yax1 = -2
self.zax1 = -2
self.cmd :np.ndarray = np.zeros(self.indi_actuator_nr)
self.cmd_eps :np.ndarray = np.ones(self.indi_actuator_nr)*0.05
self.last_vel :np.ndarray = np.zeros(3)
self.last_torque :np.ndarray = np.zeros(3) # For SU2 controller

self.xax :float = -1
self.yax :float = -1
self.zax :float = -1
self.xax1 :float = -2
self.yax1 :float = -2
self.zax1 :float = -2

# for debugging logs...
self.att_log = np.zeros((30 * 100, 20))
self.guid_log = np.zeros((30 * 100, 20))
self.att_log_inc = 0
self.guid_log_inc = 0
self.att_log :np.ndarray = np.zeros((30 * 100, 20))
self.guid_log :np.ndarray = np.zeros((30 * 100, 20))
self.att_log_inc :float = 0
self.guid_log_inc :float = 0

self.rpm = np.zeros(self.indi_actuator_nr)
self.rpm :np.ndarray = np.zeros(self.indi_actuator_nr)

def rpm_of_pwm(self, pwm):
def rpm_of_pwm(self, pwm:np.ndarray):
self.rpm = self.PWM2RPM_SCALE * pwm + self.PWM2RPM_CONST
return self.rpm

################################################################################

def computeControl(
self,
control_timestep,
cur_pos,
cur_quat,
cur_vel,
cur_ang_vel,
target_pos,
target_vel=np.zeros(3),
target_acc=np.zeros(3),
target_rpy=np.zeros(3),
target_rpy_rates=np.zeros(3),
control_timestep:float,
cur_pos :np.ndarray,
cur_quat :np.ndarray,
cur_vel :np.ndarray,
cur_ang_vel :np.ndarray,
target_pos :np.ndarray,
target_vel :np.ndarray=np.zeros(3),
target_acc :np.ndarray=np.zeros(3),
target_rpy :np.ndarray=np.zeros(3),
target_rpy_rates:np.ndarray=np.zeros(3),

):
"""Computes the INDI control action (as RPMs) for a single drone.
Expand Down Expand Up @@ -231,14 +231,14 @@ def computeControl(
################################################################################
def _INDIPositionControl(
self,
control_timestep,
cur_pos,
cur_quat,
cur_vel,
target_pos,
target_rpy,
target_vel,
target_acc=np.zeros(3),
control_timestep:float,
cur_pos :np.ndarray,
cur_quat :np.ndarray,
cur_vel :np.ndarray,
target_pos :np.ndarray,
target_rpy :np.ndarray,
target_vel :np.ndarray,
target_acc :np.ndarray=np.zeros(3),
use_quaternion=False,
nonlinear_increment=False,
):
Expand Down Expand Up @@ -354,13 +354,13 @@ def _INDIPositionControl(

def _INDIAttitudeControl(
self,
control_timestep,
thrust,
cur_quat,
cur_ang_vel,
target_euler,
target_quat,
target_rpy_rates,
control_timestep:float,
thrust :float,
cur_quat :np.ndarray,
cur_ang_vel :np.ndarray,
target_euler :np.ndarray,
target_quat :np.ndarray,
target_rpy_rates:np.ndarray,
):
"""INDI attitude control.

Expand Down Expand Up @@ -412,11 +412,11 @@ def _INDIAttitudeControl(

def _INDIRateControl(
self,
control_timestep,
thrust,
cur_quat,
cur_ang_vel,
target_rpy_rates ):
control_timestep:float,
thrust :float,
cur_quat :np.ndarray,
cur_ang_vel :np.ndarray,
target_rpy_rates:np.ndarray ):

# FIXME : rate set point, reference angular speed, rpy rates, FIND a correct unique name for all...
rate_sp = Rate()
Expand Down Expand Up @@ -454,34 +454,39 @@ def _INDIRateControl(
indi_v[3] = thrust - self.last_thrust # * 0.
self.last_thrust = thrust

pseudo_inv = 1
pseudo_inv = 0
if pseudo_inv:
indi_du = np.dot(np.linalg.pinv(self.G1 / 0.05), indi_v) # *self.m
# print(f'Command : {self.cmd}')
# pdb.set_trace()
else:
# Use Active set for control allocation
umin = np.asarray(
[self.MIN_PWM[i] - self.cmd[i] for i in range(self.indi_actuator_nr)]
[max(self.MIN_PWM[i] - self.cmd[i],-self.cmd_eps[i]) for i in range(self.indi_actuator_nr)]
)
umax = np.asarray(
[self.MAX_PWM[i] - self.cmd[i] for i in range(self.indi_actuator_nr)]
[min(self.MAX_PWM[i] - self.cmd[i],self.cmd_eps[i]) for i in range(self.indi_actuator_nr)]
)

# print(f'UMIN : {umin} --- UMAX : {umax}')

# umax = np.asarray([self.MAX_PWM for i in range(4)])
# indi_v1 = [indi_v[i] for i in range(4)]

# up = np.array([0., 0., 0., 0.])
up = np.zeros_like(umin)
Wv = np.array([1000, 1000, 0.1, 10])
Wu = np.ones(self.indi_actuator_nr) # np.array([1, 1, 1, 1, 1, 1]) #FIXME
u_guess = None
W_init = None
up = None
# up = None

# import scipy.optimize
# res = scipy.optimize.lsq_linear(A, v, bounds=(umin, umax), lsmr_tol='auto', verbose=1)
indi_du, nit = wls_alloc(
indi_v, umin, umax, self.G1 / 0.05, u_guess, W_init, Wv, Wu, up
)

# print(f'INDI_V : {indi_v} --- INDI_DU : {indi_du} --- NIT : {nit}')

self.cmd += indi_du
self.cmd = np.clip(self.cmd, self.MIN_PWM, self.MAX_PWM) # command in PWM
Expand Down
24 changes: 13 additions & 11 deletions dronesim/control/INDIControl_6DOF.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@
import os
import pdb

# Active set library from : https://github.com/JimVaranelli/ActiveSet
import sys
import xml.etree.ElementTree as etxml
from dataclasses import dataclass
Expand All @@ -13,8 +12,8 @@

from dronesim.control.BaseControl import BaseControl

# from dronesim.control.ActiveSet import ActiveSet, ConstrainedLS
from dronesim.control.wls_alloc import wls_alloc
# from dronesim.control.lnwls_alloc import indi_lsi_wrapper as wls_alloc
from dronesim.envs.BaseAviary import BaseAviary, DroneModel

# @dataclass
Expand Down Expand Up @@ -613,19 +612,22 @@ def _INDIAttitudeControl(
# umax = np.asarray([self.MAX_PWM for i in range(4)])
# indi_v1 = [indi_v[i] for i in range(4)]

# up = np.array([0., 0., 0., 0.])
# Wv = np.array([1000, 1000, 0.1, 10])
Wv = np.array([1000, 1000, 0.1, 10, 10, 100]) # This can be a decision...
Wv = np.array([10, 10, 0.1, 1, 1, 5]) # This can be a decision...
Wu = np.ones(self.indi_actuator_nr) # np.array([1, 1, 1, 1, 1, 1]) #FIXME
u_guess = None
W_init = None
up = None

# import scipy.optimize
# res = scipy.optimize.lsq_linear(A, v, bounds=(umin, umax), lsmr_tol='auto', verbose=1)
indi_du, nit = wls_alloc(
indi_v, umin, umax, self.G1 / 0.05, u_guess, W_init, Wv, Wu, up
)
up = np.zeros_like(umin)

indi_uncapped = False
if indi_uncapped:
indi_du, nit = wls_alloc(
indi_v, np.ones_like(umin) * -1e9, np.ones_like(umax) * 1e9, self.G1 / 0.05, u_guess, W_init, Wv, Wu, up
)
else:
indi_du, nit = wls_alloc(
indi_v, umin, umax, self.G1 / 0.05, u_guess, W_init, Wv, Wu, up
)

self.cmd += indi_du
self.cmd = np.clip(self.cmd, self.MIN_PWM, self.MAX_PWM) # command in PWM
Expand Down
Loading
Loading