Skip to content

Commit 80f8eff

Browse files
melodylylinmelodylylin
committed
fix loglinear position control
Signed-off-by: Li-Yu Lin <liyu8561501@gmail.com>
1 parent f1da7e2 commit 80f8eff

File tree

3 files changed

+81
-57
lines changed

3 files changed

+81
-57
lines changed

‎app/rdd2/prj.conf

+2
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,8 @@ CONFIG_CEREBRI_RDD2_ALLOCATION=y
1212
CONFIG_CEREBRI_RDD2_POSITION=y
1313
CONFIG_CEREBRI_RDD2_ANGULAR_VELOCITY=y
1414
CONFIG_CEREBRI_RDD2_ATTITUDE=y
15+
CONFIG_CEREBRI_RDD2_LOG_LINEAR_POSITION=y
16+
CONFIG_CEREBRI_RDD2_LOG_LINEAR_ATTITUDE=y
1517
CONFIG_CEREBRI_RDD2_LIGHTING=n
1618
CONFIG_CEREBRI_RDD2_CASADI=y
1719
CONFIG_CEREBRI_ACTUATE_LED_ARRAY_COUNT=36

‎app/rdd2/src/casadi/rdd2_loglinear.py

+46-41
Original file line numberDiff line numberDiff line change
@@ -7,8 +7,14 @@
77
from pathlib import Path
88
import casadi as ca
99
import cyecca.lie as lie
10-
from cyecca.lie.group_so3 import so3, SO3Quat, SO3EulerB321
11-
from cyecca.lie.group_se23 import SE23Quat, se23, SE23LieGroupElement, SE23LieAlgebraElement
10+
from cyecca.lie.group_so3 import so3, SO3Quat, SO3EulerB321, SO3Dcm
11+
from cyecca.models.bezier import derive_dcm_to_quat
12+
from cyecca.lie.group_se23 import (
13+
SE23Quat,
14+
se23,
15+
SE23LieGroupElement,
16+
SE23LieAlgebraElement,
17+
)
1218
from cyecca.symbolic import SERIES
1319

1420
print('python: ', sys.executable)
@@ -65,7 +71,6 @@ def derive_se23_error():
6571
# actual attitude, expressed as quaternion
6672
q_wb = ca.SX.sym('q_wb', 4)
6773
R_wb = SO3Quat.elem(q_wb).to_Matrix()
68-
p_b = R_wb.T @ p_w
6974
v_w = R_wb @ v_b
7075
X = SE23Quat.elem(ca.vertcat(p_w, v_w, q_wb))
7176

@@ -100,12 +105,12 @@ def derive_so3_attitude_control():
100105

101106
# INPUT CONSTANTS
102107
# -------------------------------
103-
kp = ca.SX.sym('kp', 3)
108+
kp = ca.SX.sym("kp", 3)
104109

105110
# INPUT VARIABLES
106111
# -------------------------------
107-
q = ca.SX.sym('q', 4) # actual quat
108-
q_r = ca.SX.sym('q_r', 4) # quat setpoint
112+
q = ca.SX.sym("q", 4) # actual quat
113+
q_r = ca.SX.sym("q_r", 4) # quat setpoint
109114

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

118-
omega = so3.elem(e.param).left_jacobian() @ ca.diag(kp) @ e.param # elementwise
123+
omega = so3.elem(e.param).left_jacobian() @ ca.diag(kp) @ e.param # elementwise
119124

120125
# FUNCTION
121126
# -------------------------------
122127
f_attitude_control = ca.Function(
123-
"so3_attitude_control",
124-
[kp, q, q_r],
125-
[omega],
126-
["kp", "q", "q_r"],
127-
["omega"])
128+
"so3_attitude_control", [kp, q, q_r], [omega], ["kp", "q", "q_r"], ["omega"]
129+
)
128130

129-
return {
130-
"so3_attitude_control": f_attitude_control
131-
}
131+
return {"so3_attitude_control": f_attitude_control}
132132

133133

134134
def derive_outerloop_control():
@@ -150,19 +150,26 @@ def derive_outerloop_control():
150150
#outputs: thrust force, angular errors
151151
zeta = ca.SX.sym('zeta', 9)
152152
at_w = ca.SX.sym('at_w', 3)
153-
qc_wb = SO3Quat.elem(ca.SX.sym('qc_wb', 4)) # camera orientation
154153
q_wb = SO3Quat.elem(ca.SX.sym('q_wb', 4))
155154
z_i = ca.SX.sym('z_i') # z velocity error integral
156155
dt = ca.SX.sym('dt') # time step
157156

158157
# CALC
159158
# -------------------------------
160159
# get control input
161-
K_se23 = ca.diag(ca.vertcat(kp_pos, kp_pos, kp_pos, kp_vel, kp_vel, kp_vel, kp))
160+
K_se23 = ca.DM([[-2.77504, 0, 0, 0.42856, 0, 0, 0, 0.339818, 0],
161+
[0, -2.77504, 0, 0, 0.42856, 0, -0.339818, 0, 0],
162+
[0, 0, -2.78649, 0, 0, 0.485281, 0, 0, 0],
163+
[0.42856, 0, 0, -1.95071, 0, 0, 0, -2.2064, 0],
164+
[0, 0.42856, 0, 0, -1.95071, 0, 2.2064, 0, 0],
165+
[0, 0, 0.485281, 0, 0, -2.95551, 0, 0, 0],
166+
[0, -0.339818, 0, 0, 2.2064, 0, -6.8016, 0, 0],
167+
[0.339818, 0, 0, -2.2064, 0, 0, 0, -6.8016, 0],
168+
[0, 0, 0, 0, 0, 0, 0, 0, -2.82843]])
162169
u_zeta = se23.elem(zeta).left_jacobian()@K_se23@zeta
163170

164171
# attitude control
165-
u_omega = u_zeta[6:]
172+
u_omega = -u_zeta[6:]
166173

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

179186
# normalized thrust vector
180187
p_norm_max = 0.3*m*g
181-
p_term = u_v + u_a + m * at_w
188+
p_term = q_wb @ u_v + q_wb @ u_a + m * at_w
182189
p_norm = ca.norm_2(p_term)
183190
p_term = ca.if_else(p_norm > p_norm_max, p_norm_max*p_term/p_norm, p_term)
184191

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

193200
# thrust
194201
nT = ca.norm_2(T)
195-
202+
196203
# body up is aligned with thrust
197-
zB = ca.if_else(nT > 1e-3, T/nT, zW)
204+
zB = ca.if_else(nT > 1e-3, T / nT, zW)
198205

199206
# point y using desired camera direction
200-
ec = SO3EulerB321.from_Quat(qc_wb)
201-
yt = ec.param[0]
202-
xC = ca.vertcat(ca.cos(yt), ca.sin(yt), 0)
207+
# ec = SO3EulerB321.from_Quat(qc_wb)
208+
# yt = ec.param[0]
209+
xC = ca.vertcat(1, 0, 0)
203210
yB = ca.cross(zB, xC)
204211
nyB = ca.norm_2(yB)
205-
yB = ca.if_else(nyB > 1e-3, yB/nyB, xW)
212+
yB = ca.if_else(nyB > 1e-3, yB / nyB, xW)
206213

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

221228
# FUNCTION
222229
# -------------------------------
223230
f_get_u = ca.Function(
224-
"se23_position_control",
225-
[thrust_trim, kp, zeta, at_w, qc_wb.param, q_wb.param, z_i, dt], [nT, qr_wb.param, z_i_2],
226-
227-
['thrust_trim', 'kp', 'zeta', 'at_w', 'qc_wb', 'q_wb', 'z_i', 'dt'],
228-
['nT', 'qr_wb', 'z_i_2'])
231+
"se23_control",
232+
[thrust_trim, kp, zeta, at_w, q_wb.param, z_i, dt],
233+
[nT, z_i_2, u_omega, q_sp.param],
234+
["thrust_trim", "kp", "zeta", "at_w", "q_wb", "z_i", "dt"],
235+
["nT", "z_i_2", "u_omega", "q_sp"],
236+
)
229237

230238
f_se23_attitude_control = ca.Function(
231-
"se23_attitude_control",
232-
[kp, zeta],
233-
[u_omega],
234-
['kp', "zeta"],
235-
["omega"])
236-
239+
"se23_attitude_control", [kp, zeta], [u_omega], ["kp", "zeta"], ["omega"]
240+
)
241+
237242
return {
238-
"se23_position_control" : f_get_u,
239-
"se23_attitude_control" : f_se23_attitude_control
243+
"se23_control": f_get_u,
244+
"se23_attitude_control": f_se23_attitude_control,
240245
}
241246

242247
def generate_code(eqs: dict, filename, dest_dir: str, **kwargs):
@@ -268,7 +273,7 @@ def generate_code(eqs: dict, filename, dest_dir: str, **kwargs):
268273

269274
if __name__ == "__main__":
270275
parser = argparse.ArgumentParser()
271-
parser.add_argument('dest_dir')
276+
parser.add_argument("dest_dir")
272277
args = parser.parse_args()
273278

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

280285
for name, eq in eqs.items():
281-
print('eq: ', name)
286+
print("eq: ", name)
282287

283288
generate_code(eqs, filename="rdd2_loglinear.c", dest_dir=args.dest_dir)
284289
print("complete")

‎app/rdd2/src/position.c

+33-16
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
#include <zephyr/shell/shell.h>
1919

2020
#if defined(CONFIG_CEREBRI_RDD2_LOG_LINEAR_POSITION)
21+
#include "app/rdd2/casadi/rdd2.h"
2122
#include "app/rdd2/casadi/rdd2_loglinear.h"
2223
#else
2324
#include "app/rdd2/casadi/rdd2.h"
@@ -201,35 +202,51 @@ static void rdd2_position_run(void *p0, void *p1, void *p2)
201202
};
202203

203204
double zeta[9];
205+
{
206+
// position_control:(thrust_trim,pt_w[3],vt_w[3],at_w[3],qc_wb[4],
207+
// p_w[3],v_b[3],q_wb[4],z_i,dt)->(nT,qr_wb[4],z_i_2)
208+
CASADI_FUNC_ARGS(position_control)
209+
args[0] = &thrust_trim;
210+
args[1] = p_rw;
211+
args[2] = v_rw;
212+
args[3] = at_w;
213+
args[4] = qc_wb;
214+
args[5] = p_w;
215+
args[6] = v_b;
216+
args[7] = q_wb;
217+
args[8] = &z_i;
218+
args[9] = &dt;
219+
res[1] = qr_wb;
220+
CASADI_FUNC_CALL(position_control)
221+
// LOG_INF("z_i: %10.4f", z_i);
222+
}
204223
{
205224
/* se23_error:(p_w[3],v_b[3],q_wb[4],p_rw[3],v_rw[3],q_r[4])->(zeta[9])*/
206-
CASADI_FUNC_ARGS(se23_error);
207-
args[0] = p_w;
208-
args[1] = v_b;
209-
args[2] = q_wb;
210-
args[3] = p_rw;
211-
args[4] = v_rw;
212-
args[5] = qr_wb;
225+
CASADI_FUNC_ARGS(se23_error)
226+
args[0] = p_rw;
227+
args[1] = v_rw;
228+
args[2] = qr_wb;
229+
args[3] = p_w;
230+
args[4] = v_b;
231+
args[5] = q_wb;
213232
res[0] = zeta;
214-
CASADI_FUNC_CALL(se23_error);
233+
CASADI_FUNC_CALL(se23_error)
215234
}
216235

217236
// se23_position_control:(thrust_trim,kp[3],zeta[9],at_w[3],qc_wb[4],q_wb[4],z_i,dt)
218237
// ->(nT,qr_wb[4],z_i_2)
219238
{
220-
CASADI_FUNC_ARGS(se23_position_control)
239+
CASADI_FUNC_ARGS(se23_control)
221240
args[0] = &thrust_trim;
222241
args[1] = kp;
223242
args[2] = zeta;
224243
args[3] = at_w;
225-
args[4] = qc_wb;
226-
args[5] = q_wb;
227-
args[6] = &z_i;
228-
args[7] = &dt;
244+
args[4] = q_wb;
245+
args[5] = &z_i;
246+
args[6] = &dt;
229247
res[0] = &nT;
230-
res[1] = qr_wb;
231-
res[2] = &z_i;
232-
CASADI_FUNC_CALL(se23_position_control)
248+
res[1] = &z_i;
249+
CASADI_FUNC_CALL(se23_control)
233250
}
234251
#else
235252
{

0 commit comments

Comments
 (0)