kscale 0.0.13__cp311-cp311-macosx_11_0_arm64.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.
- 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-311-darwin.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 +5 -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()
|