diff --git a/pyproject.toml b/pyproject.toml index cc43f47fc..1fc7153c3 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -14,6 +14,10 @@ authors = [ dependencies = [ "numpy", "spatialmath-python>=1.1.16", + # Pinned to a commit on SG's unreleased main (1.4 draft) -- Link.py's + # SceneGroup(initlist) call requires the API there. Switch to a normal + # ">=1.4.0" constraint once spatialgeometry actually cuts that release. + "spatialgeometry @ git+https://github.com/jhavl/spatialgeometry.git@ce69b8bbe23ec79c0c395e007725512081808487", "pgraph-python", "scipy", "matplotlib", @@ -128,10 +132,9 @@ build-backend = "scikit_build_core.build" [tool.scikit-build] -wheel.packages = ["src/roboticstoolbox", "src/spatialgeometry"] +wheel.packages = ["src/roboticstoolbox"] sdist.include = [ "src/roboticstoolbox", - "src/spatialgeometry", "tests", "CMakeLists.txt", "README.md", diff --git a/src/roboticstoolbox/bin/rtbtool.py b/src/roboticstoolbox/bin/rtbtool.py index ff4a86c01..7fe4f176d 100755 --- a/src/roboticstoolbox/bin/rtbtool.py +++ b/src/roboticstoolbox/bin/rtbtool.py @@ -16,7 +16,7 @@ from traitlets.config import Config import IPython import argparse -from pathlib import Path +import pathlib import shlex import sys import os @@ -213,7 +213,7 @@ def make_banner(): def examples_path(): # Repository layout: /src/roboticstoolbox/bin/rtbtool.py - return Path(__file__).resolve().parents[3] / "examples" + return pathlib.Path(__file__).resolve().parents[3] / "examples" def startup(): @@ -310,7 +310,7 @@ def out_prompt_tokens(self, cli=None): # set up a script to be executed by IPython when we get there code = None if args.script is not None: - path = Path(args.script) + path = pathlib.Path(args.script) if not path.exists(): raise ValueError(f"script does not exist: {args.script}") code = path.open("r").readlines() diff --git a/src/roboticstoolbox/robot/Link.py b/src/roboticstoolbox/robot/Link.py index 9753593ed..acb08043d 100644 --- a/src/roboticstoolbox/robot/Link.py +++ b/src/roboticstoolbox/robot/Link.py @@ -164,11 +164,11 @@ def __init__( self._name = name # Link geometry - self._geometry = SceneGroup(scene_children=geometry) + self._geometry = SceneGroup(geometry) self._scene_children.append(self._geometry) # Collision Geometry - self._collision = SceneGroup(scene_children=collision) + self._collision = SceneGroup(collision) self._scene_children.append(self._collision) # Link dynamic Parameters diff --git a/src/spatialgeometry/__init__.py b/src/spatialgeometry/__init__.py deleted file mode 100644 index d080d3817..000000000 --- a/src/spatialgeometry/__init__.py +++ /dev/null @@ -1,32 +0,0 @@ -from spatialgeometry.geom import ( - SceneNode, - SceneGroup, - Shape, - Axes, - Arrow, - CollisionShape, - Mesh, - Cylinder, - Cuboid, - Box, - Sphere) - -from spatialgeometry import tools - - -__all__ = [ - # aliased - "tools", - # geom - "Shape", - "CollisionShape", - "Mesh", - "Cylinder", - "Cuboid", - "Box", - "Sphere", - "Axes", - "Arrow", - "SceneNode", - "SceneGroup", -] \ No newline at end of file diff --git a/src/spatialgeometry/geom/CollisionShape.py b/src/spatialgeometry/geom/CollisionShape.py deleted file mode 100644 index 7701ca501..000000000 --- a/src/spatialgeometry/geom/CollisionShape.py +++ /dev/null @@ -1,294 +0,0 @@ -#!/usr/bin/env python -""" -@author: Jesse Haviland -""" - -import sys -import os -import numpy as np -from spatialmath.base.argcheck import getvector -from spatialgeometry.geom import Shape -from spatialgeometry.geom.Shape import update -from warnings import warn - -from typing import Tuple, Union - -# Module-level coal reference — populated on first use, never in Pyodide. -_coal = None - - -def _require_coal(): - """Import coal on first use; raise clearly if unavailable.""" - global _coal - if _coal is not None: - return - if sys.platform == "emscripten": - raise RuntimeError( - "Collision detection is not available in the browser (Pyodide) " - "environment. Use a native Python installation for collision checking." - ) - try: - import coal as _c - _coal = _c - except ImportError: - raise ImportError( - "The 'coal' package is required for collision functionality. " - "Install with: pip install coal" - ) - - -class CollisionShape(Shape): - def __init__(self, collision=True, **kwargs): - self.co = None # coal.CollisionObject, created on first use - self._cinit = False - super().__init__(**kwargs) - self._collision = collision - - def _update_coal(self): - """Push the current world transform into the Coal collision object.""" - if self.co is not None: - self.co.setTranslation(self._wT[:3, 3]) - self.co.setRotation(self._wT[:3, :3]) - - def _init_coal(self): # overridden by each subclass - pass - - def _ensure_coal(self): - """Guarantee Coal is loaded and this object's Coal twin is current.""" - _require_coal() - if not self._cinit: - self._init_coal() - self._update_coal() - - def closest_point( - self, shape: "CollisionShape", inf_dist: float = 1.0 - ) -> Tuple[Union[float, None], Union[np.ndarray, None], Union[np.ndarray, None]]: - """ - Return the minimum euclidean distance between self and shape. - - :param shape: The shape to compare distance to - :param inf_dist: Only return a result when distance < inf_dist - :returns: (d, p1, p2) — distance and closest points in world frame, - or (None, None, None) when the shapes are farther than inf_dist. - d is negative when the shapes are penetrating. - """ - self._ensure_coal() - shape._ensure_coal() - - req = _coal.DistanceRequest() - req.enable_signed_distance = True - res = _coal.DistanceResult() - _coal.distance(self.co, shape.co, req, res) - - d = res.min_distance - if d > inf_dist: - return None, None, None - return d, np.array(res.getNearestPoint1()), np.array(res.getNearestPoint2()) - - def iscollided(self, shape: "CollisionShape") -> bool: - """ - Return True if self and shape have collided (distance ≤ 0). - - :param shape: The shape to check against - """ - d, _, _ = self.closest_point(shape) - return d is not None and d <= 0 - - def collided(self, shape: "CollisionShape") -> bool: - """Deprecated — use iscollided instead.""" - warn("collided is deprecated, use iscollided instead", FutureWarning) - return self.iscollided(shape) - - -class Mesh(CollisionShape): - """ - A mesh object described by an STL, OBJ, or DAE file. - - :param filename: Absolute path to the mesh file. - :param scale: Scale factors along XYZ axes (default [1, 1, 1]). - :param collision: Whether this shape participates in collision checking. - """ - - def __init__(self, filename=None, scale=[1, 1, 1], **kwargs): - super().__init__(stype="mesh", **kwargs) - self.filename = filename - self.scale = scale - - def _init_coal(self): - if not self.collision: - raise ValueError( - "This shape has collision=False and cannot be used as a collision object" - ) - try: - import trimesh - except ImportError: - raise ImportError( - "The 'trimesh' package is required for mesh collision objects. " - "Install with: pip install trimesh" - ) - - mesh = trimesh.load(self.filename, force="mesh") - vertices = (mesh.vertices * self.scale).astype(np.float64, order="C") - triangles = mesh.faces.astype(np.int64, order="C") - - bvh = _coal.BVHModelOBBRSS() - bvh.beginModel(len(triangles), len(vertices)) - bvh.addVertices(vertices) - bvh.addTriangles(triangles) - bvh.endModel() - - self.co = _coal.CollisionObject(bvh) - self._cinit = True - - @property - def scale(self) -> np.ndarray: - return self._scale - - @scale.setter - @update - def scale(self, value): - value = getvector(value if value is not None else [1, 1, 1], 3) - self._scale = np.array(value) - - @property - def filename(self): - return self._filename - - @filename.setter - @update - def filename(self, value): - self._filename = value - - def to_dict(self): - shape = super().to_dict() - shape["filename"] = self.filename - shape["scale"] = self.scale.tolist() - return shape - - -class Cylinder(CollisionShape): - """ - A cylinder whose centre is at the local origin, axis along Z. - - :param radius: Radius in metres. - :param length: Total length in metres. - :param collision: Whether this shape participates in collision checking. - """ - - def __init__(self, radius, length, **kwargs): - super().__init__(stype="cylinder", **kwargs) - self.radius = radius - self.length = length - - def _init_coal(self): - if not self.collision: - raise ValueError( - "This shape has collision=False and cannot be used as a collision object" - ) - # Coal Cylinder(radius, halfLength) - geom = _coal.Cylinder(self.radius, self.length / 2.0) - self.co = _coal.CollisionObject(geom) - self._cinit = True - - @property - def radius(self): - return self._radius - - @radius.setter - @update - def radius(self, value): - self._radius = float(value) - - @property - def length(self): - return self._length - - @length.setter - @update - def length(self, value): - self._length = float(value) - - def to_dict(self): - shape = super().to_dict() - shape["radius"] = self.radius - shape["length"] = self.length - return shape - - -class Sphere(CollisionShape): - """ - A sphere whose centre is at the local origin. - - :param radius: Radius in metres. - :param collision: Whether this shape participates in collision checking. - """ - - def __init__(self, radius, **kwargs): - super().__init__(stype="sphere", **kwargs) - self.radius = radius - - def _init_coal(self): - if not self.collision: - raise ValueError( - "This shape has collision=False and cannot be used as a collision object" - ) - self.co = _coal.CollisionObject(_coal.Sphere(self.radius)) - self._cinit = True - - @property - def radius(self): - return self._radius - - @radius.setter - @update - def radius(self, value): - self._radius = float(value) - - def to_dict(self): - shape = super().to_dict() - shape["radius"] = self.radius - return shape - - -class Cuboid(CollisionShape): - """ - A rectangular prism whose centre is at the local origin. - - :param scale: [length, width, height] in metres. - :param collision: Whether this shape participates in collision checking. - """ - - def __init__(self, scale, **kwargs): - super().__init__(stype="cuboid", **kwargs) - self.scale = scale - - def _init_coal(self): - if not self.collision: - raise ValueError( - "This shape has collision=False and cannot be used as a collision object" - ) - s = self.scale - # Coal Box(x, y, z) takes full dimensions (not half-extents) - self.co = _coal.CollisionObject(_coal.Box(s[0], s[1], s[2])) - self._cinit = True - - @property - def scale(self) -> np.ndarray: - return self._scale - - @scale.setter - @update - def scale(self, value): - value = getvector(value if value is not None else [1, 1, 1], 3) - self._scale = np.array(value) - - def to_dict(self): - shape = super().to_dict() - shape["scale"] = self.scale.tolist() - return shape - - -class Box(Cuboid): - def __init__(self, scale, **kwargs): - warn("Box is deprecated, use Cuboid instead", FutureWarning) - super().__init__(scale, **kwargs) diff --git a/src/spatialgeometry/geom/SceneGroup.py b/src/spatialgeometry/geom/SceneGroup.py deleted file mode 100755 index aa95d701c..000000000 --- a/src/spatialgeometry/geom/SceneGroup.py +++ /dev/null @@ -1,26 +0,0 @@ -#!/usr/bin/env python -""" -@author: Jesse Haviland -""" - -from numpy import ndarray, eye, zeros, copy as npcopy -from spatialmath.base import r2q -from abc import ABC -from collections import UserList - -# from roboticstoolbox.robot.ETS import ETS -from typing import Type - -from spatialgeometry.geom.SceneNode import SceneNode - - -class SceneGroup(SceneNode, UserList): - def __init__(self, **kwargs): - super().__init__(**kwargs) - - def __getitem__(self, i: int) -> SceneNode: - return self._scene_children[i] - - @property - def data(self): - return self._scene_children diff --git a/src/spatialgeometry/geom/SceneNode.py b/src/spatialgeometry/geom/SceneNode.py deleted file mode 100644 index 0bd2cd685..000000000 --- a/src/spatialgeometry/geom/SceneNode.py +++ /dev/null @@ -1,315 +0,0 @@ -#!/usr/bin/env python -""" -@author: Jesse Haviland -""" - -from numpy import ndarray, eye, copy as npcopy, array -from spatialmath.base import r2q -from abc import ABC -from spatialgeometry.scene import node_init, node_update, scene_graph_children, scene_graph_tree -from spatialmath import SE3 -from copy import deepcopy - -# from roboticstoolbox.robot.ETS import ETS -from typing import Type, Union, List - - -class SceneNode: - def __init__( - self, - T: ndarray = eye(4), - scene_parent: Union["SceneNode", None] = None, - scene_children: Union[List["SceneNode"], None] = None, - ): - # These three are static attributes which can never be changed - # If these are directly accessed and re-written, segmentation faults - # will follow very soon after - # wT and sT cannot be accessed and set by users by base can be - # modified through its setter - - # The world transform - self.__wT = eye(4).copy(order="F") - - # The quaternion extracted from wT - self.__wq = array([0.0, 0.0, 0.0, 1.0]) - - # The local transform - self.__T = eye(4).copy(order="F") - self.__T[:] = T.copy(order="F") - - if scene_children is None: - self._scene_children = [] - else: - self._scene_children = scene_children - - self._scene_parent = scene_parent - - # Set up the c object - self.__scene = self.__init_c() - - # Update childs parent - for child in self.scene_children: - child._update_scene_parent(self) - - # Update parents child - if scene_parent is not None: - scene_parent._update_scene_children(self) - - # Update scene tree - self._propogate_scene_children() - - # --------------------------------------------------------------------- # - - def _custom_scene_node_init( - self, - T: ndarray = eye(4), - scene_parent: Union["SceneNode", None] = None, - scene_children: Union[List["SceneNode"], None] = None, - ): - # The world transform - self.__wT = eye(4).copy(order="F") - - # The quaternion extracted from wT - self.__wq = array([0.0, 0.0, 0.0, 1.0]) - - # The local transform - self.__T = eye(4).copy(order="F") - self.__T[:] = T.copy(order="F") - - if scene_children is None: - self._scene_children = [] - else: - self._scene_children = scene_children - - self._scene_parent = scene_parent - - # Set up the c object - self.__scene = self.__init_c() - - # Update childs parent - for child in self.scene_children: - child._update_scene_parent(self) - - # Update parents child - if scene_parent is not None: - scene_parent._update_scene_children(self) - - # Update scene tree - self._propogate_scene_children() - - # --------------------------------------------------------------------- # - - def __init_c(self): - """ - Super Private method which initialises a C object to hold Data - - """ - - return node_init( - len(self._scene_children), - self.__T, - self.__wT, - self.__wq, - self._scene_parent._scene if self._scene_parent is not None else None, - [child._scene for child in self._scene_children], - ) - - def __update_c(self): - """ - Super Private method which updates the C object which holds Data - - """ - - node_update( - self.__scene, - len(self._scene_children), - self._scene_parent._scene if self._scene_parent is not None else None, - [child._scene for child in self._scene_children], - ) - - @property - def _scene(self): - return self.__scene - - # --------------------------------------------------------------------- # - - def __copy__(self): - return deepcopy(self) - - def __deepcopy__(self, memo): - result = SceneNode( - T=self._T, - ) - - result._scene_children = self.scene_children.copy() - result._scene_parent = self.scene_parent - result.__update_c() - memo[id(self)] = result - return result - - def __str__(self) -> str: - if self._scene_parent is not None: - parent = f"{SE3(self._scene_parent._T, check=False).t}" - else: - parent = "None" - - return f"parent: {parent} \n self: {SE3(self._T).t} \n children: {[SE3(child._T).t for child in self._scene_children]}" - - # parent = self.scene_parent - # return f"parent: {hex(id(parent)) if parent is not None else None} \n self: {hex(id(self))} \n children: {[hex(id(child)) for child in self.scene_children]}" - - # --------------------------------------------------------------------- # - - @property - def scene_parent(self) -> Type["SceneNode"]: - """ - Returns the parent node of this object - - """ - return self._scene_parent - - @scene_parent.setter - def scene_parent(self, parent: "SceneNode"): - """ - Sets a new parent node of this object, will automatically update - the parents child - - """ - # Set our parent - self._scene_parent = parent - - # Update our parents children - parent._update_scene_children(self) - - # Update c - self.__update_c() - - def _update_scene_parent(self, parent: "SceneNode"): - """ - Sets a new parent node of this object, does NOT update - the parents child - - """ - self._scene_parent = parent - - # Update c - self.__update_c() - - # --------------------------------------------------------------------- # - - @property - def scene_children(self) -> List["SceneNode"]: - """ - Returns the child nodes of this object - - """ - return self._scene_children - - @scene_children.setter - def scene_children(self, children: List["SceneNode"]): - """ - Sets the child nodes of this object, does not update childs - parent - - """ - # Set our children - self._scene_children = children - - # Update our childrens parent - for child in children: - child._update_scene_parent(self) - - # Update c - self.__update_c() - - def _update_scene_children(self, child: "SceneNode"): - """ - Appends a new child to this object, does NOT update - the childs parent - - """ - self.scene_children.append(child) - - # Update c - self.__update_c() - - # --------------------------------------------------------------------- # - - @property - def _wT(self) -> ndarray: - """ - Returns the transform of this object in the world frame - - """ - return self.__wT - - @property - def _wq(self) -> ndarray: - """ - Returns the quaternion of this object in the world frame. - - """ - return self.__wq - - # --------------------------------------------------------------------- # - - @property - def _T_reference(self) -> ndarray: - """ - Returns the transform of this object with respect to the parent - frame. - - """ - return self.__T - - @property - def _T(self) -> ndarray: - """ - Returns a copy of the transform of this object with respect to the parent - frame. - - """ - return npcopy(self.__T) - - @_T.setter - def _T(self, T: ndarray): - self.__T[:] = T.copy(order="F") - - if self._scene_parent is not None: - self.__wT[:] = self.parent.wT @ self._T - else: - self.__wT[:] = self._T - - self.__wq[:] = r2q(self.__wT[:3, :3], order="xyzs") - - # --------------------------------------------------------------------- # - # Scene transform propogation methods - # - # The scene graph is a Forest -- A disjoint union of Rooted Trees - # Each tree has a single root, no cycles, and each node has at most one - # parent but unlimited children. - # --------------------------------------------------------------------- # - - def _propogate_scene_children(self): - """ - Propogates the world transform starting from this node going downwards - through the tree (will not go through parents) - """ - scene_graph_children(self.__scene) - - def _propogate_scene_tree(self): - """ - Propogates the world transform starting from this root of the tree in - which this node lives - """ - scene_graph_tree(self.__scene) - - # --------------------------------------------------------------------- # - - def attach(self, object: "SceneNode"): - new_childs = self.scene_children - new_childs.append(object) - self.scene_children = new_childs - - def attach_to(self, object: "SceneNode"): - self.scene_parent = object diff --git a/src/spatialgeometry/geom/Shape.py b/src/spatialgeometry/geom/Shape.py deleted file mode 100644 index ab545363e..000000000 --- a/src/spatialgeometry/geom/Shape.py +++ /dev/null @@ -1,420 +0,0 @@ -#!/usr/bin/env python -""" -@author: Jesse Haviland -""" - -from functools import wraps -from multiprocessing.sharedctypes import Value -from spatialgeometry.geom.SceneNode import SceneNode -from spatialmath import SE3 -from spatialmath.base.argcheck import getvector -from spatialmath.base import r2q -from copy import copy as ccopy, deepcopy -from numpy import ( - ndarray, - copy as npcopy, - pi, - zeros, - array, - any, - concatenate, - eye, - array_equal, -) -from typing import Union, Tuple, Dict, Any -from warnings import warn - -import spatialmath.base as smb -import numpy as np - -ArrayLike = Union[list, ndarray, tuple, set] -_mpl = False -# _rtb = False - - -def update(func): # pragma nocover - @wraps(func) - def wrapper_update(*args, **kwargs): - - if args[0]._added_to_swift: - args[0]._changed = True - - return func(*args, **kwargs) - - return wrapper_update - - -try: - from matplotlib import colors as mpc - - _mpl = True -except ImportError: # pragma nocover - pass - - -# try: -# import roboticstoolbox as rtb - -# _rtb = True -# except ImportError: # pragma nocover -# pass - - -CONST_RX = SE3.Rx(pi / 2).A - - -class Shape(SceneNode): - def __init__( - self, - pose: Union[ndarray, SE3] = eye(4), - color: ArrayLike = None, - stype: str = None, - base: Union[ndarray, SE3, None] = None, - **kwargs, - ): - - # Swift related attributes - self._added_to_swift = False - self._changed = False - - if base is not None: - warn("base kwarg is deprecated, use pose instead", FutureWarning) - - if isinstance(base, SE3): - T = base.A - else: - T = base - - if T is not None and not array_equal(pose, eye(4)): - raise ValueError( - "You cannot use both base and pose kwargs as they offer identical functionality. Use only pose." - ) - - else: - - if isinstance(pose, SE3): - T = pose.A - else: - T = pose - - if color is None: - self._color = (0.3, 0.3, 0.3, 1.0) - else: - self.color = color - - # Initialise the scene node - super().__init__(T=T, **kwargs) - - self.stype = stype - self.v = zeros(6) - self.attached = True - - self._collision = False - - # --------------------------------------------------------------------- # - - def copy(self) -> "Shape": - """ - Copy of Shape object - - :return: Shallow copy of Shape object - :rtype: Shape - """ - - new = ccopy(self) - - for k, v in self.__dict__.items(): - if k.startswith("_") and isinstance(v, ndarray): - setattr(new, k, npcopy(v)) - - return new - - def __copy__(self): - return deepcopy(self) - - def __deepcopy__(self, memo): - cls = self.__class__ - result = cls.__new__(cls) - - memo[id(self)] = result - - for k, v in self.__dict__.items(): - if not k.lower().startswith("_scene"): - setattr(result, k, deepcopy(v, memo)) - - result._custom_scene_node_init(T=deepcopy(self.T)) - - return result - - def __str__(self) -> str: - return f"stype: {self.stype} \n pose: {SE3(self._T).t}" - - # --------------------------------------------------------------------- # - - def _to_hex(self, rgb) -> int: - rgb = (array(rgb) * 255).astype(int) - return int("0x%02x%02x%02x" % (rgb[0], rgb[1], rgb[2]), 16) - - def to_dict(self) -> Dict[str, Any]: - """ - to_dict() returns the shapes information in dictionary form - - :returns: All information about the shape - :rtype: dict - """ - self._to_hex(self.color[0:3]) - - shape = { - "stype": self.stype, - "t": self._wT[:3, 3].tolist(), - "q": self._wq.tolist(), - "v": self.v.tolist(), - "color": self._to_hex(self.color[0:3]), - "opacity": self.color[3], - } - - return shape - - def fk_dict(self) -> Dict[str, Any]: - """ - fk_dict() outputs shapes pose in dictionary form - - :returns: The shape pose in translation and quternion form - :rtype: dict - """ - - # q = smb.r2q(self._wT[:3, :3]) - # q = [q[1], q[2], q[3], q[0]] - # shape = {"t": self._wT[:3, 3].tolist(), "q": q} - - shape = {"t": self._wT[:3, 3].tolist(), "q": self._wq.tolist()} - - return shape - - def __repr__(self) -> str: # pragma nocover - return f"{self.stype},\n{self.T[:3, -1]}" - # return f"{hex(id(self.stype))}" - - @property - def collision(self) -> bool: - return self._collision - - @property - def v(self) -> ndarray: - return self._v - - @v.setter - def v(self, value: ArrayLike): - self._v = array(getvector(value, 6)) - - @property - def color(self) -> Tuple[float, float, float, float]: - """ - shape.color returns a four length tuple representing (red, green, blue, alpha) - where alpha represents transparency. Values returned are in the range [0-1]. - """ - return self._color - - @color.setter - @update - def color(self, value: ArrayLike): - """ - shape.color(new_color) sets the color of a shape. - - The color format is (red, green, blue, alpha). - - Color can be set with a three length list, tuple or array which - will only set the (r, g, b) values and alpha will be set to maximum. - - Color can be set with a four length list, tuple or array which - will set the (r, g, b, a) values. - - Note: the color is auto-normalising. If any value passed is greater than - 1.0 then all values will be normalised to the [0-1] range assuming the - previous range was [0-255]. - """ - - default_color = (0.95, 0.5, 0.25, 1.0) - - if isinstance(value, str): - if _mpl: - try: - value = mpc.to_rgba(value) - except ValueError: - print(f"{value} is an invalid color name, using default color") - value = default_color - else: # pragma nocover - value = default_color - print( - "Color only supported when matplotlib is installed\n" - "Install using: pip install matplotlib" - ) - elif value is None: - value = default_color - else: - value = array(value) - - if any(value > 1.0): - value = value / 255.0 - - if value.shape[0] == 3: # type: ignore - value = concatenate([value, [1.0]]) - - value = tuple(value) - - self._color = value - - def set_alpha(self, alpha: Union[float, int]): - """ - Convenience method to set the opacity/alpha value of the robots color. - """ - - if alpha > 1.0: - alpha /= 255 - - new_color = concatenate([self._color[:3], [alpha]]) - self._color = tuple(new_color) - - # --------------------------------------------------------------------- # - # SceneNode properties - # These relate to how scene node operates - - @property - def T(self) -> ndarray: - return self._T - - @T.setter - def T(self, T_new: Union[ndarray, SE3]): - if isinstance(T_new, SE3): - T_new = T_new.A - self._T = T_new - - # --------------------------------------------------------------------- # - - -class Axes(Shape): - """An axes whose center is at the local origin. - Parameters - - :param length: The length of each axis. - :type length: float - :param pose: Local reference frame of the shape - :type pose: SE3 - - """ - - def __init__(self, length, **kwargs): - super(Axes, self).__init__(stype="axes", **kwargs) - self.length = length - - @property - def length(self): - return self._length - - @length.setter - @update - def length(self, value): - self._length = float(value) - - def to_dict(self): - """ - to_dict() returns the shapes information in dictionary form - - :returns: All information about the shape - :rtype: dict - """ - - shape = super().to_dict() - shape["length"] = self.length - return shape - - -class Arrow(Shape): - """An arrow whose center is at the local origin, and points - in the positive z direction. - - The arrow is made using a cylinder and a cone - - Parameters - - :param length: The total length of the arrow. - :param radius: The radius of the arrow body. If radius is 0, then the - arrow is made with a line. - :param head_length: The lenght of the cone (head of the arrow). This is - represented as a fraction of the lenght. Must be a value between 0 - and 1. - :param head_radius: The width of the cone (head of the arrow). This is - represented as a fraction of the head_length. - - :param pose: Local reference frame of the shape - :type pose: SE3 - - """ - - def __init__( - self, - length: float, - radius: float = 0.0, - head_length: float = 0.2, - head_radius: float = 0.2, - **kwargs, - ): - if head_length > 1.0 or head_length < 0.0: - raise ValueError("Head length must be a value between 0 and 1") - - super(Arrow, self).__init__(stype="arrow", **kwargs) - self.length = length - self.radius = radius - self.head_length = head_length - self.head_radius = head_radius - - @property - def length(self): - return self._length - - @length.setter - @update - def length(self, value): - self._length = float(value) - - @property - def radius(self): - return self._radius - - @radius.setter - @update - def radius(self, value): - self._radius = float(value) - - @property - def head_length(self): - return self._head_length - - @head_length.setter - @update - def head_length(self, value): - self._head_length = float(value) - - @property - def head_radius(self): - return self._head_radius - - @head_radius.setter - @update - def head_radius(self, value): - self._head_radius = float(value) - - def to_dict(self): - """ - to_dict() returns the shapes information in dictionary form - - :returns: All information about the shape - :rtype: dict - """ - - shape = super().to_dict() - shape["length"] = self.length - shape["radius"] = self.radius - shape["head_length"] = self.head_length - shape["head_radius"] = self.head_radius - return shape diff --git a/src/spatialgeometry/geom/__init__.py b/src/spatialgeometry/geom/__init__.py deleted file mode 100644 index cb8219605..000000000 --- a/src/spatialgeometry/geom/__init__.py +++ /dev/null @@ -1,26 +0,0 @@ -from spatialgeometry.geom.SceneNode import SceneNode -from spatialgeometry.geom.SceneGroup import SceneGroup -from spatialgeometry.geom.Shape import Shape, Axes, Arrow -from spatialgeometry.geom.CollisionShape import ( - CollisionShape, - Mesh, - Cylinder, - Cuboid, - Box, - Sphere, -) - - -__all__ = [ - "Shape", - "CollisionShape", - "Mesh", - "Cylinder", - "Cuboid", - "Box", - "Sphere", - "Axes", - "Arrow", - "SceneNode", - "SceneGroup", -] diff --git a/src/spatialgeometry/scene.py b/src/spatialgeometry/scene.py deleted file mode 100644 index 6933cf280..000000000 --- a/src/spatialgeometry/scene.py +++ /dev/null @@ -1,107 +0,0 @@ -#!/usr/bin/env python3 - -from __future__ import annotations - -from dataclasses import dataclass, field -from typing import Iterable, Optional - -import numpy as np -from numpy.typing import NDArray - - -ArrayF64 = NDArray[np.float64] - - -@dataclass -class _Node: - T: ArrayF64 - wT: ArrayF64 - wq: ArrayF64 - parent: Optional["_Node"] = None - children: list["_Node"] = field(default_factory=list) - - -def _r2q_xyzs(R: ArrayF64) -> ArrayF64: - q = np.zeros(4, dtype=np.float64) - tr = float(R[0, 0] + R[1, 1] + R[2, 2]) - - if tr > 0.0: - S = np.sqrt(tr + 1.0) * 2.0 - q[3] = 0.25 * S - q[0] = (R[2, 1] - R[1, 2]) / S - q[1] = (R[0, 2] - R[2, 0]) / S - q[2] = (R[1, 0] - R[0, 1]) / S - elif (R[0, 0] > R[1, 1]) and (R[0, 0] > R[2, 2]): - S = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2.0 - q[3] = (R[2, 1] - R[1, 2]) / S - q[0] = 0.25 * S - q[1] = (R[0, 1] + R[1, 0]) / S - q[2] = (R[0, 2] + R[2, 0]) / S - elif R[1, 1] > R[2, 2]: - S = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2.0 - q[3] = (R[0, 2] - R[2, 0]) / S - q[0] = (R[0, 1] + R[1, 0]) / S - q[1] = 0.25 * S - q[2] = (R[1, 2] + R[2, 1]) / S - else: - S = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2.0 - q[3] = (R[1, 0] - R[0, 1]) / S - q[0] = (R[0, 2] + R[2, 0]) / S - q[1] = (R[1, 2] + R[2, 1]) / S - q[2] = 0.25 * S - - return q - - -def _propogate_T(node: _Node, parent_wT: Optional[ArrayF64]) -> None: - if parent_wT is None: - node.wT[:] = node.T - else: - node.wT[:] = parent_wT @ node.T - - node.wq[:] = _r2q_xyzs(node.wT[:3, :3]) - - for child in node.children: - _propogate_T(child, node.wT) - - -def node_init( - n_children: int, - T: ArrayF64, - wT: ArrayF64, - wq: ArrayF64, - parent: Optional[_Node], - children: Iterable[_Node], -): - node = _Node(T=T, wT=wT, wq=wq) - node.parent = parent - node.children = list(children) - - if n_children != len(node.children): - node.children = node.children[:n_children] - - return node - - -def node_update( - node: _Node, - n_children: int, - parent: Optional[_Node], - children: Iterable[_Node], -): - node.parent = parent - node.children = list(children) - - if n_children != len(node.children): - node.children = node.children[:n_children] - - -def scene_graph_children(node: _Node): - _propogate_T(node, None) - - -def scene_graph_tree(node: _Node): - while node.parent is not None: - node = node.parent - - _propogate_T(node, None) diff --git a/src/spatialgeometry/tools/__init__.py b/src/spatialgeometry/tools/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/src/spatialgeometry/tools/stdout_supress.py b/src/spatialgeometry/tools/stdout_supress.py deleted file mode 100644 index 10fdf1a83..000000000 --- a/src/spatialgeometry/tools/stdout_supress.py +++ /dev/null @@ -1,302 +0,0 @@ -# Code Author Github user minrk -# Originally from https://github.com/minrk/wurlitzer/blob/master/wurlitzer.py - - -from __future__ import print_function - -from contextlib import contextmanager -import ctypes -import errno -from fcntl import fcntl, F_GETFL, F_SETFL -import io -import os - -try: - from queue import Queue -except ImportError: # pragma nocover - from Queue import Queue - -import selectors -import sys -import threading -import time - -libc = ctypes.CDLL(None) - -STDOUT = 2 -PIPE = 3 - -try: - c_stdout_p = ctypes.c_void_p.in_dll(libc, "stdout") - c_stderr_p = ctypes.c_void_p.in_dll(libc, "stderr") -except ValueError: # pragma: no cover - # libc.stdout is has a funny name on OS X - c_stdout_p = ctypes.c_void_p.in_dll(libc, "__stdoutp") # pragma: no cover - c_stderr_p = ctypes.c_void_p.in_dll(libc, "__stderrp") # pragma: no cover - -_default_encoding = getattr(sys.stdin, "encoding", None) or "utf8" -if _default_encoding.lower() == "ascii": - # don't respect ascii - _default_encoding = "utf8" # pragma: no cover - - -def dup2(a, b, timeout=3): - """Like os.dup2, but retry on EBUSY""" - dup_err = None - # give FDs 3 seconds to not be busy anymore - for i in range(int(10 * timeout)): - try: - return os.dup2(a, b) - except OSError as e: # pragma nocover - dup_err = e - if e.errno == errno.EBUSY: - time.sleep(0.1) - else: - raise - if dup_err: # pragma nocover - raise dup_err - - -class Wurlitzer(object): # pragma: no cover - """Class for Capturing Process-level FD output via dup2 - - Typically used via `wurlitzer.capture` - """ - - flush_interval = 0.2 - - def __init__(self, stdout=None, stderr=None, encoding=_default_encoding): - """ - Parameters - ---------- - stdout: stream or None - The stream for forwarding stdout. - stderr = stream or None - The stream for forwarding stderr. - encoding: str or None - The encoding to use, if streams should be interpreted as text. - """ - self._stdout = stdout - self._stderr = stderr - self.encoding = encoding - self._save_fds = {} - self._real_fds = {} - self._handlers = {} - self._handlers["stderr"] = self._handle_stderr - self._handlers["stdout"] = self._handle_stdout - - def _setup_pipe(self, name): - real_fd = getattr(sys, "__%s__" % name).fileno() - save_fd = os.dup(real_fd) - self._save_fds[name] = save_fd - - pipe_out, pipe_in = os.pipe() - dup2(pipe_in, real_fd) - os.close(pipe_in) - self._real_fds[name] = real_fd - - # make pipe_out non-blocking - flags = fcntl(pipe_out, F_GETFL) - fcntl(pipe_out, F_SETFL, flags | os.O_NONBLOCK) - return pipe_out - - def _decode(self, data): - """Decode data, if any - - Called before passing to stdout/stderr streams - """ - if self.encoding: - data = data.decode(self.encoding, "replace") - return data - - def _handle_stdout(self, data): - if self._stdout: - self._stdout.write(self._decode(data)) - - def _handle_stderr(self, data): - if self._stderr: - self._stderr.write(self._decode(data)) - - def _setup_handle(self): - """Setup handle for output, if any""" - self.handle = (self._stdout, self._stderr) - - def _finish_handle(self): - """Finish handle, if anything should be done when it's all wrapped up.""" - pass - - def _flush(self): - """flush sys.stdout/err and low-level FDs""" - if self._stdout and sys.stdout: - sys.stdout.flush() - if self._stderr and sys.stderr: - sys.stderr.flush() - - libc.fflush(c_stdout_p) - libc.fflush(c_stderr_p) - - def __enter__(self): - # flush anything out before starting - self._flush() - # setup handle - self._setup_handle() - self._control_r, self._control_w = os.pipe() - - # create pipe for stdout - pipes = [self._control_r] - names = {self._control_r: "control"} - if self._stdout: - pipe = self._setup_pipe("stdout") - pipes.append(pipe) - names[pipe] = "stdout" - if self._stderr: - pipe = self._setup_pipe("stderr") - pipes.append(pipe) - names[pipe] = "stderr" - - # flush pipes in a background thread to avoid blocking - # the reader thread when the buffer is full - flush_queue = Queue() - - def flush_main(): - while True: - msg = flush_queue.get() - if msg == "stop": - return - self._flush() - - flush_thread = threading.Thread(target=flush_main) - flush_thread.daemon = True - flush_thread.start() - - def forwarder(): - """Forward bytes on a pipe to stream messages""" - draining = False - flush_interval = 0 - poller = selectors.DefaultSelector() - - for pipe_ in pipes: - poller.register(pipe_, selectors.EVENT_READ) - - while pipes: - events = poller.select(flush_interval) - if events: - # found something to read, don't block select until - # we run out of things to read - flush_interval = 0 - else: - # nothing to read - if draining: - # if we are draining and there's nothing to read, stop - break - else: - # nothing to read, get ready to wait. - # flush the streams in case there's something waiting - # to be written. - flush_queue.put("flush") - flush_interval = self.flush_interval - continue - - for selector_key, flags in events: - fd = selector_key.fd - if fd == self._control_r: - draining = True - pipes.remove(self._control_r) - poller.unregister(self._control_r) - os.close(self._control_r) - continue - name = names[fd] - data = os.read(fd, 1024) - if not data: - # pipe closed, stop polling it - pipes.remove(fd) - poller.unregister(fd) - os.close(fd) - else: - handler = getattr(self, "_handle_%s" % name) - handler(data) - if not pipes: - # pipes closed, we are done - break - # stop flush thread - flush_queue.put("stop") - flush_thread.join() - # cleanup pipes - [os.close(pipe) for pipe in pipes] - - self.thread = threading.Thread(target=forwarder) - self.thread.daemon = True - self.thread.start() - - return self.handle - - def __exit__(self, exc_type, exc_value, traceback): - # flush before exiting - self._flush() - - # signal output is complete on control pipe - os.write(self._control_w, b"\1") - self.thread.join() - os.close(self._control_w) - - # restore original state - for name, real_fd in self._real_fds.items(): - save_fd = self._save_fds[name] - dup2(save_fd, real_fd) - os.close(save_fd) - # finalize handle - self._finish_handle() - - -@contextmanager -def pipes( - stdout=PIPE, stderr=PIPE, encoding=_default_encoding -): # pragma: no cover # noqa - """Capture C-level stdout/stderr in a context manager. - The return value for the context manager is (stdout, stderr). - Examples - -------- - >>> with capture() as (stdout, stderr): - ... printf("C-level stdout") - ... output = stdout.read() - """ - stdout_pipe = stderr_pipe = False - # setup stdout - if stdout == PIPE: - stdout_r, stdout_w = os.pipe() - stdout_w = os.fdopen(stdout_w, "wb") - if encoding: - stdout_r = io.open(stdout_r, "r", encoding=encoding) - else: - stdout_r = os.fdopen(stdout_r, "rb") - stdout_pipe = True - else: - stdout_r = stdout_w = stdout - # setup stderr - if stderr == STDOUT: - stderr_r = None - stderr_w = stdout_w - elif stderr == PIPE: - stderr_r, stderr_w = os.pipe() - stderr_w = os.fdopen(stderr_w, "wb") - if encoding: - stderr_r = io.open(stderr_r, "r", encoding=encoding) - else: - stderr_r = os.fdopen(stderr_r, "rb") - stderr_pipe = True - else: - stderr_r = stderr_w = stderr - if stdout_pipe or stderr_pipe: - capture_encoding = None - else: - capture_encoding = encoding - w = Wurlitzer(stdout=stdout_w, stderr=stderr_w, encoding=capture_encoding) - try: - with w: - yield stdout_r, stderr_r - finally: - # close pipes - if stdout_pipe: - stdout_w.close() - if stderr_pipe: - stderr_w.close() diff --git a/tests/test_DHRobot.py b/tests/test_DHRobot.py index 56f12308b..79f85a735 100644 --- a/tests/test_DHRobot.py +++ b/tests/test_DHRobot.py @@ -375,14 +375,34 @@ def test_islimit(self): nt.assert_array_equal(panda.islimit(), ans) def test_isspherical(self): - l0 = rp.RevoluteDH() - l1 = rp.RevoluteDH(alpha=-np.pi / 2) - l2 = rp.RevoluteDH(alpha=np.pi / 2) - l3 = rp.RevoluteDH() + # Sharing link instances across robots (as r2 does here, relative + # to r0/r1) should be fine -- links are meant to be stateless + # (desiderata.md). It breaks today only because Link/BaseLink + # currently inherits SceneNode, so a link's scene_parent is + # single-valued scene-graph state bolted onto what should be a + # pure kinematic-model object; SG's cycle detection then (rightly) + # rejects the same instance having two different parents across + # r0's and r2's differing link order. Workaround here, not a + # statement that link-sharing itself is wrong -- see #571 (Robot/ + # Link vs scene-graph state decoupling) for the real fix. Once + # that lands, this can go back to sharing l0-l3 directly. + # isspherical() is purely DH-parameter-based (DHRobot.py), so + # fresh instances with the same parameter values are + # behaviour-equivalent in the meantime. + def links(): + return [ + rp.RevoluteDH(), + rp.RevoluteDH(alpha=-np.pi / 2), + rp.RevoluteDH(alpha=np.pi / 2), + rp.RevoluteDH(), + ] + l0, l1, l2, l3 = links() r0 = rp.DHRobot([l0, l1, l2, l3]) r1 = rp.DHRobot([l0, l1]) - r2 = rp.DHRobot([l1, l2, l3, l0]) + + m0, m1, m2, m3 = links() + r2 = rp.DHRobot([m1, m2, m3, m0]) self.assertTrue(r0.isspherical()) self.assertFalse(r1.isspherical()) diff --git a/tests/test_collision.py b/tests/test_collision.py index bad559c32..01cca1e89 100644 --- a/tests/test_collision.py +++ b/tests/test_collision.py @@ -405,14 +405,20 @@ def test_cuboid_none_scale_defaults(self): d = gm.Cuboid(None).to_dict() assert d["scale"] == [1.0, 1.0, 1.0] - def test_mesh_stype(self): - assert gm.Mesh("robot.stl").to_dict()["stype"] == "mesh" + def test_mesh_stype(self, tmp_path): + f = tmp_path / "robot.stl" + f.touch() + assert gm.Mesh(str(f)).to_dict()["stype"] == "mesh" - def test_mesh_filename(self): - assert gm.Mesh("robot.stl").to_dict()["filename"] == "robot.stl" + def test_mesh_filename(self, tmp_path): + f = tmp_path / "robot.stl" + f.touch() + assert gm.Mesh(str(f)).to_dict()["filename"] == str(f) - def test_mesh_scale(self): - assert gm.Mesh("robot.stl", scale=[2, 2, 2]).to_dict()["scale"] == [2.0, 2.0, 2.0] + def test_mesh_scale(self, tmp_path): + f = tmp_path / "robot.stl" + f.touch() + assert gm.Mesh(str(f), scale=[2, 2, 2]).to_dict()["scale"] == [2.0, 2.0, 2.0] # ── _init_coal collision=False direct call ──────────────────────────────────── @@ -421,8 +427,10 @@ def test_mesh_scale(self): class TestInitCoalDirect: """Call _init_coal() directly, matching the pattern in the original suite.""" - def test_mesh_collision_false(self): - s = gm.Mesh("test.stl", collision=False) + def test_mesh_collision_false(self, tmp_path): + f = tmp_path / "test.stl" + f.touch() + s = gm.Mesh(str(f), collision=False) _mod._require_coal() # ensure coal loaded so _init_coal can proceed with pytest.raises(ValueError, match="collision=False"): s._init_coal()