Skip to content

Commit f00aa80

Browse files
premsuggupremsuggu
committed
refactor: Update configuration files and main execution flow for improved model handling and trajectory loading.
Update: Currenntly working MPC for simple motion for h1 model.
1 parent 2cd13fb commit f00aa80

7 files changed

Lines changed: 434 additions & 379 deletions

File tree

‎config.yaml‎

Lines changed: 63 additions & 111 deletions
Original file line numberDiff line numberDiff line change
@@ -1,153 +1,105 @@
11
robot:
2-
name: dm_humanoid
3-
model_path: "robots/dm_humanoid/humanoid.xml"
4-
urdf_path: "robots/dm_humanoid/humanoid.xml"
5-
2+
name: h1
3+
model_path: "robots/h1_description/mjcf/scene.xml"
4+
urdf_path: "robots/h1_description/urdf/h1.urdf"
5+
66
ee_feet:
7-
right_feet_ee: "foot_right"
8-
left_feet_ee: "foot_left"
9-
pelvis_body_name: "pelvis" # Body whose z-position is used in Pelvis/Feet cost
10-
torso_body_name: "torso" # Floating-base body (used as one of 4 forward-dir bodies in Walk cost)
11-
waist_lower_body_name: "waist_lower" # DeepMind: waist_lower_subcomvel = subtree CoM vel of this body
7+
right_feet_ee: "right_ankle_link"
8+
left_feet_ee: "left_ankle_link"
9+
pelvis_body_name: "pelvis"
10+
torso_body_name: "torso_link"
11+
waist_lower_body_name: "torso_link"
1212

1313
reference_trajectory:
14-
q_ref: "data/dm_humanoid/q_standing.csv"
15-
v_ref: "data/dm_humanoid/v_standing.csv"
16-
contact_schedule: "data/dm_humanoid/contact_standing.csv"
14+
q_ref: "data/h1/standing/q_T.csv"
15+
v_ref: "data/h1/standing/v_standing.csv"
16+
contact_schedule: "data/h1/standing/contact_T.csv"
1717

1818
mpc:
19-
horizon: 23 # MJPC: 0.35s / 0.015s = 23 steps
20-
dt: 0.015 # MJPC timestep (15ms)
21-
physics_dt: 0.015 # MuJoCo substep (keep fine for stability)
22-
gravity: [0.0, 0.0, -1.0] # Standard gravity (MJPC uses full gravity)
23-
sim_steps: 50 # Total simulation steps
24-
contact_impratio: 100.0 # Contact stiffness
19+
horizon: 25
20+
dt: 0.02
21+
physics_dt: 0.02
22+
gravity: [0.0, 0.0, -0.05]
23+
sim_steps: 50
24+
contact_impratio: 100.0
2525

2626
cost_weights:
27-
# -------------------------------------------------------------------------
28-
# Posture cost — penalises joint angles qpos[7:nq] from reference
29-
# DeepMind stand: NO posture term (disabled)
30-
# -------------------------------------------------------------------------
3127
posture:
32-
weight: 0.0 # DISABLED for stand task (DeepMind has no posture)
33-
terminal_weight: 0.1 # Terminal cost weight (independent of running)
28+
weight: 50.0
29+
terminal_weight: 100.0
3430

35-
# -------------------------------------------------------------------------
36-
# Control regularization — uniform penalty on actuator torques
37-
# DeepMind stand task: "Control", Cosh norm (type=3, p=0.3), weight=0.025
38-
# -------------------------------------------------------------------------
3931
control:
40-
R_control: 0.025
32+
R_control: 0.001
4133

42-
# -------------------------------------------------------------------------
43-
# Task-specific weights — used by addXxxCostDerivatives in ilqr.cpp
44-
# -------------------------------------------------------------------------
45-
W_height: 100.0 # DeepMind stand "Height" = 100.0 (SmoothAbsLoss p=0.1)
46-
W_vel: 10.0 # DeepMind stand "CoM Vel." = 10.0 (Base xy velocity damping)
47-
W_joint_vel: 0.01 # DeepMind stand "Joint Vel." = 0.01 (Joint velocity damping)
48-
W_upright: 5.0 # DeepMind "Upright" = 5.0
49-
w_balance: 50.0 # DeepMind stand "Balance" = 50.0 (SmoothAbsLoss p=0.1)
50-
balance_time_constant: 0.2 # Capture point time constant (0.2s for stand, 0.3s for walk)
51-
W_pelvis_feet: 1.0 # DeepMind "Pelvis/Feet" = 1.0 (RectifyLoss, one-sided)
52-
W_walk: 0.0 # DeepMind "Walk" = 1.0 — disabled for standing task
53-
speed_goal: 0.0 # DeepMind default walking speed (m/s). task.xml: residual_Speed data="0.5"
34+
W_height: 100.0
35+
W_vel: 0.0
36+
W_joint_vel: 1.0
37+
W_upright: 20.0
38+
w_balance: 30.0
39+
balance_time_constant: 0.2
40+
W_pelvis_feet: 0.0
41+
W_walk: 0.0
42+
speed_goal: 0.0
5443

55-
# Norm types for each cost term - EXACT DeepMind MJPC stand values
56-
# Available types: quadratic (0), l22 (1), l2 (2), cosh (3), smooth_abs_loss (6), smooth_abs_2_loss (7), rectify (8)
57-
# Parameters: p (regularization), q (power parameter)
5844
norm_types:
59-
# Posture (joint angles [7:nq]) - DISABLED for stand (weight=0)
6045
posture:
61-
type: 0 # Quadratic: x^T * x
46+
type: 0
6247
p: 0.0
6348
q: 1.0
64-
65-
# Height (torso z) - SmoothAbsLoss with p=0.1 (DeepMind stand type 6)
6649
height:
67-
type: 6 # SmoothAbsLoss: sqrt(r^2 + p^2) - p
68-
p: 0.1
50+
type: 0
51+
p: 0.0
6952
q: 1.0
70-
71-
# Balance (capture point) - SmoothAbsLoss with p=0.1 (DeepMind stand type 6)
7253
balance:
73-
type: 6 # SmoothAbsLoss: sqrt(r^2 + p^2) - p
74-
p: 0.1
54+
type: 0
55+
p: 0.0
7556
q: 1.0
76-
77-
# Pelvis/Feet alignment - RectifyLoss p=0.05 (DeepMind walk.cc:52-57)
7857
pelvis_feet:
79-
type: 8 # RectifyLoss: p*log(1 + exp(r/p))
58+
type: 8
8059
p: 0.05
81-
q: 1.0 # Not used for RectifyLoss
82-
83-
# Upright (orientation) - L2 norm with p=0.01
60+
q: 1.0
8461
upright:
85-
type: 2 # L2: sqrt(x*x' + p^2) - p
86-
p: 0.01
87-
q: 1.0 # Not used for L2
88-
89-
# CoM velocity (base xy) - Quadratic norm (DeepMind stand "CoM Vel.")
62+
type: 0
63+
p: 0.0
64+
q: 1.0
9065
velocity:
91-
type: 0 # Quadratic: 0.5 * x^T * x
66+
type: 0
9267
p: 0.0
9368
q: 1.0
94-
95-
# Joint velocity (21 joints) - Quadratic norm (DeepMind stand "Joint Vel.")
9669
joint_vel:
97-
type: 0 # Quadratic: 0.5 * x^T * x
70+
type: 0
9871
p: 0.0
9972
q: 1.0
100-
101-
# Control (actuator torques) - Cosh norm with p=0.3 (DeepMind stand task)
10273
control:
103-
type: 3 # Cosh: p^2 * (cosh(x/p) - 1)
104-
p: 0.3
74+
type: 0
75+
p: 0.0
10576
q: 1.0
106-
107-
# Walk (forward velocity toward speed_goal) - SmoothAbs2Loss p=0.5, q=3.0
10877
walk:
109-
type: 7 # SmoothAbs2Loss
78+
type: 7
11079
p: 0.5
11180
q: 3.0
11281

11382
constraints:
114-
# HARD CONSTRAINTS NOW USED: Control clamping applied in unpackControlToData()
115-
# All soft constraint penalty code removed (Phase 2 complete)
116-
joint_limit_weight: 0.0 # Unused - soft constraints removed
117-
torque_limit_weight: 0.0 # Unused - soft constraints removed
83+
joint_limit_weight: 1500.0
84+
torque_limit_weight: 1500.0
11885

119-
# iLQR solver settings - ONLY parameters actually implemented in our code
120-
# See docs/ilqr_parameter_comparison.md for full details
12186
ilqr_settings:
122-
# Core solver parameters
123-
initial_regularization: 1.0e-3 # Initial regularization value
124-
max_iterations: 10 # Maximum iLQR iterations
125-
tolerance: 1.0e-4 # Convergence tolerance
126-
127-
# Regularization bounds and adaptation (aligned with MJPC where possible)
128-
reg_min: 1.0e-6 # Minimum regularization (MJPC: 1e-6)
129-
reg_max: 1.0e6 # Maximum regularization (MJPC: 1e6, was 100.0)
130-
reg_increase_factor: 10.0 # Factor to increase regularization
131-
reg_decrease_factor: 10.0 # Factor to decrease regularization
132-
133-
# Trust region thresholds for regularization adaptation (aligned with MJPC)
134-
trust_region_good: 0.5 # Good improvement threshold (MJPC: z > 0.5)
135-
trust_region_poor: 0.25 # Poor improvement threshold (MJPC: z < 0.1)
136-
137-
# Line search parameters (log-scale, DeepMind MJPC style)
138-
num_line_search_steps: 30 # Number of line search candidates (MJPC default: 10)
139-
min_linesearch_step: 1.0e-3 # Minimum line search step size (MJPC default: 1e-3)
140-
line_search_tolerance: 1.0e-6 # Line search convergence tolerance
141-
142-
# Quu regularization for numerical stability
143-
quu_regularization: 1.0e-4 # Added to Quu diagonal
144-
145-
# Convergence criterion
146-
convergence_threshold: 1.0e-8 # Cost improvement threshold
147-
148-
# Finite difference parameters (DeepMind MJPC compatible)
149-
fd_tolerance: 1.0e-6 # FD epsilon (DeepMind default: 1e-6)
150-
fd_mode: 1 # 0=forward, 1=centered (DeepMind: configurable)
87+
initial_regularization: 1.0e-3
88+
max_iterations: 20
89+
tolerance: 1.0e-4
90+
reg_min: 1.0e-6
91+
reg_max: 1.0e6
92+
reg_increase_factor: 10.0
93+
reg_decrease_factor: 10.0
94+
trust_region_good: 0.5
95+
trust_region_poor: 0.25
96+
num_line_search_steps: 30
97+
min_linesearch_step: 1.0e-3
98+
line_search_tolerance: 1.0e-6
99+
quu_regularization: 1.0e-4
100+
convergence_threshold: 1.0e-8
101+
fd_tolerance: 1.0e-6
102+
fd_mode: 1
151103

152104
paths:
153105
logs_dir: "logs"

‎config_dm.yaml‎

Lines changed: 162 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,162 @@
1+
robot:
2+
name: dm_humanoid
3+
model_path: "robots/dm_humanoid/humanoid.xml"
4+
urdf_path: "robots/dm_humanoid/humanoid.xml"
5+
6+
ee_feet:
7+
right_feet_ee: "foot_right"
8+
left_feet_ee: "foot_left"
9+
pelvis_body_name: "pelvis" # Body whose z-position is used in Pelvis/Feet cost
10+
torso_body_name: "torso" # Floating-base body (used as one of 4 forward-dir bodies in Walk cost)
11+
waist_lower_body_name: "waist_lower" # DeepMind: waist_lower_subcomvel = subtree CoM vel of this body
12+
13+
reference_trajectory:
14+
q_ref: "data/dm_humanoid/q_standing.csv"
15+
v_ref: "data/dm_humanoid/v_standing.csv"
16+
contact_schedule: "data/dm_humanoid/contact_standing.csv"
17+
18+
mpc:
19+
horizon: 23 # MJPC: 0.35s / 0.015s = 23 steps
20+
dt: 0.015 # MJPC timestep (15ms)
21+
physics_dt: 0.015 # MuJoCo substep (keep fine for stability)
22+
gravity: [0.0, 0.0, -1.0] # Standard gravity (MJPC uses full gravity)
23+
sim_steps: 50 # Total simulation steps
24+
contact_impratio: 100.0 # Contact stiffness
25+
26+
cost_weights:
27+
# -------------------------------------------------------------------------
28+
# Posture cost — penalises joint angles qpos[7:nq] from reference
29+
# DeepMind stand: NO posture term (disabled)
30+
# -------------------------------------------------------------------------
31+
posture:
32+
weight: 0.0 # DISABLED for stand task (DeepMind has no posture)
33+
terminal_weight: 0.1 # Terminal cost weight (independent of running)
34+
35+
# -------------------------------------------------------------------------
36+
# Control regularization — uniform penalty on actuator torques
37+
# DeepMind stand task: "Control", Cosh norm (type=3, p=0.3), weight=0.025
38+
# -------------------------------------------------------------------------
39+
control:
40+
R_control: 0.025
41+
42+
# -------------------------------------------------------------------------
43+
# Task-specific weights — used by addXxxCostDerivatives in ilqr.cpp
44+
# -------------------------------------------------------------------------
45+
W_height: 100.0 # DeepMind stand "Height" = 100.0 (SmoothAbsLoss p=0.1)
46+
W_vel: 10.0 # DeepMind stand "CoM Vel." = 10.0 (Base xy velocity damping)
47+
W_joint_vel: 0.01 # DeepMind stand "Joint Vel." = 0.01 (Joint velocity damping)
48+
W_upright: 5.0 # DeepMind "Upright" = 5.0
49+
w_balance: 50.0 # DeepMind stand "Balance" = 50.0 (SmoothAbsLoss p=0.1)
50+
balance_time_constant: 0.2 # Capture point time constant (0.2s for stand, 0.3s for walk)
51+
W_pelvis_feet: 1.0 # DeepMind "Pelvis/Feet" = 1.0 (RectifyLoss, one-sided)
52+
W_walk: 0.0 # DeepMind "Walk" = 1.0 — disabled for standing task
53+
speed_goal: 0.0 # DeepMind default walking speed (m/s). task.xml: residual_Speed data="0.5"
54+
55+
# Norm types for each cost term - EXACT DeepMind MJPC stand values
56+
# Available types: quadratic (0), l22 (1), l2 (2), cosh (3), smooth_abs_loss (6), smooth_abs_2_loss (7), rectify (8)
57+
# Parameters: p (regularization), q (power parameter)
58+
norm_types:
59+
# Posture (joint angles [7:nq]) - DISABLED for stand (weight=0)
60+
posture:
61+
type: 0 # Quadratic: x^T * x
62+
p: 0.0
63+
q: 1.0
64+
65+
# Height (torso z) - SmoothAbsLoss with p=0.1 (DeepMind stand type 6)
66+
height:
67+
type: 6 # SmoothAbsLoss: sqrt(r^2 + p^2) - p
68+
p: 0.1
69+
q: 1.0
70+
71+
# Balance (capture point) - SmoothAbsLoss with p=0.1 (DeepMind stand type 6)
72+
balance:
73+
type: 6 # SmoothAbsLoss: sqrt(r^2 + p^2) - p
74+
p: 0.1
75+
q: 1.0
76+
77+
# Pelvis/Feet alignment - RectifyLoss p=0.05 (DeepMind walk.cc:52-57)
78+
pelvis_feet:
79+
type: 8 # RectifyLoss: p*log(1 + exp(r/p))
80+
p: 0.05
81+
q: 1.0 # Not used for RectifyLoss
82+
83+
# Upright (orientation) - L2 norm with p=0.01
84+
upright:
85+
type: 2 # L2: sqrt(x*x' + p^2) - p
86+
p: 0.01
87+
q: 1.0 # Not used for L2
88+
89+
# CoM velocity (base xy) - Quadratic norm (DeepMind stand "CoM Vel.")
90+
velocity:
91+
type: 0 # Quadratic: 0.5 * x^T * x
92+
p: 0.0
93+
q: 1.0
94+
95+
# Joint velocity (21 joints) - Quadratic norm (DeepMind stand "Joint Vel.")
96+
joint_vel:
97+
type: 0 # Quadratic: 0.5 * x^T * x
98+
p: 0.0
99+
q: 1.0
100+
101+
# Control (actuator torques) - Cosh norm with p=0.3 (DeepMind stand task)
102+
control:
103+
type: 3 # Cosh: p^2 * (cosh(x/p) - 1)
104+
p: 0.3
105+
q: 1.0
106+
107+
# Walk (forward velocity toward speed_goal) - SmoothAbs2Loss p=0.5, q=3.0
108+
walk:
109+
type: 7 # SmoothAbs2Loss
110+
p: 0.5
111+
q: 3.0
112+
113+
constraints:
114+
# HARD CONSTRAINTS NOW USED: Control clamping applied in unpackControlToData()
115+
# All soft constraint penalty code removed (Phase 2 complete)
116+
joint_limit_weight: 0.0 # Unused - soft constraints removed
117+
torque_limit_weight: 0.0 # Unused - soft constraints removed
118+
119+
# iLQR solver settings - ONLY parameters actually implemented in our code
120+
# See docs/ilqr_parameter_comparison.md for full details
121+
ilqr_settings:
122+
# Core solver parameters
123+
initial_regularization: 1.0e-3 # Initial regularization value
124+
max_iterations: 10 # Maximum iLQR iterations
125+
tolerance: 1.0e-4 # Convergence tolerance
126+
127+
# Regularization bounds and adaptation (aligned with MJPC where possible)
128+
reg_min: 1.0e-6 # Minimum regularization (MJPC: 1e-6)
129+
reg_max: 1.0e6 # Maximum regularization (MJPC: 1e6, was 100.0)
130+
reg_increase_factor: 10.0 # Factor to increase regularization
131+
reg_decrease_factor: 10.0 # Factor to decrease regularization
132+
133+
# Trust region thresholds for regularization adaptation (aligned with MJPC)
134+
trust_region_good: 0.5 # Good improvement threshold (MJPC: z > 0.5)
135+
trust_region_poor: 0.25 # Poor improvement threshold (MJPC: z < 0.1)
136+
137+
# Line search parameters (log-scale, DeepMind MJPC style)
138+
num_line_search_steps: 30 # Number of line search candidates (MJPC default: 10)
139+
min_linesearch_step: 1.0e-3 # Minimum line search step size (MJPC default: 1e-3)
140+
line_search_tolerance: 1.0e-6 # Line search convergence tolerance
141+
142+
# Quu regularization for numerical stability
143+
quu_regularization: 1.0e-4 # Added to Quu diagonal
144+
145+
# Convergence criterion
146+
convergence_threshold: 1.0e-8 # Cost improvement threshold
147+
148+
# Finite difference parameters (DeepMind MJPC compatible)
149+
fd_tolerance: 1.0e-6 # FD epsilon (DeepMind default: 1e-6)
150+
fd_mode: 1 # 0=forward, 1=centered (DeepMind: configurable)
151+
152+
paths:
153+
logs_dir: "logs"
154+
results_dir: "results"
155+
156+
logging:
157+
verbose: true
158+
save_trajectories: true
159+
results_path: "results"
160+
161+
visualization:
162+
enable_rerun: False

0 commit comments

Comments
 (0)