Skip to content

Commit a922a8f

Browse files
Meow404Meow404
committed
feat: add config for C3+
1 parent f437aca commit a922a8f

File tree

9 files changed

+748
-9
lines changed

9 files changed

+748
-9
lines changed

‎MODULE.bazel‎

Lines changed: 3 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -90,14 +90,10 @@ archive_override(
9090
)
9191

9292
bazel_dep(name = "c3")
93-
# git_override(
94-
# module_name = "c3",
95-
# remote = "https://github.com/DAIRLab/c3.git",
96-
# commit = "2bd13495c3306cf7d53992e2495774d1ad454cca"
97-
# )
98-
local_path_override(
93+
git_override(
9994
module_name = "c3",
100-
path = "../c3", # Adjust this path as needed.
95+
remote = "https://github.com/DAIRLab/c3.git",
96+
commit = "ed1cb0ed199901aeae85725502b583a2b40c9553"
10197
)
10298

10399
INEKF_COMMIT = "297c308e50fa599af92ce3bd5f11d71e2bf8af69"

‎examples/plate-balancing/config/C3/controller-c3plus-options/1_testing.yaml‎

Lines changed: 112 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
1+
projection_type: "C3+"
2+
3+
solve_time_filter_alpha: 0.95
4+
#set to 0 to publish as fast as possible
5+
publish_frequency: 0
6+
state_prediction_joints:
7+
[]
8+
# - name : "plate_x"
9+
# max_acceleration : 10
10+
# - name : "plate_y"
11+
# max_acceleration : 10
12+
# - name : "plate_z"
13+
# max_acceleration : 10
14+
15+
# Workspace Limits
16+
workspace_limits:
17+
[
18+
[1.0, 0.0, 0.0, 0.4, 0.6],
19+
[0.0, 1.0, 0.0, -0.2, 0.2],
20+
[0.0, 0.0, 1.0, 0.3, 0.6],
21+
]
22+
workspace_margins: 0.05
23+
24+
u_horizontal_limits: [-50, 50]
25+
u_vertical_limits: [-50, 50]
26+
27+
lcs_factory_options:
28+
#options are 'stewart_and_trinkle' or 'anitescu'
29+
# contact_model : 'stewart_and_trinkle'
30+
contact_model: "stewart_and_trinkle"
31+
num_friction_directions: 2
32+
num_contacts: 3
33+
spring_stiffness: 0
34+
mu: [0.6, 0.6, 0.6]
35+
N: 5
36+
dt: 0.05
37+
38+
c3_options:
39+
warm_start: false
40+
end_on_qp_step: false
41+
penalize_input_change: true
42+
scale_lcs: true
43+
44+
num_threads: 5
45+
delta_option: 1
46+
47+
M: 1000
48+
49+
admm_iter: 2
50+
51+
gamma: 1.0
52+
rho_scale: 4
53+
54+
# matrix scaling
55+
w_Q: 50
56+
w_R: 25
57+
# Penalty on all decision variables, assuming scalar
58+
w_G: 0.1
59+
# Penalty on all decision variables, assuming scalar
60+
w_U: 0.5
61+
62+
#State Tracking Error, assuming diagonal
63+
q_vector:
64+
[
65+
175,
66+
175,
67+
175,
68+
1,
69+
1,
70+
1,
71+
1,
72+
15000,
73+
15000,
74+
15000,
75+
5,
76+
5,
77+
5,
78+
10,
79+
10,
80+
10,
81+
5,
82+
5,
83+
5,
84+
]
85+
#Penalty on efforts, assuming diagonal
86+
r_vector: [0.15, 0.15, 0.1]
87+
88+
# Penalty on all decision variables
89+
g_x:
90+
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
91+
g_gamma: [1, 1, 1]
92+
g_lambda_n: [1, 1, 1]
93+
g_lambda_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
94+
g_lambda: []
95+
g_u: [0, 0, 0]
96+
g_eta_slack: [1, 1, 1]
97+
g_eta_n: [1, 1, 1]
98+
g_eta_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
99+
g_eta: []
100+
101+
# Penalty on all decision variables
102+
u_x:
103+
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
104+
u_gamma: [1, 1, 1]
105+
u_lambda_n: [1, 1, 1]
106+
u_lambda_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
107+
u_lambda: []
108+
u_u: [0, 0, 0]
109+
u_eta_slack: [1, 1, 1]
110+
u_eta_n: [1, 1, 1]
111+
u_eta_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
112+
u_eta: []

‎examples/plate-balancing/config/C3/controller-c3plus-options/2_supports.yaml‎

Lines changed: 223 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,223 @@
1+
projection_type: "C3+"
2+
3+
solve_time_filter_alpha: 0.95
4+
#set to 0 to publish as fast as possible
5+
publish_frequency: 0
6+
state_prediction_joints:
7+
- name: "plate_x"
8+
max_acceleration: 10
9+
- name: "plate_y"
10+
max_acceleration: 10
11+
- name: "plate_z"
12+
max_acceleration: 10
13+
14+
# Workspace Limits
15+
# End Effector Workspace Limits Specified as Linear Constraints [x, y, z, lb, ub]
16+
workspace_limits:
17+
[
18+
[1.0, 0.0, 0.0, 0.4, 0.6],
19+
[0.0, 1.0, 0.0, -0.1, 0.1],
20+
[0.0, 0.0, 1.0, 0.35, 0.7],
21+
]
22+
workspace_margins: 0.05
23+
24+
u_horizontal_limits: [-10, 10]
25+
u_vertical_limits: [0, 30]
26+
27+
lcs_factory_options:
28+
#options are 'stewart_and_trinkle' or 'anitescu'
29+
# contact_model : 'stewart_and_trinkle'
30+
contact_model: "anitescu"
31+
num_friction_directions: 2
32+
num_contacts: 7
33+
spring_stiffness: 0
34+
mu: [0.6, 0.6, 0.6, 0.1, 0.1, 0.1, 0.1]
35+
N: 5
36+
dt: 0.075
37+
38+
c3_options:
39+
warm_start: true
40+
end_on_qp_step: false
41+
penalize_input_change: true
42+
scale_lcs: true
43+
44+
num_threads: 5
45+
delta_option: 1
46+
47+
admm_iter: 2
48+
49+
gamma: 1.0
50+
rho_scale: 4
51+
52+
#matrix scaling
53+
w_Q: 50
54+
w_R: 50
55+
#Penalty on all decision variables, assuming scalar
56+
w_G: 0.15
57+
#Penalty on all decision variables, assuming scalar
58+
w_U: 0.1
59+
60+
#State Tracking Error, assuming diagonal
61+
q_vector:
62+
[
63+
150,
64+
150,
65+
150,
66+
0,
67+
1,
68+
1,
69+
0,
70+
15000,
71+
15000,
72+
15000,
73+
5,
74+
5,
75+
15,
76+
10,
77+
10,
78+
1,
79+
5,
80+
5,
81+
5,
82+
]
83+
#Penalty on efforts, assuming diagonal
84+
r_vector: [0.15, 0.15, 0.1]
85+
86+
#Penalty on matching projected variables
87+
g_x:
88+
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
89+
g_gamma: []
90+
g_lambda_n: []
91+
g_lambda_t: []
92+
g_lambda:
93+
[
94+
85,
95+
85,
96+
85,
97+
85,
98+
85,
99+
85,
100+
85,
101+
85,
102+
85,
103+
85,
104+
85,
105+
85,
106+
85,
107+
85,
108+
85,
109+
85,
110+
85,
111+
85,
112+
85,
113+
85,
114+
85,
115+
85,
116+
85,
117+
85,
118+
85,
119+
85,
120+
85,
121+
85,
122+
]
123+
g_u: [1, 1, 1]
124+
g_eta:
125+
[
126+
0.0001,
127+
0.0001,
128+
0.0001,
129+
0.0001,
130+
0.0001,
131+
0.0001,
132+
0.0001,
133+
0.0001,
134+
0.0001,
135+
0.0001,
136+
0.0001,
137+
0.0001,
138+
0.0001,
139+
0.0001,
140+
0.0001,
141+
0.0001,
142+
0.0001,
143+
0.0001,
144+
0.0001,
145+
0.0001,
146+
0.0001,
147+
0.0001,
148+
0.0001,
149+
0.0001,
150+
0.0001,
151+
0.0001,
152+
0.0001,
153+
0.0001,
154+
]
155+
156+
#Penalty on matching the QP variables
157+
u_x: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
158+
u_gamma: []
159+
u_lambda_n: []
160+
u_lambda_t: []
161+
u_lambda:
162+
[
163+
1,
164+
1,
165+
1,
166+
1,
167+
1,
168+
1,
169+
1,
170+
1,
171+
1,
172+
1,
173+
1,
174+
1,
175+
1,
176+
1,
177+
1,
178+
1,
179+
1,
180+
1,
181+
1,
182+
1,
183+
1,
184+
1,
185+
1,
186+
1,
187+
1,
188+
1,
189+
1,
190+
1,
191+
]
192+
u_u: [0, 0, 0]
193+
u_eta:
194+
[
195+
1,
196+
1,
197+
1,
198+
1,
199+
1,
200+
1,
201+
1,
202+
1,
203+
1,
204+
1,
205+
1,
206+
1,
207+
1,
208+
1,
209+
1,
210+
1,
211+
1,
212+
1,
213+
1,
214+
1,
215+
1,
216+
1,
217+
1,
218+
1,
219+
1,
220+
1,
221+
1,
222+
1,
223+
]

0 commit comments

Comments
 (0)