Skip to content

Commit d02ce3f

Browse files
author
FedeClaudi
committed
winstor
1 parent f35e384 commit d02ce3f

File tree

5 files changed

+248
-7
lines changed

5 files changed

+248
-7
lines changed

‎proj/environment/environment.py‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -107,7 +107,9 @@ def plan(self, curr_x, g_traj, itern):
107107

108108
if abs(start - end) != pred_len:
109109
g_traj = g_traj[start:end]
110-
len_diff = (start - end) - pred_len
110+
len_diff = (end - start) - pred_len
111+
if len_diff <= 0:
112+
len_diff = 1
111113
return np.pad(g_traj, ((0, len_diff), (0, 0)), mode="edge")
112114
else:
113115
return g_traj[start:end]

‎proj/model/config.py‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55
STATE_SIZE=5,
66
INPUT_SIZE=2,
77
ANGLE_IDX=2, # state vector index which is angle, used to fit diff in
8-
R=np.diag([0.05, 0.05]), # control cost
8+
R=np.diag([0.0001, 0.0001]), # control cost
99
Q=np.diag([1, 1, 1, 1, 0]), # state cost | x, y, theta, v, omega
1010
Sf=np.diag([0, 0, 0, 0, 0]), # final state cost
1111
)
@@ -48,7 +48,7 @@
4848

4949
class Config:
5050
# ----------------------------- Simulation params ---------------------------- #
51-
SIMULATION_NAME = ""
51+
SIMULATION_NAME = "horix"
5252

5353
USE_FAST = True # if true use cumba's methods
5454
SPAWN_TYPE = "trajectory"
@@ -70,7 +70,7 @@ class Config:
7070
min_speed=80,
7171
min_dist=0, # if agent is within this distance from trajectory end the goal is considered achieved
7272
# ? for trajectories from data
73-
px_to_cm=1 / 8, # convert px values to cm
73+
px_to_cm=1, # 1 / 8, # convert px values to cm
7474
# dist_th=60, # keep frames only after moved away from start location
7575
dist_th=-1,
7676
resample=True, # if True when using tracking trajectory resamples it
@@ -80,12 +80,12 @@ class Config:
8080

8181
# ------------------------------ Planning params ----------------------------- #
8282
planning = dict( # params used to compute goal states to be used for control
83-
prediction_length=80,
83+
prediction_length=40,
8484
n_ahead=5, # start prediction states from N steps ahead
8585
)
8686

8787
# --------------------------------- Plotting --------------------------------- #
88-
traj_plot_every = 80
88+
traj_plot_every = 15
8989

9090
# ------------------------------ Control params ------------------------------ #
9191
iLQR = dict(

‎proj/model/config.py.save‎

Lines changed: 99 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,99 @@
1+
import numpy as np
2+
3+
4+
class Config:
5+
SIMULATION_NAME = "test"
6+
7+
USE_FAST = True # if true use cumba's methods
8+
SPAWN_TYPE = "trajectory"
9+
LIVE_PLOT = False
10+
11+
# ----------------------------- Simulation params ---------------------------- #
12+
dt = 0.005
13+
14+
# -------------------------------- Cost params ------------------------------- #
15+
STATE_SIZE = 5
16+
INPUT_SIZE = 2
17+
ANGLE_IDX = 2 # state vector index which is angle, used to fit diff in
18+
19+
R = np.diag([0.05, 0.05]) # control cost
20+
Q = np.diag([1, 1, 1, 1, 0]) # state cost | x, y, theta, v, omega
21+
Sf = np.diag([0, 0, 0, 0, 0]) # final state cost
22+
23+
# STATE_SIZE = 4
24+
# INPUT_SIZE = 2
25+
26+
# R = np.diag([0.05, 0.05]) # control cost
27+
# Q = np.diag([2.5, 2.5, 0, 0]) # state cost | r, omega, v, omega
28+
# Sf = np.diag([2.5, 2.5, 0, 0]) # final state cost
29+
30+
# ------------------------------- Mouse params ------------------------------- #
31+
# ? works
32+
# mouse = dict(
33+
# mouse_type="working",
34+
# L=1.5, # half body width | cm
35+
# R=1, # radius of wheels | cm
36+
# d=0.1, # distance between axel and CoM | cm
37+
# length=3, # cm
38+
# m=round(20 / 9.81, 2), # mass | g
39+
# m_w=round(2 / 9.81, 2), # mass of wheels/legs |g
40+
#)
41+
42+
# ? more realistic
43+
mouse = dict(
44+
mouse_type = 'realistic',
45+
# L=2, # half body width | cm
46+
# R=2, # radius of wheels | cm
47+
# d=3, # distance between axel and CoM | cm
48+
# length=8.6, # cm
49+
# m=round(25 / 9.81, 2), # mass | g
50+
# m_w=round(0.6 / 9.81, 2), # mass of wheels/legs |g
51+
# )
52+
53+
# ------------------------------ Goal trajectory ----------------------------- #
54+
55+
trajectory = dict( # parameters of the goals trajectory
56+
name="tracking",
57+
nsteps=1000,
58+
distance=150,
59+
max_speed=100,
60+
min_speed=80,
61+
min_dist=5, # if agent is within this distance from trajectory end the goal is considered achieved
62+
dist_th=60, # keep frames only after moved away from start location
63+
resample=True, # if True when using tracking trajectory resamples it
64+
max_deg_interpol=8, # if using track fit a N degree polynomial to daa to smoothen
65+
randomize=True, # if true when using tracking it pulls a random trial
66+
)
67+
68+
# ------------------------------ Planning params ----------------------------- #
69+
planning = dict( # params used to compute goal states to be used for control
70+
prediction_length=80,
71+
n_ahead=5, # start prediction states from N steps ahead
72+
)
73+
74+
# --------------------------------- Plotting --------------------------------- #
75+
traj_plot_every = 15
76+
77+
# ------------------------------ Control params ------------------------------ #
78+
iLQR = dict(
79+
max_iter=500,
80+
init_mu=1.0,
81+
mu_min=1e-6,
82+
mu_max=1e10,
83+
init_delta=2.0,
84+
threshold=1e-6,
85+
)
86+
87+
def config_dict(self):
88+
return dict(
89+
dt=self.dt,
90+
STATE_SIZE=self.STATE_SIZE,
91+
INPUT_SIZE=self.INPUT_SIZE,
92+
R=list(np.diag(self.R).tolist()),
93+
Q=list(np.diag(self.Q).tolist()),
94+
Sf=list(np.diag(self.Sf).tolist()),
95+
mouse=self.mouse,
96+
trajectory=self.trajectory,
97+
planning=self.planning,
98+
iLQR=self.iLQR,
99+
)

‎proj/model/config.py.save.1‎

Lines changed: 137 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,137 @@
1+
import numpy as np
2+
3+
# ------------------------------- Model specific params ------------------------------ #
4+
_cart_params = dict(
5+
STATE_SIZE=5,
6+
INPUT_SIZE=2,
7+
ANGLE_IDX=2, # state vector index which is angle, used to fit diff in
8+
R=np.diag([0.0001, 0.0001]), # control cost
9+
Q=np.diag([1, 1, 1, 1, 0]), # state cost | x, y, theta, v, omega
10+
Sf=np.diag([0, 0, 0, 0, 0]), # final state cost
11+
)
12+
13+
_polar_params = dict(
14+
STATE_SIZE=4,
15+
INPUT_SIZE=2,
16+
ANGLE_IDX=2, # state vector index which is angle, used to fit diff in
17+
R=np.diag([0.05, 0.05]), # control cost
18+
Q=np.diag([2.5, 2.5, 0, 0]), # state cost | r, omega, v, omega
19+
Sf=np.diag([2.5, 2.5, 0, 0]), # final state cost
20+
)
21+
22+
# -------------------------------- Mouse types ------------------------------- #
23+
24+
_easy_mouse = dict(
25+
L=1.5, # half body width | cm
26+
R=1, # radius of wheels | cm
27+
d=0.1, # distance between axel and CoM | cm
28+
length=3, # cm
29+
m=round(20 / 9.81, 2), # mass | g
30+
m_w=round(2 / 9.81, 2), # mass of wheels/legs |g
31+
mouse_type="easy",
32+
)
33+
34+
_realistic_mouse = dict(
35+
L=2, # half body width | cm
36+
R=1.5, # radius of wheels | cm
37+
d=2, # distance between axel and CoM | cm
38+
length=1, # 8.6, # cm
39+
m=round(23 / 9.81, 2), # mass | g
40+
m_w=0.8, # mass of wheels/legs |g
41+
mouse_type="realistic",
42+
)
43+
44+
# ---------------------------------------------------------------------------- #
45+
# CONFIG #
46+
# ---------------------------------------------------------------------------- #
47+
48+
49+
class Config:
50+
# ----------------------------- Simulation params ---------------------------- #
51+
SIMULATION_NAME = "megatest
52+
"
53+
54+
USE_FAST = True # if true use cumba's methods
55+
SPAWN_TYPE = "trajectory"
56+
LIVE_PLOT = False
57+
58+
mouse_type = "easy"
59+
model_type = "cart"
60+
61+
dt = 0.005
62+
63+
# ------------------------------ Goal trajectory ----------------------------- #
64+
65+
trajectory = dict( # parameters of the goals trajectory
66+
name="tracking",
67+
# ? For artificial trajectories
68+
nsteps=1000,
69+
distance=150,
70+
max_speed=100,
71+
min_speed=80,
72+
min_dist=0, # if agent is within this distance from trajectory end the goal is considered achieved
73+
# ? for trajectories from data
74+
px_to_cm=1, # 1 / 8, # convert px values to cm
75+
# dist_th=60, # keep frames only after moved away from start location
76+
dist_th=-1,
77+
resample=True, # if True when using tracking trajectory resamples it
78+
max_deg_interpol=8, # if using track fit a N degree polynomial to daa to smoothen
79+
randomize=True, # if true when using tracking it pulls a random trial
80+
)
81+
82+
# ------------------------------ Planning params ----------------------------- #
83+
planning = dict( # params used to compute goal states to be used for control
84+
prediction_length=80,
85+
n_ahead=5, # start prediction states from N steps ahead
86+
)
87+
88+
# --------------------------------- Plotting --------------------------------- #
89+
traj_plot_every = 80
90+
91+
# ------------------------------ Control params ------------------------------ #
92+
iLQR = dict(
93+
max_iter=500,
94+
init_mu=1.0,
95+
mu_min=1e-6,
96+
mu_max=1e10,
97+
init_delta=2.0,
98+
threshold=1e-6,
99+
)
100+
101+
def __init__(self,):
102+
# get mouse params
103+
self.mouse = (
104+
_easy_mouse if self.mouse_type == "easy" else _realistic_mouse
105+
)
106+
107+
# set model params
108+
if self.model_type == "cart":
109+
params = _cart_params
110+
else:
111+
params = _polar_params
112+
113+
self.STATE_SIZE = params["STATE_SIZE"]
114+
self.INPUT_SIZE = params["INPUT_SIZE"]
115+
self.ANGLE_IDX = params["ANGLE_IDX"]
116+
self.R = params["R"]
117+
self.Q = params["Q"]
118+
self.Sf = params["Sf"]
119+
120+
# Adjust mouse length for plotting
121+
self.mouse["length"] = (
122+
self.mouse["length"] * self.trajectory["px_to_cm"]
123+
)
124+
125+
def config_dict(self):
126+
return dict(
127+
dt=self.dt,
128+
STATE_SIZE=self.STATE_SIZE,
129+
INPUT_SIZE=self.INPUT_SIZE,
130+
R=list(np.diag(self.R).tolist()),
131+
Q=list(np.diag(self.Q).tolist()),
132+
Sf=list(np.diag(self.Sf).tolist()),
133+
mouse=self.mouse,
134+
trajectory=self.trajectory,
135+
planning=self.planning,
136+
iLQR=self.iLQR,
137+
)

‎proj/run/runner.py‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -117,7 +117,10 @@ def run_experiment(
117117
logger.exception(
118118
f"Failed to take next step in simulation.\nError: {e}\n\n"
119119
)
120-
environment.failed()
120+
try:
121+
environment.conclude():
122+
except:
123+
environment.failed()
121124
return
122125

123126
logger.info(f"Terminated after {itern} iterations.")

0 commit comments

Comments
 (0)