Skip to content
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

fix log-linear position control #189

Merged
merged 2 commits into from
Feb 21, 2025
Merged

fix log-linear position control #189

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c86e6d7
fix loglinear position control
melodylylin Nov 23, 2024
8e8ec32
change cyecca version
melodylylin Feb 21, 2025
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
2 changes: 2 additions & 0 deletions app/rdd2/prj.conf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,8 @@ CONFIG_CEREBRI_RDD2_ALLOCATION=y
CONFIG_CEREBRI_RDD2_POSITION=y
CONFIG_CEREBRI_RDD2_ANGULAR_VELOCITY=y
CONFIG_CEREBRI_RDD2_ATTITUDE=y
CONFIG_CEREBRI_RDD2_LOG_LINEAR_POSITION=y
CONFIG_CEREBRI_RDD2_LOG_LINEAR_ATTITUDE=y
CONFIG_CEREBRI_RDD2_LIGHTING=n
CONFIG_CEREBRI_RDD2_CASADI=y
CONFIG_CEREBRI_ACTUATE_LED_ARRAY_COUNT=36
Expand Down
87 changes: 46 additions & 41 deletions app/rdd2/src/casadi/rdd2_loglinear.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,14 @@
from pathlib import Path
import casadi as ca
import cyecca.lie as lie
from cyecca.lie.group_so3 import so3, SO3Quat, SO3EulerB321
from cyecca.lie.group_se23 import SE23Quat, se23, SE23LieGroupElement, SE23LieAlgebraElement
from cyecca.lie.group_so3 import so3, SO3Quat, SO3EulerB321, SO3Dcm
from cyecca.models.bezier import derive_dcm_to_quat
from cyecca.lie.group_se23 import (
SE23Quat,
se23,
SE23LieGroupElement,
SE23LieAlgebraElement,
)
from cyecca.symbolic import SERIES

print('python: ', sys.executable)
Expand Down Expand Up @@ -65,7 +71,6 @@ def derive_se23_error():
# actual attitude, expressed as quaternion
q_wb = ca.SX.sym('q_wb', 4)
R_wb = SO3Quat.elem(q_wb).to_Matrix()
p_b = R_wb.T @ p_w
v_w = R_wb @ v_b
X = SE23Quat.elem(ca.vertcat(p_w, v_w, q_wb))

Expand Down Expand Up @@ -100,12 +105,12 @@ def derive_so3_attitude_control():

# INPUT CONSTANTS
# -------------------------------
kp = ca.SX.sym('kp', 3)
kp = ca.SX.sym("kp", 3)

# INPUT VARIABLES
# -------------------------------
q = ca.SX.sym('q', 4) # actual quat
q_r = ca.SX.sym('q_r', 4) # quat setpoint
q = ca.SX.sym("q", 4) # actual quat
q_r = ca.SX.sym("q_r", 4) # quat setpoint

# CALC
# -------------------------------
Expand All @@ -115,20 +120,15 @@ def derive_so3_attitude_control():
# Lie algebra
e = (X.inverse() * X_r).log() # angular velocity to get to desired att in 1 sec

omega = so3.elem(e.param).left_jacobian() @ ca.diag(kp) @ e.param # elementwise
omega = so3.elem(e.param).left_jacobian() @ ca.diag(kp) @ e.param # elementwise

# FUNCTION
# -------------------------------
f_attitude_control = ca.Function(
"so3_attitude_control",
[kp, q, q_r],
[omega],
["kp", "q", "q_r"],
["omega"])
"so3_attitude_control", [kp, q, q_r], [omega], ["kp", "q", "q_r"], ["omega"]
)

return {
"so3_attitude_control": f_attitude_control
}
return {"so3_attitude_control": f_attitude_control}


def derive_outerloop_control():
Expand All @@ -150,19 +150,26 @@ def derive_outerloop_control():
#outputs: thrust force, angular errors
zeta = ca.SX.sym('zeta', 9)
at_w = ca.SX.sym('at_w', 3)
qc_wb = SO3Quat.elem(ca.SX.sym('qc_wb', 4)) # camera orientation
q_wb = SO3Quat.elem(ca.SX.sym('q_wb', 4))
z_i = ca.SX.sym('z_i') # z velocity error integral
dt = ca.SX.sym('dt') # time step

# CALC
# -------------------------------
# get control input
K_se23 = ca.diag(ca.vertcat(kp_pos, kp_pos, kp_pos, kp_vel, kp_vel, kp_vel, kp))
K_se23 = ca.DM([[-2.77504, 0, 0, 0.42856, 0, 0, 0, 0.339818, 0],
[0, -2.77504, 0, 0, 0.42856, 0, -0.339818, 0, 0],
[0, 0, -2.78649, 0, 0, 0.485281, 0, 0, 0],
[0.42856, 0, 0, -1.95071, 0, 0, 0, -2.2064, 0],
[0, 0.42856, 0, 0, -1.95071, 0, 2.2064, 0, 0],
[0, 0, 0.485281, 0, 0, -2.95551, 0, 0, 0],
[0, -0.339818, 0, 0, 2.2064, 0, -6.8016, 0, 0],
[0.339818, 0, 0, -2.2064, 0, 0, 0, -6.8016, 0],
[0, 0, 0, 0, 0, 0, 0, 0, -2.82843]])
u_zeta = se23.elem(zeta).left_jacobian()@K_se23@zeta

# attitude control
u_omega = u_zeta[6:]
u_omega = -u_zeta[6:]

# position control
u_v = u_zeta[0:3]
Expand All @@ -178,7 +185,7 @@ def derive_outerloop_control():

# normalized thrust vector
p_norm_max = 0.3*m*g
p_term = u_v + u_a + m * at_w
p_term = q_wb @ u_v + q_wb @ u_a + m * at_w
p_norm = ca.norm_2(p_term)
p_term = ca.if_else(p_norm > p_norm_max, p_norm_max*p_term/p_norm, p_term)

Expand All @@ -192,17 +199,17 @@ def derive_outerloop_control():

# thrust
nT = ca.norm_2(T)

# body up is aligned with thrust
zB = ca.if_else(nT > 1e-3, T/nT, zW)
zB = ca.if_else(nT > 1e-3, T / nT, zW)

# point y using desired camera direction
ec = SO3EulerB321.from_Quat(qc_wb)
yt = ec.param[0]
xC = ca.vertcat(ca.cos(yt), ca.sin(yt), 0)
# ec = SO3EulerB321.from_Quat(qc_wb)
# yt = ec.param[0]
xC = ca.vertcat(1, 0, 0)
yB = ca.cross(zB, xC)
nyB = ca.norm_2(yB)
yB = ca.if_else(nyB > 1e-3, yB/nyB, xW)
yB = ca.if_else(nyB > 1e-3, yB / nyB, xW)

# point x using cross product of unit vectors
xB = ca.cross(yB, zB)
Expand All @@ -216,27 +223,25 @@ def derive_outerloop_control():
# deisred euler angles
# note using euler angles as set point is not problematic
# using Lie group approach for control
qr_wb = SO3Quat.from_Matrix(Rd_wb)
q_sp = SO3Quat.from_Matrix(Rd_wb)

# FUNCTION
# -------------------------------
f_get_u = ca.Function(
"se23_position_control",
[thrust_trim, kp, zeta, at_w, qc_wb.param, q_wb.param, z_i, dt], [nT, qr_wb.param, z_i_2],

['thrust_trim', 'kp', 'zeta', 'at_w', 'qc_wb', 'q_wb', 'z_i', 'dt'],
['nT', 'qr_wb', 'z_i_2'])
"se23_control",
[thrust_trim, kp, zeta, at_w, q_wb.param, z_i, dt],
[nT, z_i_2, u_omega, q_sp.param],
["thrust_trim", "kp", "zeta", "at_w", "q_wb", "z_i", "dt"],
["nT", "z_i_2", "u_omega", "q_sp"],
)

f_se23_attitude_control = ca.Function(
"se23_attitude_control",
[kp, zeta],
[u_omega],
['kp', "zeta"],
["omega"])

"se23_attitude_control", [kp, zeta], [u_omega], ["kp", "zeta"], ["omega"]
)

return {
"se23_position_control" : f_get_u,
"se23_attitude_control" : f_se23_attitude_control
"se23_control": f_get_u,
"se23_attitude_control": f_se23_attitude_control,
}

def generate_code(eqs: dict, filename, dest_dir: str, **kwargs):
Expand Down Expand Up @@ -268,7 +273,7 @@ def generate_code(eqs: dict, filename, dest_dir: str, **kwargs):

if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument('dest_dir')
parser.add_argument("dest_dir")
args = parser.parse_args()

print("generating casadi equations in {:s}".format(args.dest_dir))
Expand All @@ -278,7 +283,7 @@ def generate_code(eqs: dict, filename, dest_dir: str, **kwargs):
eqs.update(derive_se23_error())

for name, eq in eqs.items():
print('eq: ', name)
print("eq: ", name)

generate_code(eqs, filename="rdd2_loglinear.c", dest_dir=args.dest_dir)
print("complete")
49 changes: 33 additions & 16 deletions app/rdd2/src/position.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <zephyr/shell/shell.h>

#if defined(CONFIG_CEREBRI_RDD2_LOG_LINEAR_POSITION)
#include "app/rdd2/casadi/rdd2.h"
#include "app/rdd2/casadi/rdd2_loglinear.h"
#else
#include "app/rdd2/casadi/rdd2.h"
Expand Down Expand Up @@ -201,35 +202,51 @@ static void rdd2_position_run(void *p0, void *p1, void *p2)
};

double zeta[9];
{
// position_control:(thrust_trim,pt_w[3],vt_w[3],at_w[3],qc_wb[4],
// p_w[3],v_b[3],q_wb[4],z_i,dt)->(nT,qr_wb[4],z_i_2)
CASADI_FUNC_ARGS(position_control)
args[0] = &thrust_trim;
args[1] = p_rw;
args[2] = v_rw;
args[3] = at_w;
args[4] = qc_wb;
args[5] = p_w;
args[6] = v_b;
args[7] = q_wb;
args[8] = &z_i;
args[9] = &dt;
res[1] = qr_wb;
CASADI_FUNC_CALL(position_control)
// LOG_INF("z_i: %10.4f", z_i);
}
{
/* se23_error:(p_w[3],v_b[3],q_wb[4],p_rw[3],v_rw[3],q_r[4])->(zeta[9])*/
CASADI_FUNC_ARGS(se23_error);
args[0] = p_w;
args[1] = v_b;
args[2] = q_wb;
args[3] = p_rw;
args[4] = v_rw;
args[5] = qr_wb;
CASADI_FUNC_ARGS(se23_error)
args[0] = p_rw;
args[1] = v_rw;
args[2] = qr_wb;
args[3] = p_w;
args[4] = v_b;
args[5] = q_wb;
res[0] = zeta;
CASADI_FUNC_CALL(se23_error);
CASADI_FUNC_CALL(se23_error)
}

// se23_position_control:(thrust_trim,kp[3],zeta[9],at_w[3],qc_wb[4],q_wb[4],z_i,dt)
// ->(nT,qr_wb[4],z_i_2)
{
CASADI_FUNC_ARGS(se23_position_control)
CASADI_FUNC_ARGS(se23_control)
args[0] = &thrust_trim;
args[1] = kp;
args[2] = zeta;
args[3] = at_w;
args[4] = qc_wb;
args[5] = q_wb;
args[6] = &z_i;
args[7] = &dt;
args[4] = q_wb;
args[5] = &z_i;
args[6] = &dt;
res[0] = &nT;
res[1] = qr_wb;
res[2] = &z_i;
CASADI_FUNC_CALL(se23_position_control)
res[1] = &z_i;
CASADI_FUNC_CALL(se23_control)
}
#else
{
Expand Down
2 changes: 1 addition & 1 deletion west.yml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ manifest:
path: modules/lib/zros
- name: cyecca
remote: cognipilot
revision: 9c0f4e73ae1a64a7a9a676975e8895d4775e1186 # main 6/17/24
revision: 45f92d1107e350504ed8dad4e526aedf0843490b # main 12/1/24
path: modules/lib/cyecca
- name: synapse_pb
remote: cognipilot
Expand Down