tinysim 0.0.1__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- tinysim/__init__.py +11 -0
- tinysim/_tk_base.py +54 -0
- tinysim/flappy/__init__.py +103 -0
- tinysim/flappy/sim.js +118 -0
- tinysim/flappy/tk.py +94 -0
- tinysim/flappy/widget.py +53 -0
- tinysim/frogger/__init__.py +145 -0
- tinysim/frogger/sim.js +110 -0
- tinysim/frogger/tk.py +95 -0
- tinysim/frogger/widget.py +60 -0
- tinysim/mountain_car/__init__.py +56 -0
- tinysim/mountain_car/sim.js +158 -0
- tinysim/mountain_car/styles.css +5 -0
- tinysim/mountain_car/tk.py +141 -0
- tinysim/mountain_car/widget.py +56 -0
- tinysim/simple_amr/__init__.py +0 -0
- tinysim/simple_amr/example_maps.py +121 -0
- tinysim/simple_amr/sim.js +430 -0
- tinysim/simple_amr/styles.css +54 -0
- tinysim/simple_amr/widget.py +73 -0
- tinysim/topdown_driving/__init__.py +190 -0
- tinysim/topdown_driving/sim.js +136 -0
- tinysim/topdown_driving/tk.py +180 -0
- tinysim/topdown_driving/track_0.json +753 -0
- tinysim/topdown_driving/widget.py +60 -0
- tinysim-0.0.1.dist-info/METADATA +56 -0
- tinysim-0.0.1.dist-info/RECORD +33 -0
- tinysim-0.0.1.dist-info/WHEEL +5 -0
- tinysim-0.0.1.dist-info/top_level.txt +2 -0
- tinysim_warp/cart_pole/__init__.py +259 -0
- tinysim_warp/quadruped/__init__.py +203 -0
- tinysim_warp/simple_quadruped/__init__.py +168 -0
- tinysim_warp/simple_quadruped/simple_quadruped.urdf +247 -0
|
@@ -0,0 +1,60 @@
|
|
|
1
|
+
import pathlib
|
|
2
|
+
import anywidget
|
|
3
|
+
import traitlets
|
|
4
|
+
import asyncio
|
|
5
|
+
import numpy as np
|
|
6
|
+
from IPython.display import display
|
|
7
|
+
from jupyter_ui_poll import ui_events
|
|
8
|
+
|
|
9
|
+
from . import TopDownDrivingEnv, LOCAL_WALLS
|
|
10
|
+
|
|
11
|
+
|
|
12
|
+
class TopDownDrivingWidget(anywidget.AnyWidget):
|
|
13
|
+
_esm = pathlib.Path(__file__).parent / "sim.js"
|
|
14
|
+
|
|
15
|
+
sim_state = traitlets.Dict(default_value={}).tag(sync=True)
|
|
16
|
+
wall_positions = traitlets.List(default_value=LOCAL_WALLS).tag(sync=True)
|
|
17
|
+
|
|
18
|
+
_viewport_size = traitlets.Tuple(
|
|
19
|
+
traitlets.Int(), traitlets.Int(), default_value=(800, 600)
|
|
20
|
+
).tag(sync=True)
|
|
21
|
+
_view_ready = traitlets.Bool(default_value=False).tag(sync=True)
|
|
22
|
+
|
|
23
|
+
def __init__(self, viewport_size=(800, 600), sim_env=None):
|
|
24
|
+
self._viewport_size = viewport_size
|
|
25
|
+
super().__init__()
|
|
26
|
+
if sim_env is None:
|
|
27
|
+
sim_env = TopDownDrivingEnv()
|
|
28
|
+
|
|
29
|
+
self.sim_env = sim_env
|
|
30
|
+
self.copy_py_state(self.sim_env.reset())
|
|
31
|
+
|
|
32
|
+
def render(self):
|
|
33
|
+
display(self)
|
|
34
|
+
|
|
35
|
+
try:
|
|
36
|
+
with ui_events() as ui_poll:
|
|
37
|
+
while not self._view_ready:
|
|
38
|
+
ui_poll(100)
|
|
39
|
+
except Exception:
|
|
40
|
+
pass
|
|
41
|
+
|
|
42
|
+
|
|
43
|
+
def copy_py_state(self, sim_state: dict):
|
|
44
|
+
self.sim_state = {
|
|
45
|
+
"x": sim_state["x"].tolist(),
|
|
46
|
+
"y": sim_state["y"].tolist(),
|
|
47
|
+
"angle": sim_state["angle"].tolist(),
|
|
48
|
+
}
|
|
49
|
+
|
|
50
|
+
async def step(self, action: int, dt: float = 0.01) -> dict:
|
|
51
|
+
sim_state = self.sim_env.step(action)
|
|
52
|
+
self.copy_py_state(sim_state)
|
|
53
|
+
|
|
54
|
+
await asyncio.sleep(dt)
|
|
55
|
+
return sim_state
|
|
56
|
+
|
|
57
|
+
async def reset(self) -> dict:
|
|
58
|
+
sim_state = self.sim_env.reset()
|
|
59
|
+
self.copy_py_state(sim_state)
|
|
60
|
+
return sim_state
|
|
@@ -0,0 +1,56 @@
|
|
|
1
|
+
Metadata-Version: 2.4
|
|
2
|
+
Name: tinysim
|
|
3
|
+
Version: 0.0.1
|
|
4
|
+
Summary: small modular simulation environments
|
|
5
|
+
Author-email: Matthew Taylor <matthew.taylor.andre@gmail.com>
|
|
6
|
+
Project-URL: Homepage, https://github.com/MatthewAndreTaylor/TinySim
|
|
7
|
+
Keywords: jupyter,simulation
|
|
8
|
+
Classifier: Framework :: Jupyter
|
|
9
|
+
Classifier: Framework :: Jupyter :: JupyterLab
|
|
10
|
+
Classifier: Framework :: Jupyter :: JupyterLab :: 4
|
|
11
|
+
Classifier: Framework :: Jupyter :: JupyterLab :: Extensions
|
|
12
|
+
Classifier: Programming Language :: Python
|
|
13
|
+
Classifier: Programming Language :: Python :: 3
|
|
14
|
+
Classifier: Programming Language :: Python :: 3.9
|
|
15
|
+
Classifier: Programming Language :: Python :: 3.10
|
|
16
|
+
Classifier: Programming Language :: Python :: 3.11
|
|
17
|
+
Classifier: Programming Language :: Python :: 3.12
|
|
18
|
+
Classifier: Programming Language :: Python :: 3.13
|
|
19
|
+
Requires-Python: >=3.9
|
|
20
|
+
Description-Content-Type: text/markdown
|
|
21
|
+
Requires-Dist: numpy
|
|
22
|
+
Requires-Dist: anywidget
|
|
23
|
+
Requires-Dist: jupyter-ui-poll
|
|
24
|
+
Provides-Extra: warp
|
|
25
|
+
Requires-Dist: warp-lang==1.8.1; extra == "warp"
|
|
26
|
+
|
|
27
|
+
|
|
28
|
+
# TinySim
|
|
29
|
+
|
|
30
|
+
[](https://pypi.org/project/coderbot-sim)
|
|
31
|
+
[](https://matthewandretaylor.github.io/CoderBots/lab?path=example.ipynb)
|
|
32
|
+
|
|
33
|
+
The goal of this project is to create more minimal simulation environments for robotics and machine learning.
|
|
34
|
+
|
|
35
|
+
Existing solutions like [ROS 2](https://github.com/ros2) and [Gymnasium](https://gymnasium.farama.org/) come with many heavy dependencies.
|
|
36
|
+
|
|
37
|
+
Additionally they are usually difficult to get setup and may require domain specific knowledge to use.
|
|
38
|
+
Simulation environments should be able to run in any notebook environment and require fewer dependencies.
|
|
39
|
+
|
|
40
|
+
|
|
41
|
+
## Get Started 🚀
|
|
42
|
+
|
|
43
|
+
To use the baseline Python only simulation environments.
|
|
44
|
+
|
|
45
|
+
```bash
|
|
46
|
+
pip install tinysim
|
|
47
|
+
```
|
|
48
|
+
|
|
49
|
+
To use the [warp](https://github.com/NVIDIA/warp) simulation environments.
|
|
50
|
+
|
|
51
|
+
```bash
|
|
52
|
+
pip install tinysim[warp]
|
|
53
|
+
```
|
|
54
|
+
|
|
55
|
+
|
|
56
|
+
|
|
@@ -0,0 +1,33 @@
|
|
|
1
|
+
tinysim/__init__.py,sha256=ptkxqwWun-Tw-L2-zUTm2fqunNgdLBkdU3DHF1wAMBI,197
|
|
2
|
+
tinysim/_tk_base.py,sha256=kzF-41H56A5FWNxtMhlvEbl3iH8Wp04zTqAxTcjRPDM,1279
|
|
3
|
+
tinysim/flappy/__init__.py,sha256=lUHGteFeYRb2vQ3-x9WxnKWM7sfsDwIHoVps732JIDY,3482
|
|
4
|
+
tinysim/flappy/sim.js,sha256=g8ycKdJGopu0Z5CeBgtF8Rr2Pe64dTFcSiYh7hzYPzU,3190
|
|
5
|
+
tinysim/flappy/tk.py,sha256=DUJqy0uPn-AvNE4tuA3a4Dh6xaj0twsUOP8leqGgBX4,2458
|
|
6
|
+
tinysim/flappy/widget.py,sha256=zyaoW-rWgAkmbCY7yrSYyAZDUQaAPnuQEzldSawkOnI,1597
|
|
7
|
+
tinysim/frogger/__init__.py,sha256=gA2jvG_WDZRpZY0Zg_aJmWrdsk2siIL49wjFTRfOSxo,5227
|
|
8
|
+
tinysim/frogger/sim.js,sha256=b1HfEtFZFbbsrZNtltuWO4h-QM4Fw3XjMRydwm5l2NU,3081
|
|
9
|
+
tinysim/frogger/tk.py,sha256=3A1wnWcOBCoRNSI4-oHFbMpRwf0iwdV54GBgtAxVufo,2823
|
|
10
|
+
tinysim/frogger/widget.py,sha256=Bvb6aUiVqJTHTdd6EzpCp0FqpRa0bl9RNcK12_weiPM,1810
|
|
11
|
+
tinysim/mountain_car/__init__.py,sha256=V3Qxj_ydWAqOJiQUqmIRegBe9GNjU82BfbG2XvV0OQg,1986
|
|
12
|
+
tinysim/mountain_car/sim.js,sha256=z7kxl7LbkRglfzXxmrBbwz2vMYdQV84QP3WhE0hoPVQ,4581
|
|
13
|
+
tinysim/mountain_car/styles.css,sha256=a8Bc64RdRNB1uWsBdpMEQA9STZO8mVAB2vvIPhhYuik,63
|
|
14
|
+
tinysim/mountain_car/tk.py,sha256=EbnLirA0Q5egJaDUzu_WkGjbofbclwYUcHIvvifNt0Y,4256
|
|
15
|
+
tinysim/mountain_car/widget.py,sha256=bUIXsofy5moBXMDBdIOVvRi59NFbLRRQ1WZcwm_mYG8,1726
|
|
16
|
+
tinysim/simple_amr/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
|
17
|
+
tinysim/simple_amr/example_maps.py,sha256=38duuHbd6HvhQBIIcg9PWokNMjkasPSsKPjHnBhv9qQ,3307
|
|
18
|
+
tinysim/simple_amr/sim.js,sha256=2YW2YWEEe-QWgJGLAdpt1aZi__sLL5ML84fv1s90poY,11575
|
|
19
|
+
tinysim/simple_amr/styles.css,sha256=t85kXf0xyrq5ldL-j_pV0FEekWoorK63T_ZorWEgBOw,1087
|
|
20
|
+
tinysim/simple_amr/widget.py,sha256=dmNcftB7-Z9Kf3hQ0MBaJvo97eyjCA6R74oEkHASMas,2557
|
|
21
|
+
tinysim/topdown_driving/__init__.py,sha256=HiRpxQNeu4IIOPFraGHLTO2F0VNISAk6bUiK1rsbZTE,6228
|
|
22
|
+
tinysim/topdown_driving/sim.js,sha256=5jmyQk4zp4KFhaG-uTsW_Cq1gHoIkR9d2Cv_ezDKarM,3076
|
|
23
|
+
tinysim/topdown_driving/tk.py,sha256=8UB95ZjIJiUQeXC5w3pXkjukOJXfisLKrfIn0bVFzjU,4751
|
|
24
|
+
tinysim/topdown_driving/track_0.json,sha256=3zPfPX72s5zxs4znoO8rXBqGNciai10Jbl2rFCaACow,15338
|
|
25
|
+
tinysim/topdown_driving/widget.py,sha256=UqhV9Dps8BQTwTFP1g2wlmzql2ooAhAU8-YF8OmpoD8,1734
|
|
26
|
+
tinysim_warp/cart_pole/__init__.py,sha256=fITV-kzKqhvgqek4_qBkL1OeAYFfdpv1Xp7PkM9h0EY,8349
|
|
27
|
+
tinysim_warp/quadruped/__init__.py,sha256=GZUb4GGaVIVTgnm98DEthSmZ36uWxsGeHH7qafawtOE,6398
|
|
28
|
+
tinysim_warp/simple_quadruped/__init__.py,sha256=iXnFzJQzDhhJtK0EYgJF8_4r-QMb7QtEbmdHxcul2_M,5334
|
|
29
|
+
tinysim_warp/simple_quadruped/simple_quadruped.urdf,sha256=UfyGROUD5Qlf_t5ISipE2L7uQPBl-hwFnmRsjNL9yH4,8311
|
|
30
|
+
tinysim-0.0.1.dist-info/METADATA,sha256=T3JEKYm1o7dE59b_gMb_6PnYWEFz3MDWi6kJS-GBNfw,1954
|
|
31
|
+
tinysim-0.0.1.dist-info/WHEEL,sha256=_zCd3N1l69ArxyTb8rzEoP9TpbYXkqRFSNOD5OuxnTs,91
|
|
32
|
+
tinysim-0.0.1.dist-info/top_level.txt,sha256=JsBSgxajxod2mlNodBvwkBsn3vMHse0jLqqni2Q5-9Y,21
|
|
33
|
+
tinysim-0.0.1.dist-info/RECORD,,
|
|
@@ -0,0 +1,259 @@
|
|
|
1
|
+
# written by: Matthew Taylor
|
|
2
|
+
# source: https://github.com/MatthewAndreTaylor/WarpSimBalance
|
|
3
|
+
|
|
4
|
+
try:
|
|
5
|
+
import warp as wp
|
|
6
|
+
import warp.sim.render
|
|
7
|
+
except ImportError:
|
|
8
|
+
raise ImportError(
|
|
9
|
+
"Warp is not installed. Install using `pip install coderbot_sim[warp]`"
|
|
10
|
+
)
|
|
11
|
+
|
|
12
|
+
import numpy as np
|
|
13
|
+
|
|
14
|
+
|
|
15
|
+
def compute_env_offsets(num_envs, env_offset=(5.0, 0.0, 0.0)):
|
|
16
|
+
env_offset = np.array(env_offset, dtype=float)
|
|
17
|
+
axis = np.nonzero(env_offset)[0]
|
|
18
|
+
axis = axis[0] if len(axis) else 0
|
|
19
|
+
env_offsets = np.zeros((num_envs, 3))
|
|
20
|
+
env_offsets[:, axis] = np.arange(num_envs) * env_offset[axis]
|
|
21
|
+
env_offsets -= env_offsets.mean(axis=0)
|
|
22
|
+
env_offsets[:, 1] = 0.0
|
|
23
|
+
return env_offsets
|
|
24
|
+
|
|
25
|
+
|
|
26
|
+
class CartPoleExample:
|
|
27
|
+
def __init__(self, use_cuda_graph=False, headless=False, num_envs=3):
|
|
28
|
+
self.actions = np.array([0.0, 0.5, -0.5])
|
|
29
|
+
self.num_envs = num_envs
|
|
30
|
+
|
|
31
|
+
fps = 60
|
|
32
|
+
self.frame_dt = 1.0 / fps
|
|
33
|
+
self.sim_substeps = 10
|
|
34
|
+
self.sim_dt = self.frame_dt / self.sim_substeps
|
|
35
|
+
|
|
36
|
+
self.initialize()
|
|
37
|
+
|
|
38
|
+
self.integrator = wp.sim.SemiImplicitIntegrator()
|
|
39
|
+
self.renderer = wp.sim.render.SimRendererOpenGL(
|
|
40
|
+
self.model, "example", headless=headless
|
|
41
|
+
)
|
|
42
|
+
|
|
43
|
+
# CUDA graph
|
|
44
|
+
self.use_cuda_graph = (
|
|
45
|
+
use_cuda_graph
|
|
46
|
+
and wp.get_device().is_cuda
|
|
47
|
+
and wp.is_mempool_enabled(wp.get_device())
|
|
48
|
+
)
|
|
49
|
+
if self.use_cuda_graph:
|
|
50
|
+
with wp.ScopedCapture() as capture:
|
|
51
|
+
self.simulate()
|
|
52
|
+
self.graph = capture.graph
|
|
53
|
+
|
|
54
|
+
self.step(actions=[1] * self.num_envs)
|
|
55
|
+
|
|
56
|
+
def create_cartpole(self):
|
|
57
|
+
"""Create cartpole system using pure Python/Warp API"""
|
|
58
|
+
builder = wp.sim.ModelBuilder(gravity=-0.3)
|
|
59
|
+
pole_size = wp.vec3(0.04, 1.0, 0.06)
|
|
60
|
+
cart_body = builder.add_body(
|
|
61
|
+
origin=wp.transform(wp.vec3(0.0, 2.0, 0.0), wp.quat_identity()), m=1.0
|
|
62
|
+
)
|
|
63
|
+
builder.add_shape_sphere(body=cart_body, radius=0.1, density=100.0)
|
|
64
|
+
pole_body = builder.add_body(
|
|
65
|
+
origin=wp.transform(wp.vec3(0.0, 2.5, 0.0), wp.quat_identity()), m=0.01
|
|
66
|
+
)
|
|
67
|
+
builder.add_shape_box(
|
|
68
|
+
body=pole_body,
|
|
69
|
+
pos=wp.vec3(0.0, 0.0, 0.0),
|
|
70
|
+
hx=pole_size[0] / 2.0,
|
|
71
|
+
hy=pole_size[1] / 2.0,
|
|
72
|
+
hz=pole_size[2] / 2.0,
|
|
73
|
+
density=50.0,
|
|
74
|
+
)
|
|
75
|
+
builder.add_joint_revolute(
|
|
76
|
+
parent=cart_body,
|
|
77
|
+
child=pole_body,
|
|
78
|
+
axis=wp.vec3(0.0, 0.0, 1.0), # rotation around Z-axis
|
|
79
|
+
parent_xform=wp.transform(wp.vec3(0.0, 0.0, 0.0), wp.quat_identity()),
|
|
80
|
+
child_xform=wp.transform(
|
|
81
|
+
wp.vec3(0.0, -0.5, 0.0), wp.quat_identity()
|
|
82
|
+
), # joint at pole base
|
|
83
|
+
limit_ke=1.0e4,
|
|
84
|
+
limit_kd=1.0e1,
|
|
85
|
+
)
|
|
86
|
+
builder.add_joint_prismatic(
|
|
87
|
+
parent=-1,
|
|
88
|
+
child=cart_body,
|
|
89
|
+
axis=wp.vec3(1.0, 0.0, 0.0),
|
|
90
|
+
parent_xform=wp.transform(wp.vec3(0.0, 2.0, 0.0), wp.quat_identity()),
|
|
91
|
+
child_xform=wp.transform(wp.vec3(0.0, 0.0, 0.0), wp.quat_identity()),
|
|
92
|
+
limit_lower=-5.0,
|
|
93
|
+
limit_upper=5.0,
|
|
94
|
+
limit_ke=1.0e4,
|
|
95
|
+
limit_kd=1.0e2,
|
|
96
|
+
)
|
|
97
|
+
builder.joint_axis_mode = [wp.sim.JOINT_MODE_FORCE] * len(
|
|
98
|
+
builder.joint_axis_mode
|
|
99
|
+
)
|
|
100
|
+
return builder
|
|
101
|
+
|
|
102
|
+
def set_cart_trajectory(self, actions):
|
|
103
|
+
if len(actions) != self.num_envs:
|
|
104
|
+
raise ValueError("Length of actions must match number of environments.")
|
|
105
|
+
|
|
106
|
+
act = np.zeros(self.model.joint_dof_count, dtype=np.float32)
|
|
107
|
+
dof_indices = np.arange(self.num_envs) * 2 + 1
|
|
108
|
+
act[dof_indices] = self.actions[actions]
|
|
109
|
+
wp.copy(self.model.joint_act, wp.array(act, dtype=wp.float32))
|
|
110
|
+
|
|
111
|
+
def is_fallen(self, pole_quat):
|
|
112
|
+
qw = float(np.clip(pole_quat[3], -1.0, 1.0))
|
|
113
|
+
tilt = 2.0 * np.arccos(qw) # radians, in [0, pi]
|
|
114
|
+
max_tilt = np.deg2rad(90.0)
|
|
115
|
+
return tilt > max_tilt
|
|
116
|
+
|
|
117
|
+
def step(self, actions):
|
|
118
|
+
"""Apply action and step the simulation for one environment timestep.
|
|
119
|
+
|
|
120
|
+
Returns (observation, reward, terminated)
|
|
121
|
+
"""
|
|
122
|
+
self.set_cart_trajectory(actions)
|
|
123
|
+
if self.use_cuda_graph:
|
|
124
|
+
wp.capture_launch(self.graph)
|
|
125
|
+
else:
|
|
126
|
+
self.simulate()
|
|
127
|
+
|
|
128
|
+
self.sim_time += self.frame_dt
|
|
129
|
+
obs = self.get_state_vector()
|
|
130
|
+
pole_quat = obs[1:5]
|
|
131
|
+
terminated = self.is_fallen(pole_quat)
|
|
132
|
+
return obs, 1.0, terminated
|
|
133
|
+
|
|
134
|
+
def simulate(self):
|
|
135
|
+
for _ in range(self.sim_substeps):
|
|
136
|
+
self.state.clear_forces()
|
|
137
|
+
self.state = self.integrator.simulate(
|
|
138
|
+
self.model, self.state, self.state, self.sim_dt
|
|
139
|
+
)
|
|
140
|
+
|
|
141
|
+
def reset(self):
|
|
142
|
+
self.initialize()
|
|
143
|
+
return self.step(actions=[1] * self.num_envs)
|
|
144
|
+
|
|
145
|
+
def initialize(self):
|
|
146
|
+
self.sim_time = 0.0
|
|
147
|
+
builder = wp.sim.ModelBuilder()
|
|
148
|
+
offsets = compute_env_offsets(self.num_envs)
|
|
149
|
+
|
|
150
|
+
for i in range(self.num_envs):
|
|
151
|
+
builder.add_builder(
|
|
152
|
+
self.create_cartpole(),
|
|
153
|
+
xform=wp.transform(offsets[i], wp.quat_identity()),
|
|
154
|
+
)
|
|
155
|
+
|
|
156
|
+
self.model = builder.finalize()
|
|
157
|
+
self.model.joint_attach_ke = 1000.0
|
|
158
|
+
self.model.joint_attach_kd = 1.0
|
|
159
|
+
self.state = self.model.state()
|
|
160
|
+
|
|
161
|
+
wp.sim.eval_fk(
|
|
162
|
+
self.model, self.model.joint_q, self.model.joint_qd, None, self.state
|
|
163
|
+
)
|
|
164
|
+
self.joint_act_np = np.zeros(self.model.joint_dof_count, dtype=np.float32)
|
|
165
|
+
self.joint_act_wp = wp.array(
|
|
166
|
+
self.joint_act_np, dtype=float, device=wp.get_device()
|
|
167
|
+
)
|
|
168
|
+
self.model.joint_act = self.joint_act_wp
|
|
169
|
+
|
|
170
|
+
def render(self):
|
|
171
|
+
self.renderer.begin_frame(self.sim_time)
|
|
172
|
+
self.renderer.render(self.state)
|
|
173
|
+
self.renderer.end_frame()
|
|
174
|
+
|
|
175
|
+
def get_state_vector(self):
|
|
176
|
+
cart_id = 0
|
|
177
|
+
pole_id = 1
|
|
178
|
+
|
|
179
|
+
# convert transforms to numpy
|
|
180
|
+
body_q = self.state.body_q.numpy()
|
|
181
|
+
body_qd = self.state.body_qd.numpy()
|
|
182
|
+
|
|
183
|
+
# --- Cart ---
|
|
184
|
+
cart_pos = body_q[cart_id] # [px, py, pz, qx, qy, qz, qw]
|
|
185
|
+
# cart_pos = cart_pos[[0, 2]]
|
|
186
|
+
cart_pos = cart_pos[[0]]
|
|
187
|
+
|
|
188
|
+
# --- Pole ---
|
|
189
|
+
pole_quat = body_q[pole_id][3:] # quaternion part
|
|
190
|
+
pole_vel = body_qd[pole_id][3:] # angular velocity part
|
|
191
|
+
|
|
192
|
+
return np.concatenate([cart_pos, pole_quat, pole_vel])
|
|
193
|
+
|
|
194
|
+
|
|
195
|
+
if __name__ == "__main__":
|
|
196
|
+
import argparse
|
|
197
|
+
|
|
198
|
+
parser = argparse.ArgumentParser(
|
|
199
|
+
formatter_class=argparse.ArgumentDefaultsHelpFormatter
|
|
200
|
+
)
|
|
201
|
+
parser.add_argument(
|
|
202
|
+
"--device", type=str, default=None, help="Override the default Warp device."
|
|
203
|
+
)
|
|
204
|
+
parser.add_argument(
|
|
205
|
+
"--num-frames", type=int, default=1200, help="Total number of frames."
|
|
206
|
+
)
|
|
207
|
+
parser.add_argument(
|
|
208
|
+
"--manual-control", action="store_true", help="Enable manual control."
|
|
209
|
+
)
|
|
210
|
+
|
|
211
|
+
args = parser.parse_known_args()[0]
|
|
212
|
+
|
|
213
|
+
if args.manual_control:
|
|
214
|
+
print("Manual control enabled")
|
|
215
|
+
import keyboard
|
|
216
|
+
|
|
217
|
+
with wp.ScopedDevice(args.device):
|
|
218
|
+
example = CartPoleExample(use_cuda_graph=True)
|
|
219
|
+
|
|
220
|
+
terminated = False
|
|
221
|
+
check_terminated = True
|
|
222
|
+
|
|
223
|
+
for i in range(args.num_frames):
|
|
224
|
+
if args.manual_control:
|
|
225
|
+
# Manual control logic
|
|
226
|
+
if keyboard.is_pressed("k"):
|
|
227
|
+
action = 2
|
|
228
|
+
elif keyboard.is_pressed("l"):
|
|
229
|
+
action = 1
|
|
230
|
+
else:
|
|
231
|
+
action = 0
|
|
232
|
+
|
|
233
|
+
else:
|
|
234
|
+
# Example control signals
|
|
235
|
+
if i < 200:
|
|
236
|
+
action = 0
|
|
237
|
+
elif i < 255: # 260
|
|
238
|
+
action = 2
|
|
239
|
+
else:
|
|
240
|
+
action = 0
|
|
241
|
+
|
|
242
|
+
obs, reward, terminated = example.step(actions=[action] * example.num_envs)
|
|
243
|
+
# print(f"Step {i}")
|
|
244
|
+
|
|
245
|
+
if check_terminated and terminated:
|
|
246
|
+
print("Pole fallen")
|
|
247
|
+
check_terminated = False
|
|
248
|
+
|
|
249
|
+
example.render()
|
|
250
|
+
|
|
251
|
+
# print("State Vector:", obs)
|
|
252
|
+
# print("Reward:", reward)
|
|
253
|
+
# print("Terminated:", terminated)
|
|
254
|
+
|
|
255
|
+
state = example.reset()
|
|
256
|
+
print("Reset State Vector:", state)
|
|
257
|
+
example.render()
|
|
258
|
+
|
|
259
|
+
example.renderer.save()
|
|
@@ -0,0 +1,203 @@
|
|
|
1
|
+
import math
|
|
2
|
+
import os
|
|
3
|
+
|
|
4
|
+
import numpy as np
|
|
5
|
+
|
|
6
|
+
try:
|
|
7
|
+
import warp as wp
|
|
8
|
+
import warp.examples
|
|
9
|
+
import warp.sim.render
|
|
10
|
+
except ImportError:
|
|
11
|
+
raise ImportError(
|
|
12
|
+
"Warp is not installed. Install using `pip install coderbot_sim[warp]`"
|
|
13
|
+
)
|
|
14
|
+
|
|
15
|
+
|
|
16
|
+
def compute_env_offsets(num_envs, env_offset=(5.0, 0.0, 0.0)):
|
|
17
|
+
env_offset = np.array(env_offset, dtype=float)
|
|
18
|
+
axis = np.nonzero(env_offset)[0]
|
|
19
|
+
axis = axis[0] if len(axis) else 0
|
|
20
|
+
env_offsets = np.zeros((num_envs, 3))
|
|
21
|
+
env_offsets[:, axis] = np.arange(num_envs) * env_offset[axis]
|
|
22
|
+
env_offsets -= env_offsets.mean(axis=0)
|
|
23
|
+
env_offsets[:, 1] = 0.0
|
|
24
|
+
return env_offsets
|
|
25
|
+
|
|
26
|
+
|
|
27
|
+
class RobotDogExample:
|
|
28
|
+
def __init__(self, use_cuda_graph=False, headless=False, num_envs=8):
|
|
29
|
+
articulation_builder = wp.sim.ModelBuilder()
|
|
30
|
+
rot_x = wp.quat_from_axis_angle(wp.vec3(1.0, 0.0, 0.0), -math.pi * 0.5)
|
|
31
|
+
rot_y = wp.quat_from_axis_angle(wp.vec3(0.0, 1.0, 0.0), math.pi * 0.5)
|
|
32
|
+
xform = wp.transform(wp.vec3(0.0, 0.65, 0.0), rot_y * rot_x)
|
|
33
|
+
wp.sim.parse_urdf(
|
|
34
|
+
os.path.join(warp.examples.get_asset_directory(), "quadruped.urdf"),
|
|
35
|
+
articulation_builder,
|
|
36
|
+
xform=xform,
|
|
37
|
+
floating=True,
|
|
38
|
+
density=900,
|
|
39
|
+
armature=0.01,
|
|
40
|
+
stiffness=200,
|
|
41
|
+
damping=1,
|
|
42
|
+
contact_ke=1.0e4,
|
|
43
|
+
contact_kd=1.0e2,
|
|
44
|
+
contact_kf=1.0e2,
|
|
45
|
+
contact_mu=1.0,
|
|
46
|
+
limit_ke=1.0e4,
|
|
47
|
+
limit_kd=1.0e1,
|
|
48
|
+
)
|
|
49
|
+
|
|
50
|
+
builder = wp.sim.ModelBuilder()
|
|
51
|
+
|
|
52
|
+
self.sim_time = 0.0
|
|
53
|
+
fps = 100
|
|
54
|
+
self.frame_dt = 1.0 / fps
|
|
55
|
+
|
|
56
|
+
self.sim_substeps = 10
|
|
57
|
+
self.sim_dt = self.frame_dt / self.sim_substeps
|
|
58
|
+
|
|
59
|
+
self.num_envs = num_envs
|
|
60
|
+
offsets = compute_env_offsets(self.num_envs)
|
|
61
|
+
|
|
62
|
+
for i in range(self.num_envs):
|
|
63
|
+
builder.add_builder(
|
|
64
|
+
articulation_builder, xform=wp.transform(offsets[i], wp.quat_identity())
|
|
65
|
+
)
|
|
66
|
+
builder.joint_q[-12:] = [0.2, 0.4, -0.6, -0.2, -0.4, 0.6, -0.2, 0.4, -0.6, 0.2, -0.4, 0.6] # fmt: skip
|
|
67
|
+
builder.joint_act[-12:] = [0.2, 0.4, -0.6, -0.2, -0.4, 0.6, -0.2, 0.4, -0.6, 0.2, -0.4, 0.6] # fmt: skip
|
|
68
|
+
builder.joint_axis_mode = [wp.sim.JOINT_MODE_TARGET_POSITION] * len(
|
|
69
|
+
builder.joint_axis_mode
|
|
70
|
+
)
|
|
71
|
+
|
|
72
|
+
# finalize model
|
|
73
|
+
self.model = builder.finalize()
|
|
74
|
+
self.model.ground = True
|
|
75
|
+
self.model.joint_attach_ke = 16000.0
|
|
76
|
+
self.model.joint_attach_kd = 200.0
|
|
77
|
+
self.use_tile_gemm = False
|
|
78
|
+
self.fuse_cholesky = False
|
|
79
|
+
|
|
80
|
+
self.integrator = wp.sim.FeatherstoneIntegrator(
|
|
81
|
+
self.model,
|
|
82
|
+
use_tile_gemm=self.use_tile_gemm,
|
|
83
|
+
fuse_cholesky=self.fuse_cholesky,
|
|
84
|
+
)
|
|
85
|
+
|
|
86
|
+
self.renderer = wp.sim.render.SimRendererOpenGL(
|
|
87
|
+
self.model, "example", headless=headless
|
|
88
|
+
)
|
|
89
|
+
self.state_0 = self.model.state()
|
|
90
|
+
self.state_1 = self.model.state()
|
|
91
|
+
|
|
92
|
+
wp.sim.eval_fk(
|
|
93
|
+
self.model, self.model.joint_q, self.model.joint_qd, None, self.state_0
|
|
94
|
+
)
|
|
95
|
+
|
|
96
|
+
self.use_cuda_graph = (
|
|
97
|
+
use_cuda_graph
|
|
98
|
+
and wp.get_device().is_cuda
|
|
99
|
+
and wp.is_mempool_enabled(wp.get_device())
|
|
100
|
+
)
|
|
101
|
+
if self.use_cuda_graph:
|
|
102
|
+
with wp.ScopedCapture() as capture:
|
|
103
|
+
self.simulate()
|
|
104
|
+
self.graph = capture.graph
|
|
105
|
+
|
|
106
|
+
def simulate(self):
|
|
107
|
+
for _ in range(self.sim_substeps):
|
|
108
|
+
self.state_0.clear_forces()
|
|
109
|
+
wp.sim.collide(self.model, self.state_0)
|
|
110
|
+
self.integrator.simulate(
|
|
111
|
+
self.model, self.state_0, self.state_1, self.sim_dt
|
|
112
|
+
)
|
|
113
|
+
self.state_0, self.state_1 = self.state_1, self.state_0
|
|
114
|
+
|
|
115
|
+
def set_leg_poses(self, actions):
|
|
116
|
+
action_space_size = 12 * self.num_envs
|
|
117
|
+
if len(actions) != action_space_size:
|
|
118
|
+
raise ValueError(
|
|
119
|
+
f"Expected {action_space_size} actions, but got {len(actions)}"
|
|
120
|
+
)
|
|
121
|
+
|
|
122
|
+
wp.copy(self.model.joint_act, wp.array(actions, dtype=wp.float32))
|
|
123
|
+
|
|
124
|
+
def step(self, actions):
|
|
125
|
+
self.set_leg_poses(actions)
|
|
126
|
+
|
|
127
|
+
if self.use_cuda_graph:
|
|
128
|
+
wp.capture_launch(self.graph)
|
|
129
|
+
else:
|
|
130
|
+
self.simulate()
|
|
131
|
+
self.sim_time += self.frame_dt
|
|
132
|
+
|
|
133
|
+
def render(self):
|
|
134
|
+
self.renderer.begin_frame(self.sim_time)
|
|
135
|
+
self.renderer.render(self.state_0)
|
|
136
|
+
self.renderer.end_frame()
|
|
137
|
+
|
|
138
|
+
|
|
139
|
+
if __name__ == "__main__":
|
|
140
|
+
import argparse
|
|
141
|
+
|
|
142
|
+
parser = argparse.ArgumentParser(
|
|
143
|
+
formatter_class=argparse.ArgumentDefaultsHelpFormatter
|
|
144
|
+
)
|
|
145
|
+
parser.add_argument(
|
|
146
|
+
"--device", type=str, default=None, help="Override the default Warp device."
|
|
147
|
+
)
|
|
148
|
+
parser.add_argument(
|
|
149
|
+
"--num-frames", type=int, default=300, help="Total number of frames."
|
|
150
|
+
)
|
|
151
|
+
parser.add_argument(
|
|
152
|
+
"--num-envs",
|
|
153
|
+
type=int,
|
|
154
|
+
default=3,
|
|
155
|
+
help="Total number of simulated environments.",
|
|
156
|
+
)
|
|
157
|
+
|
|
158
|
+
args = parser.parse_known_args()[0]
|
|
159
|
+
|
|
160
|
+
with wp.ScopedDevice(args.device):
|
|
161
|
+
example = RobotDogExample(num_envs=args.num_envs)
|
|
162
|
+
# [
|
|
163
|
+
# (front right leg out),
|
|
164
|
+
# (front right leg forward),
|
|
165
|
+
# (front right forearm forward),
|
|
166
|
+
|
|
167
|
+
# (front left leg out),
|
|
168
|
+
# (front left leg forward),
|
|
169
|
+
# (front left forearm forward),
|
|
170
|
+
|
|
171
|
+
# (back right leg out),
|
|
172
|
+
# (back right leg forward),
|
|
173
|
+
# (back right forearm forward),
|
|
174
|
+
|
|
175
|
+
# (back left leg out),
|
|
176
|
+
# (back left leg forward),
|
|
177
|
+
# (back left forearm forward),
|
|
178
|
+
# ]
|
|
179
|
+
# left and right directions are reversed
|
|
180
|
+
|
|
181
|
+
actions = [0.2, 0.4, -0.6, -0.2, -0.4, 0.6, -0.2, 0.4, -0.6, 0.2, -0.4, 0.6] * args.num_envs # fmt: skip
|
|
182
|
+
|
|
183
|
+
for i in range(args.num_frames):
|
|
184
|
+
example.step(actions)
|
|
185
|
+
example.render()
|
|
186
|
+
|
|
187
|
+
if i % 10 == 0:
|
|
188
|
+
# Stretch the right leg to the side
|
|
189
|
+
# actions[0] += 0.1
|
|
190
|
+
|
|
191
|
+
# Stretch the right leg forward
|
|
192
|
+
actions[1] += 0.1
|
|
193
|
+
|
|
194
|
+
# Strecth the forearm foward
|
|
195
|
+
# actions[2] += 0.1
|
|
196
|
+
|
|
197
|
+
# Stretch the left leg forward
|
|
198
|
+
actions[4] -= 0.1
|
|
199
|
+
|
|
200
|
+
# Back right leg backwards
|
|
201
|
+
# actions[7] += 0.1
|
|
202
|
+
|
|
203
|
+
example.renderer.save()
|