Skip to content

Commit e31792e

Browse files
benjamcbenjamc
committed
Update CARLBipedalWalker
1 parent 4654e94 commit e31792e

File tree

2 files changed

+99
-91
lines changed

2 files changed

+99
-91
lines changed

‎carl/envs/gymnasium/box2d/carl_bipedal_walker.py‎

Lines changed: 1 addition & 91 deletions
Original file line numberDiff line numberDiff line change
@@ -87,6 +87,7 @@ def get_context_features() -> dict[str, ContextFeature]:
8787

8888
def _update_context(self) -> None:
8989
self.env: bipedal_walker.BipedalWalker
90+
self.context = CARLBipedalWalker.get_context_space().insert_defaults(self.context)
9091
bpw.FPS = self.context["FPS"]
9192
bpw.SCALE = self.context["SCALE"]
9293
bpw.FRICTION = self.context["FRICTION"]
@@ -153,94 +154,3 @@ def _update_context(self) -> None:
153154
)
154155

155156
self.env.world.gravity = gravity
156-
157-
158-
def demo_heuristic(env: CARLBipedalWalker | bipedal_walker.BipedalWalker) -> None:
159-
env.reset()
160-
steps = 0
161-
total_reward = 0
162-
a = np.array([0.0, 0.0, 0.0, 0.0])
163-
STAY_ON_ONE_LEG, PUT_OTHER_DOWN, PUSH_OFF = 1, 2, 3
164-
SPEED = 0.29 # Will fall forward on higher speed
165-
state = STAY_ON_ONE_LEG
166-
moving_leg = 0
167-
supporting_leg = 1 - moving_leg
168-
SUPPORT_KNEE_ANGLE = +0.1
169-
supporting_knee_angle = SUPPORT_KNEE_ANGLE
170-
while True:
171-
s, r, terminated, truncated, info = env.step(a)
172-
s = s["state"]
173-
total_reward += r
174-
if steps % 20 == 0 or terminated or truncated:
175-
print("\naction " + str(["{:+0.2f}".format(x) for x in a]))
176-
print("step {} total_reward {:+0.2f}".format(steps, total_reward))
177-
print("hull " + str(["{:+0.2f}".format(x) for x in s[0:4]]))
178-
print("leg0 " + str(["{:+0.2f}".format(x) for x in s[4:9]]))
179-
print("leg1 " + str(["{:+0.2f}".format(x) for x in s[9:14]]))
180-
steps += 1
181-
182-
contact0 = s[8] # noqa: F841
183-
contact1 = s[13] # noqa: F841
184-
moving_s_base = 4 + 5 * moving_leg
185-
supporting_s_base = 4 + 5 * supporting_leg
186-
187-
hip_targ = np.array([None, None]) # -0.8 .. +1.1
188-
knee_targ = np.array([None, None]) # -0.6 .. +0.9
189-
hip_todo = np.array([0.0, 0.0])
190-
knee_todo = np.array([0.0, 0.0])
191-
192-
if state == STAY_ON_ONE_LEG:
193-
hip_targ[moving_leg] = 1.1
194-
knee_targ[moving_leg] = -0.6
195-
supporting_knee_angle += 0.03
196-
if s[2] > SPEED:
197-
supporting_knee_angle += 0.03
198-
supporting_knee_angle = min(supporting_knee_angle, SUPPORT_KNEE_ANGLE)
199-
knee_targ[supporting_leg] = supporting_knee_angle
200-
if s[supporting_s_base + 0] < 0.10: # supporting leg is behind
201-
state = PUT_OTHER_DOWN
202-
if state == PUT_OTHER_DOWN:
203-
hip_targ[moving_leg] = +0.1
204-
knee_targ[moving_leg] = SUPPORT_KNEE_ANGLE
205-
knee_targ[supporting_leg] = supporting_knee_angle
206-
if s[moving_s_base + 4]:
207-
state = PUSH_OFF
208-
supporting_knee_angle = min(s[moving_s_base + 2], SUPPORT_KNEE_ANGLE)
209-
if state == PUSH_OFF:
210-
knee_targ[moving_leg] = supporting_knee_angle
211-
knee_targ[supporting_leg] = +1.0
212-
if s[supporting_s_base + 2] > 0.88 or s[2] > 1.2 * SPEED:
213-
state = STAY_ON_ONE_LEG
214-
moving_leg = 1 - moving_leg
215-
supporting_leg = 1 - moving_leg
216-
217-
if hip_targ[0]:
218-
hip_todo[0] = 0.9 * (hip_targ[0] - s[4]) - 0.25 * s[5]
219-
if hip_targ[1]:
220-
hip_todo[1] = 0.9 * (hip_targ[1] - s[9]) - 0.25 * s[10]
221-
if knee_targ[0]:
222-
knee_todo[0] = 4.0 * (knee_targ[0] - s[6]) - 0.25 * s[7]
223-
if knee_targ[1]:
224-
knee_todo[1] = 4.0 * (knee_targ[1] - s[11]) - 0.25 * s[12]
225-
226-
hip_todo[0] -= 0.9 * (0 - s[0]) - 1.5 * s[1] # PID to keep head strait
227-
hip_todo[1] -= 0.9 * (0 - s[0]) - 1.5 * s[1]
228-
knee_todo[0] -= 15.0 * s[3] # vertical speed, to damp oscillations
229-
knee_todo[1] -= 15.0 * s[3]
230-
231-
a[0] = hip_todo[0]
232-
a[1] = knee_todo[0]
233-
a[2] = hip_todo[1]
234-
a[3] = knee_todo[1]
235-
a = np.clip(0.5 * a, -1.0, 1.0)
236-
237-
env.render()
238-
if terminated or truncated:
239-
break
240-
241-
242-
if __name__ == "__main__":
243-
# Heurisic: suboptimal, have no notion of balance.
244-
env = CARLBipedalWalker()
245-
demo_heuristic(env)
246-
env.close()

‎examples/demo_heuristic_bipedalwalker.py‎

Lines changed: 98 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,98 @@
1+
from carl.envs import CARLBipedalWalker
2+
from gymnasium.envs.box2d import bipedal_walker
3+
import numpy as np
4+
5+
6+
def demo_heuristic(env: CARLBipedalWalker | bipedal_walker.BipedalWalker) -> None:
7+
env.reset()
8+
steps = 0
9+
total_reward = 0
10+
a = np.array([0.0, 0.0, 0.0, 0.0])
11+
STAY_ON_ONE_LEG, PUT_OTHER_DOWN, PUSH_OFF = 1, 2, 3
12+
SPEED = 0.29 # Will fall forward on higher speed
13+
state = STAY_ON_ONE_LEG
14+
moving_leg = 0
15+
supporting_leg = 1 - moving_leg
16+
SUPPORT_KNEE_ANGLE = +0.1
17+
supporting_knee_angle = SUPPORT_KNEE_ANGLE
18+
while True:
19+
s, r, terminated, truncated, info = env.step(a)
20+
s = s["obs"]
21+
total_reward += r
22+
if steps % 20 == 0 or terminated or truncated:
23+
print("\naction " + str(["{:+0.2f}".format(x) for x in a]))
24+
print("step {} total_reward {:+0.2f}".format(steps, total_reward))
25+
print("hull " + str(["{:+0.2f}".format(x) for x in s[0:4]]))
26+
print("leg0 " + str(["{:+0.2f}".format(x) for x in s[4:9]]))
27+
print("leg1 " + str(["{:+0.2f}".format(x) for x in s[9:14]]))
28+
steps += 1
29+
30+
contact0 = s[8] # noqa: F841
31+
contact1 = s[13] # noqa: F841
32+
moving_s_base = 4 + 5 * moving_leg
33+
supporting_s_base = 4 + 5 * supporting_leg
34+
35+
hip_targ = np.array([None, None]) # -0.8 .. +1.1
36+
knee_targ = np.array([None, None]) # -0.6 .. +0.9
37+
hip_todo = np.array([0.0, 0.0])
38+
knee_todo = np.array([0.0, 0.0])
39+
40+
if state == STAY_ON_ONE_LEG:
41+
hip_targ[moving_leg] = 1.1
42+
knee_targ[moving_leg] = -0.6
43+
supporting_knee_angle += 0.03
44+
if s[2] > SPEED:
45+
supporting_knee_angle += 0.03
46+
supporting_knee_angle = min(supporting_knee_angle, SUPPORT_KNEE_ANGLE)
47+
knee_targ[supporting_leg] = supporting_knee_angle
48+
if s[supporting_s_base + 0] < 0.10: # supporting leg is behind
49+
state = PUT_OTHER_DOWN
50+
if state == PUT_OTHER_DOWN:
51+
hip_targ[moving_leg] = +0.1
52+
knee_targ[moving_leg] = SUPPORT_KNEE_ANGLE
53+
knee_targ[supporting_leg] = supporting_knee_angle
54+
if s[moving_s_base + 4]:
55+
state = PUSH_OFF
56+
supporting_knee_angle = min(s[moving_s_base + 2], SUPPORT_KNEE_ANGLE)
57+
if state == PUSH_OFF:
58+
knee_targ[moving_leg] = supporting_knee_angle
59+
knee_targ[supporting_leg] = +1.0
60+
if s[supporting_s_base + 2] > 0.88 or s[2] > 1.2 * SPEED:
61+
state = STAY_ON_ONE_LEG
62+
moving_leg = 1 - moving_leg
63+
supporting_leg = 1 - moving_leg
64+
65+
if hip_targ[0]:
66+
hip_todo[0] = 0.9 * (hip_targ[0] - s[4]) - 0.25 * s[5]
67+
if hip_targ[1]:
68+
hip_todo[1] = 0.9 * (hip_targ[1] - s[9]) - 0.25 * s[10]
69+
if knee_targ[0]:
70+
knee_todo[0] = 4.0 * (knee_targ[0] - s[6]) - 0.25 * s[7]
71+
if knee_targ[1]:
72+
knee_todo[1] = 4.0 * (knee_targ[1] - s[11]) - 0.25 * s[12]
73+
74+
hip_todo[0] -= 0.9 * (0 - s[0]) - 1.5 * s[1] # PID to keep head strait
75+
hip_todo[1] -= 0.9 * (0 - s[0]) - 1.5 * s[1]
76+
knee_todo[0] -= 15.0 * s[3] # vertical speed, to damp oscillations
77+
knee_todo[1] -= 15.0 * s[3]
78+
79+
a[0] = hip_todo[0]
80+
a[1] = knee_todo[0]
81+
a[2] = hip_todo[1]
82+
a[3] = knee_todo[1]
83+
a = np.clip(0.5 * a, -1.0, 1.0)
84+
85+
env.render()
86+
if terminated or truncated:
87+
break
88+
89+
90+
if __name__ == "__main__":
91+
# Heurisic: suboptimal, have no notion of balance.
92+
CARLBipedalWalker.render_mode = "human"
93+
env = CARLBipedalWalker(
94+
# Play with leg height :)
95+
contexts = {0: {"LEG_H": 4}}
96+
)
97+
demo_heuristic(env)
98+
env.close()

0 commit comments

Comments
 (0)