dodox/mujoco_maze
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Remove assertion from collision detection

Browse Source
This commit is contained in:
kngwyu 2020-09-10 11:52:32 +09:00
parent 1bcb90a259
commit 4a195d9848
1 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
mujoco_maze/maze_env_utils.py
View File

@ -84,9 +84,7 @@ class Line:
self.p2 = p2 if isinstance(p2, Point) else np.complex(*p2) self.p2 = p2 if isinstance(p2, Point) else np.complex(*p2)
self.v1 = self.p2 - self.p1 self.v1 = self.p2 - self.p1
self.conj_v1 = np.conjugate(self.v1) self.conj_v1 = np.conjugate(self.v1)
self.norm = np.absolute(self.v1)
if np.absolute(self.v1) <= 1e-8:
raise ValueError(f"p1({p1}) and p2({p2}) are too close")
def _intersect(self, other: Self) -> bool: def _intersect(self, other: Self) -> bool:
v2 = other.p1 - self.p1 v2 = other.p1 - self.p1
@ -180,6 +178,9 @@ class CollisionDetector:
def detect(self, old_pos: np.ndarray, new_pos: np.ndarray) -> Optional[Collision]: def detect(self, old_pos: np.ndarray, new_pos: np.ndarray) -> Optional[Collision]:
move = Line(old_pos, new_pos) move = Line(old_pos, new_pos)
# First, checks that it actually moved
if move.norm <= 1e-8:
return None
# Next, checks that the trajectory cross the wall or not # Next, checks that the trajectory cross the wall or not
collisions = [] collisions = []
for line in self.lines: for line in self.lines: