reality 0.1.0__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.
- reality/__init__.py +128 -0
- reality/__main__.py +12 -0
- reality/_accelerators.py +63 -0
- reality/_articulation.py +274 -0
- reality/_batch.py +598 -0
- reality/_branch.py +661 -0
- reality/_explore.py +227 -0
- reality/_graph.py +383 -0
- reality/_loaders.py +190 -0
- reality/_metadata.py +264 -0
- reality/_models.py +303 -0
- reality/_navigation.py +594 -0
- reality/_physics.py +114 -0
- reality/_state.py +215 -0
- reality/_warp_ops.py +574 -0
- reality/_world.py +945 -0
- reality/backends/__init__.py +5 -0
- reality/backends/base.py +37 -0
- reality/backends/mujoco.py +324 -0
- reality/experimental/__init__.py +8 -0
- reality/experimental/predictor.py +60 -0
- reality/predicates.py +103 -0
- reality/py.typed +1 -0
- reality-0.1.0.dist-info/METADATA +245 -0
- reality-0.1.0.dist-info/RECORD +28 -0
- reality-0.1.0.dist-info/WHEEL +4 -0
- reality-0.1.0.dist-info/entry_points.txt +2 -0
- reality-0.1.0.dist-info/licenses/LICENSE +173 -0
reality/__init__.py
ADDED
|
@@ -0,0 +1,128 @@
|
|
|
1
|
+
"""Reality: a backend-neutral foundation for programmable 3D worlds."""
|
|
2
|
+
|
|
3
|
+
from ._accelerators import AccelerationUnavailableError, WarpStatus, warp_status
|
|
4
|
+
from ._articulation import (
|
|
5
|
+
ArticulatedObject,
|
|
6
|
+
Articulation,
|
|
7
|
+
Joint,
|
|
8
|
+
MotionResult,
|
|
9
|
+
PrismaticJoint,
|
|
10
|
+
RevoluteJoint,
|
|
11
|
+
)
|
|
12
|
+
from ._batch import (
|
|
13
|
+
BatchEvaluation,
|
|
14
|
+
BranchBatch,
|
|
15
|
+
FutureCandidate,
|
|
16
|
+
Futures,
|
|
17
|
+
FutureSelection,
|
|
18
|
+
RankedFutures,
|
|
19
|
+
)
|
|
20
|
+
from ._branch import WorldBranch
|
|
21
|
+
from ._explore import ExplorationResult, PositionSearchChange, position
|
|
22
|
+
from ._graph import GraphUpdateStats, RealityGraph, Relationship, RelationshipType
|
|
23
|
+
from ._loaders import load
|
|
24
|
+
from ._metadata import SceneMetadataError
|
|
25
|
+
from ._models import Bounds, PhysicalProperties, PredicateResult, Transform, WorldObject
|
|
26
|
+
from ._navigation import (
|
|
27
|
+
Agent,
|
|
28
|
+
ClearanceResult,
|
|
29
|
+
NavigationGrid,
|
|
30
|
+
PassageResult,
|
|
31
|
+
PathResult,
|
|
32
|
+
ReachabilityResult,
|
|
33
|
+
)
|
|
34
|
+
from ._physics import (
|
|
35
|
+
BodySimulationResult,
|
|
36
|
+
Contact,
|
|
37
|
+
PhysicsBackendUnavailableError,
|
|
38
|
+
SimulationResult,
|
|
39
|
+
StabilityResult,
|
|
40
|
+
UnsupportedPhysicsOperationError,
|
|
41
|
+
)
|
|
42
|
+
from ._state import (
|
|
43
|
+
Change,
|
|
44
|
+
ChangeSet,
|
|
45
|
+
Consequence,
|
|
46
|
+
ConsequenceSet,
|
|
47
|
+
MoveObject,
|
|
48
|
+
RotateObject,
|
|
49
|
+
ScaleObject,
|
|
50
|
+
WorldSnapshot,
|
|
51
|
+
)
|
|
52
|
+
from ._world import AmbiguousObjectError, ObjectNotFoundError, World
|
|
53
|
+
from .predicates import (
|
|
54
|
+
Condition,
|
|
55
|
+
Objective,
|
|
56
|
+
PredicateSpec,
|
|
57
|
+
collision,
|
|
58
|
+
distance,
|
|
59
|
+
maximize,
|
|
60
|
+
minimize,
|
|
61
|
+
no_collision,
|
|
62
|
+
visibility,
|
|
63
|
+
)
|
|
64
|
+
|
|
65
|
+
__all__ = [
|
|
66
|
+
"AmbiguousObjectError",
|
|
67
|
+
"AccelerationUnavailableError",
|
|
68
|
+
"Agent",
|
|
69
|
+
"ArticulatedObject",
|
|
70
|
+
"Articulation",
|
|
71
|
+
"Bounds",
|
|
72
|
+
"BatchEvaluation",
|
|
73
|
+
"BodySimulationResult",
|
|
74
|
+
"BranchBatch",
|
|
75
|
+
"Condition",
|
|
76
|
+
"Change",
|
|
77
|
+
"ChangeSet",
|
|
78
|
+
"Consequence",
|
|
79
|
+
"ConsequenceSet",
|
|
80
|
+
"Contact",
|
|
81
|
+
"ClearanceResult",
|
|
82
|
+
"GraphUpdateStats",
|
|
83
|
+
"FutureCandidate",
|
|
84
|
+
"FutureSelection",
|
|
85
|
+
"Futures",
|
|
86
|
+
"ExplorationResult",
|
|
87
|
+
"Joint",
|
|
88
|
+
"MoveObject",
|
|
89
|
+
"MotionResult",
|
|
90
|
+
"NavigationGrid",
|
|
91
|
+
"ObjectNotFoundError",
|
|
92
|
+
"PhysicalProperties",
|
|
93
|
+
"PositionSearchChange",
|
|
94
|
+
"PhysicsBackendUnavailableError",
|
|
95
|
+
"PredicateResult",
|
|
96
|
+
"PrismaticJoint",
|
|
97
|
+
"PassageResult",
|
|
98
|
+
"PathResult",
|
|
99
|
+
"RealityGraph",
|
|
100
|
+
"ReachabilityResult",
|
|
101
|
+
"RankedFutures",
|
|
102
|
+
"Relationship",
|
|
103
|
+
"RelationshipType",
|
|
104
|
+
"RevoluteJoint",
|
|
105
|
+
"RotateObject",
|
|
106
|
+
"ScaleObject",
|
|
107
|
+
"SceneMetadataError",
|
|
108
|
+
"SimulationResult",
|
|
109
|
+
"StabilityResult",
|
|
110
|
+
"Transform",
|
|
111
|
+
"World",
|
|
112
|
+
"WorldBranch",
|
|
113
|
+
"WorldObject",
|
|
114
|
+
"WorldSnapshot",
|
|
115
|
+
"WarpStatus",
|
|
116
|
+
"UnsupportedPhysicsOperationError",
|
|
117
|
+
"load",
|
|
118
|
+
"Objective",
|
|
119
|
+
"PredicateSpec",
|
|
120
|
+
"collision",
|
|
121
|
+
"distance",
|
|
122
|
+
"maximize",
|
|
123
|
+
"minimize",
|
|
124
|
+
"no_collision",
|
|
125
|
+
"position",
|
|
126
|
+
"visibility",
|
|
127
|
+
"warp_status",
|
|
128
|
+
]
|
reality/__main__.py
ADDED
|
@@ -0,0 +1,12 @@
|
|
|
1
|
+
"""Small, side-effect-free command-line entry point for Reality."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
|
|
6
|
+
def main() -> None:
|
|
7
|
+
"""Print Reality's installation greeting."""
|
|
8
|
+
print("Andani is goat!!")
|
|
9
|
+
|
|
10
|
+
|
|
11
|
+
if __name__ == "__main__": # pragma: no cover - exercised through the console script
|
|
12
|
+
main()
|
reality/_accelerators.py
ADDED
|
@@ -0,0 +1,63 @@
|
|
|
1
|
+
"""Optional NVIDIA Warp capability probing without import-time GPU requirements."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
import os
|
|
6
|
+
import tempfile
|
|
7
|
+
from dataclasses import dataclass
|
|
8
|
+
from pathlib import Path
|
|
9
|
+
|
|
10
|
+
|
|
11
|
+
class AccelerationUnavailableError(RuntimeError):
|
|
12
|
+
"""Raised when a requested numerical accelerator cannot run."""
|
|
13
|
+
|
|
14
|
+
|
|
15
|
+
@dataclass(frozen=True, slots=True)
|
|
16
|
+
class WarpStatus:
|
|
17
|
+
installed: bool
|
|
18
|
+
version: str | None
|
|
19
|
+
cuda_available: bool
|
|
20
|
+
devices: tuple[str, ...]
|
|
21
|
+
reason: str
|
|
22
|
+
|
|
23
|
+
|
|
24
|
+
def warp_status() -> WarpStatus:
|
|
25
|
+
try:
|
|
26
|
+
import warp as wp
|
|
27
|
+
except ImportError:
|
|
28
|
+
return WarpStatus(False, None, False, (), "warp-lang is not installed")
|
|
29
|
+
try:
|
|
30
|
+
cache = Path(tempfile.gettempdir()) / "reality-warp-cache"
|
|
31
|
+
os.environ.setdefault("WARP_CACHE_PATH", str(cache))
|
|
32
|
+
wp.init()
|
|
33
|
+
devices = tuple(str(device) for device in wp.get_devices())
|
|
34
|
+
available = bool(wp.is_cuda_available())
|
|
35
|
+
return WarpStatus(
|
|
36
|
+
True,
|
|
37
|
+
str(wp.__version__),
|
|
38
|
+
available,
|
|
39
|
+
devices,
|
|
40
|
+
"CUDA is available." if available else "CUDA driver/device is unavailable.",
|
|
41
|
+
)
|
|
42
|
+
except Exception as error: # pragma: no cover - depends on host driver/runtime
|
|
43
|
+
return WarpStatus(
|
|
44
|
+
True,
|
|
45
|
+
str(wp.__version__),
|
|
46
|
+
False,
|
|
47
|
+
(),
|
|
48
|
+
f"Warp initialization failed: {error}",
|
|
49
|
+
)
|
|
50
|
+
|
|
51
|
+
|
|
52
|
+
def require_cuda() -> None:
|
|
53
|
+
status = warp_status()
|
|
54
|
+
if not status.installed:
|
|
55
|
+
raise AccelerationUnavailableError(
|
|
56
|
+
"CUDA backend requires optional dependency 'warp-lang'; install reality[gpu]. "
|
|
57
|
+
"Select backend='cpu'."
|
|
58
|
+
)
|
|
59
|
+
if not status.cuda_available:
|
|
60
|
+
raise AccelerationUnavailableError(
|
|
61
|
+
"NVIDIA Warp is installed but no usable CUDA driver/device is available. "
|
|
62
|
+
"Select backend='cpu'."
|
|
63
|
+
)
|
reality/_articulation.py
ADDED
|
@@ -0,0 +1,274 @@
|
|
|
1
|
+
"""Explicit articulated joints and deterministic swept-AABB motion analysis."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
from collections.abc import Mapping
|
|
6
|
+
from dataclasses import dataclass, field
|
|
7
|
+
from math import ceil, cos, isfinite, radians, sin, sqrt
|
|
8
|
+
from types import MappingProxyType
|
|
9
|
+
from typing import TYPE_CHECKING, TypeAlias
|
|
10
|
+
|
|
11
|
+
import numpy as np
|
|
12
|
+
|
|
13
|
+
from ._models import Bounds, Vector3, WorldObject
|
|
14
|
+
|
|
15
|
+
if TYPE_CHECKING:
|
|
16
|
+
from ._world import World
|
|
17
|
+
|
|
18
|
+
MotionKey: TypeAlias = tuple[str, str, float]
|
|
19
|
+
MotionRecord: TypeAlias = tuple[str, str, float, "MotionResult"]
|
|
20
|
+
|
|
21
|
+
|
|
22
|
+
def _unit_vector(value: Vector3) -> Vector3:
|
|
23
|
+
converted = tuple(float(component) for component in value)
|
|
24
|
+
length = sqrt(sum(component * component for component in converted))
|
|
25
|
+
if length <= 1e-12 or not all(isfinite(component) for component in converted):
|
|
26
|
+
raise ValueError("joint axis must be finite and non-zero")
|
|
27
|
+
return tuple(component / length for component in converted) # type: ignore[return-value]
|
|
28
|
+
|
|
29
|
+
|
|
30
|
+
@dataclass(frozen=True, slots=True)
|
|
31
|
+
class Joint:
|
|
32
|
+
axis: Vector3
|
|
33
|
+
minimum: float
|
|
34
|
+
maximum: float
|
|
35
|
+
current: float = 0.0
|
|
36
|
+
|
|
37
|
+
def __post_init__(self) -> None:
|
|
38
|
+
object.__setattr__(self, "axis", _unit_vector(self.axis))
|
|
39
|
+
if not all(isfinite(value) for value in (self.minimum, self.maximum, self.current)):
|
|
40
|
+
raise ValueError("joint values must be finite")
|
|
41
|
+
if self.minimum > self.maximum:
|
|
42
|
+
raise ValueError("joint minimum must not exceed maximum")
|
|
43
|
+
if not self.minimum <= self.current <= self.maximum:
|
|
44
|
+
raise ValueError("joint current value must lie within limits")
|
|
45
|
+
|
|
46
|
+
@property
|
|
47
|
+
def kind(self) -> str:
|
|
48
|
+
raise NotImplementedError
|
|
49
|
+
|
|
50
|
+
|
|
51
|
+
@dataclass(frozen=True, slots=True)
|
|
52
|
+
class RevoluteJoint(Joint):
|
|
53
|
+
"""A degree-valued rotation about a world-space pivot and axis."""
|
|
54
|
+
|
|
55
|
+
pivot: Vector3 = (0.0, 0.0, 0.0)
|
|
56
|
+
angular_resolution: float = 1.0
|
|
57
|
+
|
|
58
|
+
def __post_init__(self) -> None:
|
|
59
|
+
super(RevoluteJoint, self).__post_init__()
|
|
60
|
+
if len(self.pivot) != 3 or not all(isfinite(value) for value in self.pivot):
|
|
61
|
+
raise ValueError("pivot must contain three finite values")
|
|
62
|
+
if self.angular_resolution <= 0.0:
|
|
63
|
+
raise ValueError("angular_resolution must be positive")
|
|
64
|
+
|
|
65
|
+
@property
|
|
66
|
+
def kind(self) -> str:
|
|
67
|
+
return "revolute"
|
|
68
|
+
|
|
69
|
+
|
|
70
|
+
@dataclass(frozen=True, slots=True)
|
|
71
|
+
class PrismaticJoint(Joint):
|
|
72
|
+
"""A metre-valued translation along a world-space axis."""
|
|
73
|
+
|
|
74
|
+
linear_resolution: float = 0.01
|
|
75
|
+
|
|
76
|
+
def __post_init__(self) -> None:
|
|
77
|
+
super(PrismaticJoint, self).__post_init__()
|
|
78
|
+
if self.linear_resolution <= 0.0:
|
|
79
|
+
raise ValueError("linear_resolution must be positive")
|
|
80
|
+
|
|
81
|
+
@property
|
|
82
|
+
def kind(self) -> str:
|
|
83
|
+
return "prismatic"
|
|
84
|
+
|
|
85
|
+
|
|
86
|
+
@dataclass(frozen=True, slots=True)
|
|
87
|
+
class Articulation:
|
|
88
|
+
object_id: str
|
|
89
|
+
joint: RevoluteJoint | PrismaticJoint
|
|
90
|
+
|
|
91
|
+
@property
|
|
92
|
+
def value(self) -> bool:
|
|
93
|
+
return True
|
|
94
|
+
|
|
95
|
+
|
|
96
|
+
@dataclass(frozen=True, slots=True)
|
|
97
|
+
class MotionResult:
|
|
98
|
+
possible: bool
|
|
99
|
+
requested: float
|
|
100
|
+
maximum_collision_free: float
|
|
101
|
+
collision_at: float | None
|
|
102
|
+
collides_with: tuple[WorldObject, ...]
|
|
103
|
+
units: str
|
|
104
|
+
reason: str
|
|
105
|
+
samples_tested: int
|
|
106
|
+
evidence: Mapping[str, object] = field(default_factory=dict)
|
|
107
|
+
|
|
108
|
+
def __post_init__(self) -> None:
|
|
109
|
+
object.__setattr__(self, "evidence", MappingProxyType(dict(self.evidence)))
|
|
110
|
+
|
|
111
|
+
@property
|
|
112
|
+
def value(self) -> bool:
|
|
113
|
+
return self.possible
|
|
114
|
+
|
|
115
|
+
def to_dict(self) -> dict[str, object]:
|
|
116
|
+
return {
|
|
117
|
+
"possible": self.possible,
|
|
118
|
+
"requested": self.requested,
|
|
119
|
+
"maximum_collision_free": self.maximum_collision_free,
|
|
120
|
+
"collision_at": self.collision_at,
|
|
121
|
+
"collides_with": [
|
|
122
|
+
{"id": object_.id, "name": object_.name} for object_ in self.collides_with
|
|
123
|
+
],
|
|
124
|
+
"units": self.units,
|
|
125
|
+
"reason": self.reason,
|
|
126
|
+
"samples_tested": self.samples_tested,
|
|
127
|
+
"evidence": dict(self.evidence),
|
|
128
|
+
}
|
|
129
|
+
|
|
130
|
+
|
|
131
|
+
@dataclass(frozen=True, slots=True)
|
|
132
|
+
class ArticulatedObject:
|
|
133
|
+
"""World-bound handle exposing an object's constrained motion queries."""
|
|
134
|
+
|
|
135
|
+
object: WorldObject
|
|
136
|
+
articulation: Articulation
|
|
137
|
+
_world: World = field(compare=False, repr=False)
|
|
138
|
+
|
|
139
|
+
def motion_range(self) -> tuple[float, float]:
|
|
140
|
+
return self.articulation.joint.minimum, self.articulation.joint.maximum
|
|
141
|
+
|
|
142
|
+
def can_rotate(self, degrees: float) -> MotionResult:
|
|
143
|
+
if not isinstance(self.articulation.joint, RevoluteJoint):
|
|
144
|
+
raise TypeError(f"{self.object.name!r} does not have a revolute joint")
|
|
145
|
+
return self._world._motion_query(self.object.id, "revolute", float(degrees))
|
|
146
|
+
|
|
147
|
+
def can_extend(self, distance: float) -> MotionResult:
|
|
148
|
+
if not isinstance(self.articulation.joint, PrismaticJoint):
|
|
149
|
+
raise TypeError(f"{self.object.name!r} does not have a prismatic joint")
|
|
150
|
+
return self._world._motion_query(self.object.id, "prismatic", float(distance))
|
|
151
|
+
|
|
152
|
+
|
|
153
|
+
def analyze_motion(
|
|
154
|
+
world: World,
|
|
155
|
+
object_: WorldObject,
|
|
156
|
+
articulation: Articulation,
|
|
157
|
+
requested: float,
|
|
158
|
+
) -> MotionResult:
|
|
159
|
+
joint = articulation.joint
|
|
160
|
+
units = "deg" if isinstance(joint, RevoluteJoint) else world.units
|
|
161
|
+
limited = min(joint.maximum, max(joint.minimum, requested))
|
|
162
|
+
limit_prevents = limited != requested
|
|
163
|
+
resolution = (
|
|
164
|
+
joint.angular_resolution if isinstance(joint, RevoluteJoint) else joint.linear_resolution
|
|
165
|
+
)
|
|
166
|
+
span = limited - joint.current
|
|
167
|
+
sample_count = max(1, int(ceil(abs(span) / resolution)))
|
|
168
|
+
samples = [joint.current + span * index / sample_count for index in range(1, sample_count + 1)]
|
|
169
|
+
last_safe = joint.current
|
|
170
|
+
tested = 0
|
|
171
|
+
for sample in samples:
|
|
172
|
+
tested += 1
|
|
173
|
+
bounds = _motion_bounds(object_.bounds, joint, sample)
|
|
174
|
+
blockers = tuple(
|
|
175
|
+
candidate
|
|
176
|
+
for candidate in world.objects
|
|
177
|
+
if candidate.id != object_.id and bounds.intersects(candidate.bounds)
|
|
178
|
+
)
|
|
179
|
+
if blockers:
|
|
180
|
+
refined = _refine_collision(world, object_, joint, last_safe, sample)
|
|
181
|
+
refined_bounds = _motion_bounds(object_.bounds, joint, sample)
|
|
182
|
+
refined_blockers = tuple(
|
|
183
|
+
candidate
|
|
184
|
+
for candidate in world.objects
|
|
185
|
+
if candidate.id != object_.id and refined_bounds.intersects(candidate.bounds)
|
|
186
|
+
)
|
|
187
|
+
names = ", ".join(item.name for item in refined_blockers or blockers)
|
|
188
|
+
return MotionResult(
|
|
189
|
+
possible=False,
|
|
190
|
+
requested=requested,
|
|
191
|
+
maximum_collision_free=refined,
|
|
192
|
+
collision_at=sample,
|
|
193
|
+
collides_with=refined_blockers or blockers,
|
|
194
|
+
units=units,
|
|
195
|
+
reason=f"Motion is blocked by {names} near {sample:.4g} {units}.",
|
|
196
|
+
samples_tested=tested,
|
|
197
|
+
evidence={
|
|
198
|
+
"method": "sampled swept world-AABB with binary refinement",
|
|
199
|
+
"resolution": resolution,
|
|
200
|
+
"joint_limits": (joint.minimum, joint.maximum),
|
|
201
|
+
},
|
|
202
|
+
)
|
|
203
|
+
last_safe = sample
|
|
204
|
+
if limit_prevents:
|
|
205
|
+
return MotionResult(
|
|
206
|
+
possible=False,
|
|
207
|
+
requested=requested,
|
|
208
|
+
maximum_collision_free=limited,
|
|
209
|
+
collision_at=None,
|
|
210
|
+
collides_with=(),
|
|
211
|
+
units=units,
|
|
212
|
+
reason=f"Requested motion exceeds the configured joint limit of {limited:g} {units}.",
|
|
213
|
+
samples_tested=tested,
|
|
214
|
+
evidence={"joint_limits": (joint.minimum, joint.maximum), "resolution": resolution},
|
|
215
|
+
)
|
|
216
|
+
return MotionResult(
|
|
217
|
+
possible=True,
|
|
218
|
+
requested=requested,
|
|
219
|
+
maximum_collision_free=requested,
|
|
220
|
+
collision_at=None,
|
|
221
|
+
collides_with=(),
|
|
222
|
+
units=units,
|
|
223
|
+
reason="The complete sampled motion path is collision-free.",
|
|
224
|
+
samples_tested=tested,
|
|
225
|
+
evidence={
|
|
226
|
+
"method": "sampled swept world-AABB",
|
|
227
|
+
"resolution": resolution,
|
|
228
|
+
"joint_limits": (joint.minimum, joint.maximum),
|
|
229
|
+
},
|
|
230
|
+
)
|
|
231
|
+
|
|
232
|
+
|
|
233
|
+
def _motion_bounds(initial: Bounds, joint: RevoluteJoint | PrismaticJoint, value: float) -> Bounds:
|
|
234
|
+
delta = value - joint.current
|
|
235
|
+
if isinstance(joint, PrismaticJoint):
|
|
236
|
+
offset = tuple(component * delta for component in joint.axis)
|
|
237
|
+
return Bounds(
|
|
238
|
+
tuple(initial.minimum[index] + offset[index] for index in range(3)), # type: ignore[arg-type]
|
|
239
|
+
tuple(initial.maximum[index] + offset[index] for index in range(3)), # type: ignore[arg-type]
|
|
240
|
+
)
|
|
241
|
+
angle = radians(delta)
|
|
242
|
+
axis = np.asarray(joint.axis, dtype=np.float64)
|
|
243
|
+
cross = np.array([[0.0, -axis[2], axis[1]], [axis[2], 0.0, -axis[0]], [-axis[1], axis[0], 0.0]])
|
|
244
|
+
rotation = (
|
|
245
|
+
np.eye(3) * cos(angle) + (1.0 - cos(angle)) * np.outer(axis, axis) + sin(angle) * cross
|
|
246
|
+
)
|
|
247
|
+
pivot = np.asarray(joint.pivot, dtype=np.float64)
|
|
248
|
+
points = [rotation @ (np.asarray(corner) - pivot) + pivot for corner in initial.corners]
|
|
249
|
+
return Bounds.from_points(np.asarray(points))
|
|
250
|
+
|
|
251
|
+
|
|
252
|
+
def _refine_collision(
|
|
253
|
+
world: World,
|
|
254
|
+
object_: WorldObject,
|
|
255
|
+
joint: RevoluteJoint | PrismaticJoint,
|
|
256
|
+
safe: float,
|
|
257
|
+
blocked: float,
|
|
258
|
+
) -> float:
|
|
259
|
+
tolerance = 0.01 if isinstance(joint, RevoluteJoint) else 0.0001
|
|
260
|
+
low, high = safe, blocked
|
|
261
|
+
for _ in range(24):
|
|
262
|
+
if abs(high - low) <= tolerance:
|
|
263
|
+
break
|
|
264
|
+
middle = (low + high) / 2.0
|
|
265
|
+
bounds = _motion_bounds(object_.bounds, joint, middle)
|
|
266
|
+
collision = any(
|
|
267
|
+
candidate.id != object_.id and bounds.intersects(candidate.bounds)
|
|
268
|
+
for candidate in world.objects
|
|
269
|
+
)
|
|
270
|
+
if collision:
|
|
271
|
+
high = middle
|
|
272
|
+
else:
|
|
273
|
+
low = middle
|
|
274
|
+
return low
|