kscale 0.0.13__cp312-cp312-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl
Sign up to get free protection for your applications and to get access to all the features.
- kscale/__init__.py +9 -0
- kscale/api.py +14 -0
- kscale/artifacts/__init__.py +8 -0
- kscale/artifacts/plane.obj +18 -0
- kscale/artifacts/plane.urdf +28 -0
- kscale/cli.py +25 -0
- kscale/conf.py +45 -0
- kscale/py.typed +0 -0
- kscale/requirements-dev.txt +11 -0
- kscale/requirements.txt +17 -0
- kscale/rust.cpython-312-i386-linux-gnu.so +0 -0
- kscale/utils/__init__.py +0 -0
- kscale/utils/api_base.py +6 -0
- kscale/utils/checksum.py +41 -0
- kscale/utils/cli.py +28 -0
- kscale/web/__init__.py +0 -0
- kscale/web/api.py +98 -0
- kscale/web/gen/__init__.py +0 -0
- kscale/web/gen/api.py +612 -0
- kscale/web/kernels.py +207 -0
- kscale/web/krec.py +175 -0
- kscale/web/pybullet.py +188 -0
- kscale/web/urdf.py +185 -0
- kscale/web/utils.py +48 -0
- kscale/web/www_client.py +134 -0
- kscale-0.0.13.dist-info/LICENSE +21 -0
- kscale-0.0.13.dist-info/METADATA +55 -0
- kscale-0.0.13.dist-info/RECORD +31 -0
- kscale-0.0.13.dist-info/WHEEL +8 -0
- kscale-0.0.13.dist-info/entry_points.txt +2 -0
- kscale-0.0.13.dist-info/top_level.txt +1 -0
kscale/web/kernels.py
ADDED
@@ -0,0 +1,207 @@
|
|
1
|
+
"""Utility functions for managing kernel images in K-Scale WWW."""
|
2
|
+
|
3
|
+
import hashlib
|
4
|
+
import logging
|
5
|
+
import shutil
|
6
|
+
from pathlib import Path
|
7
|
+
|
8
|
+
import aiofiles
|
9
|
+
import click
|
10
|
+
import httpx
|
11
|
+
|
12
|
+
from kscale.utils.checksum import FileChecksum
|
13
|
+
from kscale.utils.cli import coro
|
14
|
+
from kscale.web.gen.api import SingleArtifactResponse
|
15
|
+
from kscale.web.utils import DEFAULT_UPLOAD_TIMEOUT, get_api_key, get_artifact_dir, get_cache_dir
|
16
|
+
from kscale.web.www_client import KScaleWWWClient
|
17
|
+
|
18
|
+
httpx_logger = logging.getLogger("httpx")
|
19
|
+
httpx_logger.setLevel(logging.WARNING)
|
20
|
+
|
21
|
+
logger = logging.getLogger(__name__)
|
22
|
+
|
23
|
+
ALLOWED_SUFFIXES = {".img"}
|
24
|
+
|
25
|
+
|
26
|
+
async def fetch_kernel_image_info(artifact_id: str, cache_dir: Path) -> SingleArtifactResponse:
|
27
|
+
response_path = cache_dir / "response.json"
|
28
|
+
if response_path.exists():
|
29
|
+
return SingleArtifactResponse.model_validate_json(response_path.read_text())
|
30
|
+
async with KScaleWWWClient() as client:
|
31
|
+
response = await client.get_artifact_info(artifact_id)
|
32
|
+
response_path.write_text(response.model_dump_json())
|
33
|
+
return response
|
34
|
+
|
35
|
+
|
36
|
+
async def download_kernel_image(artifact_id: str) -> Path:
|
37
|
+
"""Download a kernel image artifact."""
|
38
|
+
cache_dir = get_artifact_dir(artifact_id)
|
39
|
+
try:
|
40
|
+
kernel_image_info = await fetch_kernel_image_info(artifact_id, cache_dir)
|
41
|
+
artifact_url = kernel_image_info.urls.large
|
42
|
+
|
43
|
+
original_name = Path(artifact_url).name
|
44
|
+
if not original_name.endswith(".img"):
|
45
|
+
filename = cache_dir / f"{original_name}.img"
|
46
|
+
else:
|
47
|
+
filename = cache_dir / original_name
|
48
|
+
|
49
|
+
headers = {
|
50
|
+
"Authorization": f"Bearer {get_api_key()}",
|
51
|
+
"Accept": "application/octet-stream",
|
52
|
+
}
|
53
|
+
|
54
|
+
if not filename.exists():
|
55
|
+
logger.info("Downloading kernel image...")
|
56
|
+
sha256_hash = hashlib.sha256()
|
57
|
+
|
58
|
+
async with httpx.AsyncClient() as client:
|
59
|
+
async with client.stream("GET", artifact_url, headers=headers) as response:
|
60
|
+
response.raise_for_status()
|
61
|
+
|
62
|
+
async with aiofiles.open(filename, "wb") as f:
|
63
|
+
async for chunk in response.aiter_bytes():
|
64
|
+
FileChecksum.update_hash(sha256_hash, chunk)
|
65
|
+
await f.write(chunk)
|
66
|
+
|
67
|
+
logger.info("Kernel image downloaded to %s", filename)
|
68
|
+
else:
|
69
|
+
logger.info("Kernel image already cached at %s", filename)
|
70
|
+
|
71
|
+
return filename
|
72
|
+
|
73
|
+
except httpx.RequestError:
|
74
|
+
logger.exception("Failed to fetch kernel image")
|
75
|
+
raise
|
76
|
+
|
77
|
+
|
78
|
+
async def show_kernel_image_info(artifact_id: str) -> None:
|
79
|
+
"""Show information about a kernel image artifact."""
|
80
|
+
try:
|
81
|
+
kernel_image_info = await fetch_kernel_image_info(artifact_id, get_artifact_dir(artifact_id))
|
82
|
+
logger.info("Kernel Image Artifact ID: %s", kernel_image_info.artifact_id)
|
83
|
+
logger.info("Kernel Image URL: %s", kernel_image_info.urls.large)
|
84
|
+
except httpx.RequestError:
|
85
|
+
logger.exception("Failed to fetch kernel image info")
|
86
|
+
raise
|
87
|
+
|
88
|
+
|
89
|
+
async def remove_local_kernel_image(artifact_id: str) -> None:
|
90
|
+
"""Remove local cache of a kernel image artifact."""
|
91
|
+
try:
|
92
|
+
if artifact_id.lower() == "all":
|
93
|
+
cache_dir = get_cache_dir()
|
94
|
+
if cache_dir.exists():
|
95
|
+
logger.info("Removing all local caches at %s", cache_dir)
|
96
|
+
shutil.rmtree(cache_dir)
|
97
|
+
else:
|
98
|
+
logger.error("No local caches found")
|
99
|
+
else:
|
100
|
+
artifact_dir = get_artifact_dir(artifact_id)
|
101
|
+
if artifact_dir.exists():
|
102
|
+
logger.info("Removing local cache at %s", artifact_dir)
|
103
|
+
shutil.rmtree(artifact_dir)
|
104
|
+
else:
|
105
|
+
logger.error("No local cache found for artifact %s", artifact_id)
|
106
|
+
|
107
|
+
except Exception:
|
108
|
+
logger.error("Failed to remove local cache")
|
109
|
+
raise
|
110
|
+
|
111
|
+
|
112
|
+
async def upload_kernel_image(
|
113
|
+
listing_id: str,
|
114
|
+
image_path: Path,
|
115
|
+
upload_timeout: float = DEFAULT_UPLOAD_TIMEOUT,
|
116
|
+
) -> SingleArtifactResponse:
|
117
|
+
"""Upload a kernel image."""
|
118
|
+
if image_path.suffix.lower() not in ALLOWED_SUFFIXES:
|
119
|
+
raise ValueError("Invalid file type. Must be one of: %s", ALLOWED_SUFFIXES)
|
120
|
+
|
121
|
+
if not image_path.exists():
|
122
|
+
raise FileNotFoundError(f"Image file not found: {image_path}")
|
123
|
+
|
124
|
+
checksum, file_size = await FileChecksum.calculate(str(image_path))
|
125
|
+
logger.info("Uploading kernel image: %s", image_path)
|
126
|
+
logger.info("File name: %s", image_path.name)
|
127
|
+
logger.info("File size: %.1f MB", file_size / 1024 / 1024)
|
128
|
+
|
129
|
+
async with KScaleWWWClient(upload_timeout=upload_timeout) as client:
|
130
|
+
presigned_data = await client.get_presigned_url(
|
131
|
+
listing_id=listing_id,
|
132
|
+
file_name=image_path.name,
|
133
|
+
checksum=checksum,
|
134
|
+
)
|
135
|
+
|
136
|
+
logger.info("Starting upload...")
|
137
|
+
async with httpx.AsyncClient() as http_client:
|
138
|
+
logger.info("Reading file content into memory...")
|
139
|
+
async with aiofiles.open(image_path, "rb") as f:
|
140
|
+
contents = await f.read()
|
141
|
+
|
142
|
+
logger.info("Uploading file content to %s", presigned_data["upload_url"])
|
143
|
+
response = await http_client.put(
|
144
|
+
presigned_data["upload_url"],
|
145
|
+
content=contents,
|
146
|
+
headers={"Content-Type": "application/x-raw-disk-image"},
|
147
|
+
timeout=upload_timeout,
|
148
|
+
)
|
149
|
+
response.raise_for_status()
|
150
|
+
|
151
|
+
artifact_response: SingleArtifactResponse = await client.get_artifact_info(presigned_data["artifact_id"])
|
152
|
+
logger.info("Uploaded artifact: %s", artifact_response.artifact_id)
|
153
|
+
return artifact_response
|
154
|
+
|
155
|
+
|
156
|
+
async def upload_kernel_image_cli(
|
157
|
+
listing_id: str, image_path: Path, upload_timeout: float = DEFAULT_UPLOAD_TIMEOUT
|
158
|
+
) -> SingleArtifactResponse:
|
159
|
+
"""CLI wrapper for upload_kernel_image."""
|
160
|
+
response = await upload_kernel_image(listing_id, image_path, upload_timeout=upload_timeout)
|
161
|
+
return response
|
162
|
+
|
163
|
+
|
164
|
+
@click.group()
|
165
|
+
def cli() -> None:
|
166
|
+
"""K-Scale Kernel Image CLI tool."""
|
167
|
+
pass
|
168
|
+
|
169
|
+
|
170
|
+
@cli.command()
|
171
|
+
@click.argument("artifact_id")
|
172
|
+
@coro
|
173
|
+
async def download(artifact_id: str) -> None:
|
174
|
+
"""Download a kernel image artifact."""
|
175
|
+
await download_kernel_image(artifact_id)
|
176
|
+
|
177
|
+
|
178
|
+
@cli.command()
|
179
|
+
@click.argument("artifact_id")
|
180
|
+
@coro
|
181
|
+
async def info(artifact_id: str) -> None:
|
182
|
+
"""Show information about a kernel image artifact."""
|
183
|
+
await show_kernel_image_info(artifact_id)
|
184
|
+
|
185
|
+
|
186
|
+
@cli.command("remove-local")
|
187
|
+
@click.argument("artifact_id")
|
188
|
+
@coro
|
189
|
+
async def remove_local(artifact_id: str) -> None:
|
190
|
+
"""Remove local cache of a kernel image artifact."""
|
191
|
+
await remove_local_kernel_image(artifact_id)
|
192
|
+
|
193
|
+
|
194
|
+
@cli.command()
|
195
|
+
@click.argument("listing_id")
|
196
|
+
@click.argument("image_path", type=click.Path(exists=True, path_type=Path))
|
197
|
+
@click.option(
|
198
|
+
"--upload-timeout", type=float, default=DEFAULT_UPLOAD_TIMEOUT, help="Timeout in seconds for upload operations"
|
199
|
+
)
|
200
|
+
@coro
|
201
|
+
async def upload(listing_id: str, image_path: Path, upload_timeout: float) -> None:
|
202
|
+
"""Upload a kernel image artifact."""
|
203
|
+
await upload_kernel_image_cli(listing_id, image_path, upload_timeout=upload_timeout)
|
204
|
+
|
205
|
+
|
206
|
+
if __name__ == "__main__":
|
207
|
+
cli()
|
kscale/web/krec.py
ADDED
@@ -0,0 +1,175 @@
|
|
1
|
+
"""Utility functions for managing K-Recs in K-Scale WWW."""
|
2
|
+
|
3
|
+
import asyncio
|
4
|
+
import json
|
5
|
+
import logging
|
6
|
+
from pathlib import Path
|
7
|
+
|
8
|
+
import aiofiles
|
9
|
+
import click
|
10
|
+
import httpx
|
11
|
+
import krec
|
12
|
+
|
13
|
+
from kscale.utils.cli import coro
|
14
|
+
from kscale.web.gen.api import UploadKRecRequest
|
15
|
+
from kscale.web.utils import DEFAULT_UPLOAD_TIMEOUT, get_api_key, get_artifact_dir
|
16
|
+
from kscale.web.www_client import KScaleWWWClient
|
17
|
+
|
18
|
+
logger = logging.getLogger(__name__)
|
19
|
+
|
20
|
+
|
21
|
+
async def upload_krec(
|
22
|
+
robot_id: str,
|
23
|
+
file_path: Path,
|
24
|
+
description: str | None = None,
|
25
|
+
upload_timeout: float = DEFAULT_UPLOAD_TIMEOUT,
|
26
|
+
) -> str:
|
27
|
+
if not file_path.exists():
|
28
|
+
raise FileNotFoundError(f"File not found: {file_path}")
|
29
|
+
|
30
|
+
file_size = file_path.stat().st_size
|
31
|
+
logger.info("Uploading K-Rec: %s", file_path)
|
32
|
+
logger.info("File name: %s", file_path.name)
|
33
|
+
logger.info("File size: %.1f MB", file_size / 1024 / 1024)
|
34
|
+
|
35
|
+
if not file_path.suffix.lower() == ".krec":
|
36
|
+
logger.warning("File extension is not .krec - are you sure this is a valid K-Rec file?")
|
37
|
+
|
38
|
+
try:
|
39
|
+
krec.KRec.load(str(file_path.resolve()))
|
40
|
+
except Exception as e:
|
41
|
+
raise ValueError(f"Failed to load K-Rec from {file_path} - are you sure this is a valid K-Rec file?") from e
|
42
|
+
|
43
|
+
async with KScaleWWWClient(upload_timeout=upload_timeout) as client:
|
44
|
+
create_response = await client.create_krec(
|
45
|
+
UploadKRecRequest(
|
46
|
+
robot_id=robot_id,
|
47
|
+
name=file_path.name,
|
48
|
+
description=description,
|
49
|
+
)
|
50
|
+
)
|
51
|
+
|
52
|
+
logger.info("Initialized K-Rec upload with ID: %s", create_response["krec_id"])
|
53
|
+
logger.info("Starting upload...")
|
54
|
+
async with httpx.AsyncClient() as http_client:
|
55
|
+
logger.info("Reading file content into memory...")
|
56
|
+
async with aiofiles.open(file_path, "rb") as f:
|
57
|
+
contents = await f.read()
|
58
|
+
|
59
|
+
logger.info("Uploading file content to %s", create_response["upload_url"])
|
60
|
+
response = await http_client.put(
|
61
|
+
create_response["upload_url"],
|
62
|
+
content=contents,
|
63
|
+
headers={"Content-Type": "video/x-matroska"},
|
64
|
+
timeout=upload_timeout,
|
65
|
+
)
|
66
|
+
response.raise_for_status()
|
67
|
+
|
68
|
+
logger.info("Successfully uploaded K-Rec: %s", create_response["krec_id"])
|
69
|
+
return create_response["krec_id"]
|
70
|
+
|
71
|
+
|
72
|
+
def upload_krec_sync(robot_id: str, file_path: Path, description: str | None = None) -> str:
|
73
|
+
return asyncio.run(upload_krec(robot_id, file_path, description))
|
74
|
+
|
75
|
+
|
76
|
+
async def fetch_krec_info(krec_id: str, cache_dir: Path) -> dict:
|
77
|
+
"""Fetch K-Rec info from the server or cache."""
|
78
|
+
response_path = cache_dir / "response.json"
|
79
|
+
if response_path.exists():
|
80
|
+
return json.loads(response_path.read_text())
|
81
|
+
|
82
|
+
async with KScaleWWWClient() as client:
|
83
|
+
try:
|
84
|
+
response = await client.get_krec_info(krec_id)
|
85
|
+
|
86
|
+
if not response:
|
87
|
+
raise ValueError(f"Empty response from server for K-Rec ID: {krec_id}")
|
88
|
+
|
89
|
+
response_path.write_text(json.dumps(response))
|
90
|
+
return response
|
91
|
+
except Exception as e:
|
92
|
+
logger.error("Error fetching K-Rec info: %s", str(e))
|
93
|
+
raise
|
94
|
+
|
95
|
+
|
96
|
+
async def download_krec(krec_id: str) -> Path:
|
97
|
+
"""Download a K-Rec file."""
|
98
|
+
cache_dir = get_artifact_dir(krec_id)
|
99
|
+
|
100
|
+
try:
|
101
|
+
krec_info = await fetch_krec_info(krec_id, cache_dir)
|
102
|
+
|
103
|
+
if not isinstance(krec_info, dict):
|
104
|
+
logger.error("Unexpected response type: %s", type(krec_info))
|
105
|
+
raise ValueError(f"Invalid response format for K-Rec ID: {krec_id}")
|
106
|
+
|
107
|
+
if "url" not in krec_info or "filename" not in krec_info:
|
108
|
+
logger.error("Response missing required fields: %s", krec_info)
|
109
|
+
raise ValueError(f"Invalid response format for K-Rec ID: {krec_id}")
|
110
|
+
|
111
|
+
download_url = krec_info["url"]
|
112
|
+
filename = krec_info["filename"]
|
113
|
+
full_path = cache_dir / filename
|
114
|
+
|
115
|
+
if full_path.exists():
|
116
|
+
logger.info("K-Rec already cached at %s", full_path)
|
117
|
+
return full_path
|
118
|
+
|
119
|
+
logger.info("Downloading K-Rec %s to %s", krec_id, full_path)
|
120
|
+
|
121
|
+
api_key = get_api_key()
|
122
|
+
headers = {"Authorization": f"Bearer {api_key}", "Accept": "application/octet-stream"}
|
123
|
+
|
124
|
+
async with httpx.AsyncClient() as client:
|
125
|
+
async with client.stream("GET", download_url, headers=headers) as response:
|
126
|
+
response.raise_for_status()
|
127
|
+
|
128
|
+
with open(full_path, "wb") as f:
|
129
|
+
async for chunk in response.aiter_bytes():
|
130
|
+
f.write(chunk)
|
131
|
+
|
132
|
+
logger.info("Successfully downloaded K-Rec to %s", full_path)
|
133
|
+
return full_path
|
134
|
+
|
135
|
+
except httpx.RequestError as e:
|
136
|
+
logger.exception("Failed to fetch K-Rec: %s", str(e))
|
137
|
+
raise
|
138
|
+
except Exception as e:
|
139
|
+
logger.exception("Unexpected error: %s", str(e))
|
140
|
+
raise
|
141
|
+
|
142
|
+
|
143
|
+
def download_krec_sync(krec_id: str) -> Path:
|
144
|
+
"""Sync wrapper for download_krec."""
|
145
|
+
return asyncio.run(download_krec(krec_id))
|
146
|
+
|
147
|
+
|
148
|
+
@click.group(name="krec")
|
149
|
+
def cli() -> None:
|
150
|
+
"""K-Scale K-Rec management commands."""
|
151
|
+
pass
|
152
|
+
|
153
|
+
|
154
|
+
@cli.command()
|
155
|
+
@click.argument("robot_id")
|
156
|
+
@click.argument("file_path", type=click.Path(exists=True, path_type=Path))
|
157
|
+
@click.option("--description", "-d", help="Description of the K-Rec")
|
158
|
+
@coro
|
159
|
+
async def upload(robot_id: str, file_path: Path, description: str | None = None) -> None:
|
160
|
+
"""Upload a K-Rec file."""
|
161
|
+
krec_id = await upload_krec(robot_id, file_path, description)
|
162
|
+
click.echo(f"Successfully uploaded K-Rec: {krec_id}")
|
163
|
+
|
164
|
+
|
165
|
+
@cli.command()
|
166
|
+
@click.argument("krec_id")
|
167
|
+
@coro
|
168
|
+
async def download(krec_id: str) -> None:
|
169
|
+
"""Download a K-Rec file."""
|
170
|
+
file_path = await download_krec(krec_id)
|
171
|
+
click.echo(f"Successfully downloaded K-Rec to: {file_path}")
|
172
|
+
|
173
|
+
|
174
|
+
if __name__ == "__main__":
|
175
|
+
cli()
|
kscale/web/pybullet.py
ADDED
@@ -0,0 +1,188 @@
|
|
1
|
+
"""Simple script to interact with a URDF in PyBullet."""
|
2
|
+
|
3
|
+
import itertools
|
4
|
+
import logging
|
5
|
+
import math
|
6
|
+
import time
|
7
|
+
|
8
|
+
import click
|
9
|
+
|
10
|
+
from kscale.artifacts import PLANE_URDF_PATH
|
11
|
+
from kscale.utils.cli import coro
|
12
|
+
from kscale.web.urdf import download_urdf
|
13
|
+
|
14
|
+
logger = logging.getLogger(__name__)
|
15
|
+
|
16
|
+
|
17
|
+
@click.command(help="Show a URDF in PyBullet")
|
18
|
+
@click.argument("listing_id")
|
19
|
+
@click.option("--dt", type=float, default=0.01, help="Time step")
|
20
|
+
@click.option("-n", "--hide-gui", is_flag=True, help="Hide the GUI")
|
21
|
+
@click.option("--no-merge", is_flag=True, help="Do not merge fixed links")
|
22
|
+
@click.option("--hide-origin", is_flag=True, help="Do not show the origin")
|
23
|
+
@click.option("--show-inertia", is_flag=True, help="Visualizes the inertia frames")
|
24
|
+
@click.option("--see-thru", is_flag=True, help="Use see-through mode")
|
25
|
+
@click.option("--show-collision", is_flag=True, help="Show collision meshes")
|
26
|
+
@coro
|
27
|
+
async def cli(
|
28
|
+
listing_id: str,
|
29
|
+
dt: float,
|
30
|
+
hide_gui: bool,
|
31
|
+
no_merge: bool,
|
32
|
+
hide_origin: bool,
|
33
|
+
show_inertia: bool,
|
34
|
+
see_thru: bool,
|
35
|
+
show_collision: bool,
|
36
|
+
) -> None:
|
37
|
+
# Gets the URDF path.
|
38
|
+
urdf_dir = await download_urdf(listing_id)
|
39
|
+
urdf_path = next(urdf_dir.glob("*.urdf"), None)
|
40
|
+
if urdf_path is None:
|
41
|
+
raise ValueError(f"No URDF found in {urdf_dir}")
|
42
|
+
|
43
|
+
try:
|
44
|
+
import pybullet as p # type: ignore[import-not-found]
|
45
|
+
except ImportError:
|
46
|
+
raise ImportError("pybullet is required to run this script")
|
47
|
+
|
48
|
+
# Connect to PyBullet.
|
49
|
+
p.connect(p.GUI)
|
50
|
+
p.setGravity(0, 0, -9.81)
|
51
|
+
p.setRealTimeSimulation(0)
|
52
|
+
|
53
|
+
# Turn off panels.
|
54
|
+
if hide_gui:
|
55
|
+
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
|
56
|
+
p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
|
57
|
+
p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
|
58
|
+
p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
|
59
|
+
|
60
|
+
# Enable mouse picking.
|
61
|
+
p.configureDebugVisualizer(p.COV_ENABLE_MOUSE_PICKING, 1)
|
62
|
+
|
63
|
+
# Loads the floor plane.
|
64
|
+
floor = p.loadURDF(str(PLANE_URDF_PATH))
|
65
|
+
|
66
|
+
# Load the robot URDF.
|
67
|
+
start_position = [0.0, 0.0, 1.0]
|
68
|
+
start_orientation = p.getQuaternionFromEuler([0.0, 0.0, 0.0])
|
69
|
+
flags = p.URDF_USE_INERTIA_FROM_FILE
|
70
|
+
if not no_merge:
|
71
|
+
flags |= p.URDF_MERGE_FIXED_LINKS
|
72
|
+
robot = p.loadURDF(str(urdf_path), start_position, start_orientation, flags=flags, useFixedBase=0)
|
73
|
+
|
74
|
+
# Display collision meshes as separate object.
|
75
|
+
if show_collision:
|
76
|
+
collision_flags = p.URDF_USE_INERTIA_FROM_FILE | p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
|
77
|
+
collision = p.loadURDF(str(urdf_path), start_position, start_orientation, flags=collision_flags, useFixedBase=0)
|
78
|
+
|
79
|
+
# Make collision shapes semi-transparent.
|
80
|
+
joint_ids = [i for i in range(p.getNumJoints(collision))] + [-1]
|
81
|
+
for i in joint_ids:
|
82
|
+
p.changeVisualShape(collision, i, rgbaColor=[1, 0, 0, 0.5])
|
83
|
+
|
84
|
+
# Initializes physics parameters.
|
85
|
+
p.changeDynamics(floor, -1, lateralFriction=1, spinningFriction=-1, rollingFriction=-1)
|
86
|
+
p.setPhysicsEngineParameter(fixedTimeStep=dt, maxNumCmdPer1ms=1000)
|
87
|
+
|
88
|
+
# Shows the origin of the robot.
|
89
|
+
if not hide_origin:
|
90
|
+
p.addUserDebugLine([0, 0, 0], [0.1, 0, 0], [1, 0, 0], parentObjectUniqueId=robot, parentLinkIndex=-1)
|
91
|
+
p.addUserDebugLine([0, 0, 0], [0, 0.1, 0], [0, 1, 0], parentObjectUniqueId=robot, parentLinkIndex=-1)
|
92
|
+
p.addUserDebugLine([0, 0, 0], [0, 0, 0.1], [0, 0, 1], parentObjectUniqueId=robot, parentLinkIndex=-1)
|
93
|
+
|
94
|
+
# Make the robot see-through.
|
95
|
+
joint_ids = [i for i in range(p.getNumJoints(robot))] + [-1]
|
96
|
+
if see_thru:
|
97
|
+
for i in joint_ids:
|
98
|
+
p.changeVisualShape(robot, i, rgbaColor=[1, 1, 1, 0.5])
|
99
|
+
|
100
|
+
def draw_box(pt: list[list[float]], color: tuple[float, float, float], obj_id: int, link_id: int) -> None:
|
101
|
+
assert len(pt) == 8
|
102
|
+
assert all(len(p) == 3 for p in pt)
|
103
|
+
|
104
|
+
mapping = [1, 3, 0, 2]
|
105
|
+
for i in range(4):
|
106
|
+
p.addUserDebugLine(pt[i], pt[i + 4], color, 1, parentObjectUniqueId=obj_id, parentLinkIndex=link_id)
|
107
|
+
p.addUserDebugLine(pt[i], pt[mapping[i]], color, 1, parentObjectUniqueId=obj_id, parentLinkIndex=link_id)
|
108
|
+
p.addUserDebugLine(
|
109
|
+
pt[i + 4], pt[mapping[i] + 4], color, 1, parentObjectUniqueId=obj_id, parentLinkIndex=link_id
|
110
|
+
)
|
111
|
+
|
112
|
+
# Shows bounding boxes around each part of the robot representing the inertia frame.
|
113
|
+
if show_inertia:
|
114
|
+
for i in joint_ids:
|
115
|
+
dynamics_info = p.getDynamicsInfo(robot, i)
|
116
|
+
mass = dynamics_info[0]
|
117
|
+
if mass <= 0:
|
118
|
+
continue
|
119
|
+
inertia = dynamics_info[2]
|
120
|
+
ixx = inertia[0]
|
121
|
+
iyy = inertia[1]
|
122
|
+
izz = inertia[2]
|
123
|
+
box_scale_x = 0.5 * math.sqrt(6 * (izz + iyy - ixx) / mass)
|
124
|
+
box_scale_y = 0.5 * math.sqrt(6 * (izz + ixx - iyy) / mass)
|
125
|
+
box_scale_z = 0.5 * math.sqrt(6 * (ixx + iyy - izz) / mass)
|
126
|
+
|
127
|
+
half_extents = [box_scale_x, box_scale_y, box_scale_z]
|
128
|
+
pt = [
|
129
|
+
[x, y, z]
|
130
|
+
for x, y, z in itertools.product(
|
131
|
+
[-half_extents[0], half_extents[0]],
|
132
|
+
[-half_extents[1], half_extents[1]],
|
133
|
+
[-half_extents[2], half_extents[2]],
|
134
|
+
)
|
135
|
+
]
|
136
|
+
draw_box(pt, (1, 0, 0), robot, i)
|
137
|
+
|
138
|
+
# Show joint controller.
|
139
|
+
joints: dict[str, int] = {}
|
140
|
+
controls: dict[str, float] = {}
|
141
|
+
for i in range(p.getNumJoints(robot)):
|
142
|
+
joint_info = p.getJointInfo(robot, i)
|
143
|
+
name = joint_info[1].decode("utf-8")
|
144
|
+
joint_type = joint_info[2]
|
145
|
+
joints[name] = i
|
146
|
+
if joint_type == p.JOINT_PRISMATIC:
|
147
|
+
joint_min, joint_max = joint_info[8:10]
|
148
|
+
controls[name] = p.addUserDebugParameter(name, joint_min, joint_max, 0.0)
|
149
|
+
elif joint_type == p.JOINT_REVOLUTE:
|
150
|
+
joint_min, joint_max = joint_info[8:10]
|
151
|
+
controls[name] = p.addUserDebugParameter(name, joint_min, joint_max, 0.0)
|
152
|
+
|
153
|
+
# Run the simulation until the user closes the window.
|
154
|
+
last_time = time.time()
|
155
|
+
prev_control_values = {k: 0.0 for k in controls}
|
156
|
+
while p.isConnected():
|
157
|
+
# Reset the simulation if "r" was pressed.
|
158
|
+
keys = p.getKeyboardEvents()
|
159
|
+
if ord("r") in keys and keys[ord("r")] & p.KEY_WAS_TRIGGERED:
|
160
|
+
p.resetBasePositionAndOrientation(robot, start_position, start_orientation)
|
161
|
+
p.setJointMotorControlArray(
|
162
|
+
robot,
|
163
|
+
range(p.getNumJoints(robot)),
|
164
|
+
p.POSITION_CONTROL,
|
165
|
+
targetPositions=[0] * p.getNumJoints(robot),
|
166
|
+
)
|
167
|
+
|
168
|
+
# Set joint positions.
|
169
|
+
for k, v in controls.items():
|
170
|
+
try:
|
171
|
+
target_position = p.readUserDebugParameter(v)
|
172
|
+
if target_position != prev_control_values[k]:
|
173
|
+
prev_control_values[k] = target_position
|
174
|
+
p.setJointMotorControl2(robot, joints[k], p.POSITION_CONTROL, target_position)
|
175
|
+
except p.error:
|
176
|
+
logger.debug("Failed to set joint %s", k)
|
177
|
+
pass
|
178
|
+
|
179
|
+
# Step simulation.
|
180
|
+
p.stepSimulation()
|
181
|
+
cur_time = time.time()
|
182
|
+
time.sleep(max(0, dt - (cur_time - last_time)))
|
183
|
+
last_time = cur_time
|
184
|
+
|
185
|
+
|
186
|
+
if __name__ == "__main__":
|
187
|
+
# python -m kscale.web.pybullet
|
188
|
+
cli()
|