Tool mounting base frame

CHANGELOG.md

@@ -9,8 +9,14 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Added
 
+* New `ToolModel.reframe_base` method, and a matching `base_frame` argument on the constructor: state that the robot's flange takes hold of the tool at a given frame rather than at the origin, and the tool is re-expressed accordingly. It lets a tool be modelled wherever is convenient (a mesh drawn in a CAD document, say) while stating separately where the robot mounts it — previously the geometry had to be modelled in the tool's base frame, since that frame was hardcoded to the origin. Works for tools with joints as well: the geometry of the base link, the joints leaving it, and the TCF all follow, so a gripper keeps articulating around the correct axes. The geometry is not baked, the frame is folded into the `origin` of the link and joint elements, the same mechanism URDF uses.
+
 ### Changed
 
+### Fixed
+
+* `Joint._create` no longer rotates `current_axis` once more every time it runs. It transformed the axis in place instead of recomputing it from `axis`, so re-initializing the transformation tree of a model that has joints (e.g. after its structure changed) left the joint axes wrong — 90 degrees out per extra call for a joint whose origin is rotated by 90 degrees. Models built or loaded in one pass were unaffected, which is why this went unnoticed.
+
 ### Removed
 
 

conftest.py

@@ -5,19 +5,20 @@
 import numpy
 
 
-def pytest_ignore_collect(path):
-    if "rhino" in str(path):
+def pytest_ignore_collect(collection_path):
+    if "rhino" in str(collection_path):
         return True
 
-    if "blender" in str(path):
+    if "blender" in str(collection_path):
         return True
 
-    if "ghpython" in str(path):
+    if "ghpython" in str(collection_path):
         return True
-    
-    if "viewer" in str(path):
+
+    if "viewer" in str(collection_path):
         return True
 
+
 @pytest.fixture(autouse=True)
 def add_compas(doctest_namespace):
     doctest_namespace["compas"] = compas

src/compas_robots/model/joint.py

@@ -599,6 +599,10 @@ def _create(self, transformation: Transformation) -> None:
 
         """
         self.current_origin = self.origin.transformed(transformation)
+        # Recomputed from `axis` rather than transformed in place, so that
+        # re-initializing the tree (e.g. after the model structure changed)
+        # yields the same result instead of rotating the axis once more.
+        self.current_axis = self.axis.copy()
         self.current_axis.transform(self.current_transformation)
 
     def calculate_revolute_transformation(self, position: float) -> Rotation:

src/compas_robots/model/tool.py

@@ -1,5 +1,6 @@
 from __future__ import annotations
 
+import itertools
 from typing import TYPE_CHECKING
 
 from compas.geometry import Frame
@@ -17,6 +18,28 @@
 class ToolModel(RobotModel):
     """Represents a tool to be attached to the robot's flange.
 
+    Parameters
+    ----------
+    visual
+        The visual mesh of the tool.
+    frame_in_tool0_frame
+        The frame of the tool tip (TCF), see `base_frame` for the
+        coordinate system it is expressed in.
+    collision
+        The collision mesh of the tool. Defaults to the visual mesh.
+    name
+        The name of the tool. Defaults to ``'attached_tool'``.
+    connected_to
+        The name of the `Link` to which the tool is attached. Defaults to `None`.
+    base_frame
+        The frame at which the tool mounts onto the robot's flange, expressed in
+        the coordinate system the geometry was modelled in. Shorthand for calling
+        [reframe_base][compas_robots.ToolModel.reframe_base] right after
+        construction, see there for details.
+
+        Defaults to `None`, meaning the geometry is already modelled in the tool's
+        base frame, i.e. the flange sits at its origin.
+
     Attributes
     ----------
     frame : compas.geometry.Frame
@@ -33,6 +56,14 @@ class ToolModel(RobotModel):
     >>> frame = Frame([0.14, 0, 0], [0, 1, 0], [0, 0, 1])
     >>> tool = ToolModel(mesh, frame)
 
+    A tool modelled along the Z axis of the document it was drawn in, mounted so
+    that the Z axis of the drawing points away from the robot:
+
+    >>> base_frame = Frame([0, 0, 0], [0, 0, 1], [1, 0, 0])
+    >>> tool = ToolModel(mesh, Frame([0, 0, 0.14], [0, 1, 0], [1, 0, 0]), base_frame=base_frame)
+    >>> tool.frame.point
+    Point(x=0.140, y=0.000, z=0.000)
+
     """
 
     def __init__(
@@ -42,6 +73,7 @@ def __init__(
         collision=None,
         name="attached_tool",
         connected_to=None,
+        base_frame=None,
     ):
         super(ToolModel, self).__init__(name)
         self.frame = frame_in_tool0_frame
@@ -54,6 +86,64 @@ def __init__(
             self._rebuild_tree()
             self._create(self.root, Transformation())
 
+        if base_frame:
+            self.reframe_base(base_frame)
+
+    def reframe_base(self, base_frame: Frame) -> None:
+        """Re-express the tool relative to the frame the robot's flange takes hold of it at.
+
+        Everything the tool is made of — its geometry, its TCF, and the joints
+        leaving its base link — is currently expressed relative to the tool's base
+        frame. This states that the flange actually sits at `base_frame` instead of
+        at the origin, and re-expresses all of it accordingly.
+
+        Use it to model a tool wherever is convenient (a mesh drawn in a CAD document,
+        say) and declare separately where the robot takes hold of it, rather than
+        having to model it in its own base frame.
+
+        Nothing is baked into the geometry: the frame is folded into the `origin` of
+        the base link's visual and collision elements, and of the joints attached to
+        it, which is the same mechanism URDF uses. Calling this again re-frames the
+        tool again, relative to its current base.
+
+        Parameters
+        ----------
+        base_frame
+            The frame at which the robot's flange takes hold of the tool, expressed
+            in the tool's current coordinate system.
+
+        Examples
+        --------
+        >>> import compas
+        >>> from compas.datastructures import Mesh
+        >>> from compas.geometry import Frame
+        >>> mesh = Mesh.from_stl(compas.get("cone.stl"))
+        >>> tool = ToolModel(mesh, Frame([0, 0, 0.14], [0, 1, 0], [1, 0, 0]))
+        >>> tool.reframe_base(Frame([0, 0, 0], [0, 0, 1], [1, 0, 0]))
+        >>> tool.frame.point
+        Point(x=0.140, y=0.000, z=0.000)
+
+        """
+        transformation = Transformation.from_frame(base_frame).inverted()
+
+        self.frame = base_frame.to_local_coordinates(self.frame)
+
+        if self.root:
+            # The geometry of the base link, and the joints hanging off it, are placed
+            # relative to the base link's frame, which is what is being moved here.
+            for item in itertools.chain(self.root.visual, self.root.collision, self.root.joints):
+                item.origin = self._reframed_origin(item.origin, transformation)
+
+            self._rebuild_tree()
+            self._create(self.root, Transformation())
+
+    @staticmethod
+    def _reframed_origin(origin: Optional[Frame], transformation: Transformation) -> Frame:
+        """Compose `transformation` onto an origin that may not be set yet (i.e. identity)."""
+        if not origin:
+            return Frame.from_transformation(transformation)
+        return Frame.from_transformation(transformation * Transformation.from_frame(origin))
+
     @classmethod
     def from_robot_model(cls, robot: RobotModel, frame_in_tool0_frame: Frame, connected_to: Optional[str] = None) -> ToolModel:
         """Creates a `ToolModel` from a [RobotModel][compas_robots.RobotModel] instance.

tests/test_tool.py

@@ -3,21 +3,57 @@
 import compas
 import pytest
 from compas.datastructures import Mesh
+from compas.geometry import Box
+from compas.geometry import Cone
 from compas.geometry import Frame
 from compas.geometry import Point
 from compas.geometry import Vector
 from compas.geometry import allclose
 
 from compas_robots import ToolModel
+from compas_robots.model import Joint
 
 BASE_FOLDER = os.path.dirname(__file__)
 
 
+def unproxied(origin):
+    """Link origins are wrapped in a URDF `FrameProxy`, which proxies attributes
+    but not equality, so unwrap before comparing."""
+    return Frame(origin.point, origin.xaxis, origin.yaxis)
+
+
 @pytest.fixture
 def mesh():
     return Mesh.from_stl(compas.get("cone.stl"))
 
 
+@pytest.fixture
+def cone_along_z():
+    """A cone modelled along +Z, 0.1 long with a radius of 0.02, the way a tool
+    tends to come out of a CAD document."""
+    return Mesh.from_shape(Cone(radius=0.02, height=0.1))
+
+
+@pytest.fixture
+def kinematic_gripper():
+    """A tool with joints: a body plus two jaws sliding apart along Y, modelled
+    reaching along +Z the way a gripper tends to come out of a CAD document."""
+    tool = ToolModel(Mesh.from_shape(Box(0.04)), Frame([0, 0, 0.1], [0, 1, 0], [1, 0, 0]), name="gripper")
+    base = tool.root
+    for index, side in enumerate([1, -1]):
+        jaw = tool.add_link("jaw_{}".format(index), visual_mesh=Mesh.from_shape(Box(0.01)))
+        tool.add_joint(
+            "jaw_joint_{}".format(index),
+            Joint.PRISMATIC,
+            base,
+            jaw,
+            origin=Frame([0, side * 0.02, 0.05], [1, 0, 0], [0, 1, 0]),
+            axis=Vector(0, side, 0),
+            limit=(0.0, 0.02),
+        )
+    return tool
+
+
 @pytest.fixture
 def cone_tool_json():
     return os.path.join(BASE_FOLDER, "fixtures", "cone_tool.json")
@@ -33,6 +69,76 @@ def test_basic_tool_model(mesh, frame):
     assert tool.name == "attached_tool"
 
 
+def test_without_a_base_frame_the_modelling_coordinates_are_the_base(mesh, frame):
+    tool = ToolModel(mesh, frame)
+    assert tool.frame == frame
+    assert all(item.origin is None for item in tool.root.visual + tool.root.collision)
+
+
+def test_base_frame_re_expresses_geometry_and_tcf(cone_along_z):
+    """A tool modelled along +Z, mounted so that +Z of the drawing points away
+    from the robot: the tool's own frame ends up with the tip on +X.
+    """
+    base_frame = Frame([0, 0, 0], [0, 0, 1], [1, 0, 0])
+    tcf_as_modelled = Frame([0, 0, 0.1], [0, 1, 0], [1, 0, 0])
+
+    tool = ToolModel(cone_along_z, tcf_as_modelled, base_frame=base_frame)
+
+    assert allclose(tool.frame.point, Point(0.1, 0, 0), tol=1e-9)
+
+    # The geometry is not baked, it carries the origin like a URDF <origin> would
+    for item in tool.root.visual + tool.root.collision:
+        assert allclose(unproxied(item.origin), Frame([0, 0, 0], [0, 1, 0], [0, 0, 1]), tol=1e-9)
+
+    # ...and the origin is honoured by the meshes handed to consumers: the cone is
+    # modelled 0.1 long along +Z, and comes back 0.1 long along +X
+    collision_mesh = tool.get_link_collision_meshes(tool.root)[0]
+    points = [Point(*collision_mesh.vertex_coordinates(v)) for v in collision_mesh.vertices()]
+    assert min(p.x for p in points) == pytest.approx(0.0, abs=1e-9)
+    assert max(p.x for p in points) == pytest.approx(0.1, abs=1e-9)
+    assert max(abs(p.y) for p in points) == pytest.approx(0.02, abs=1e-9)
+    assert max(abs(p.z) for p in points) == pytest.approx(0.02, abs=1e-9)
+
+
+def test_reframe_base_moves_a_kinematic_tool_as_a_whole(kinematic_gripper):
+    """Re-framing a tool with joints must move the whole mechanism, not just the
+    base geometry: the jaws, their joint axes, and the TCF all have to follow.
+    """
+    configuration = kinematic_gripper.zero_configuration()
+    configuration.joint_values = [0.01, 0.01]
+
+    base_frame = Frame([0, 0, 0], [0, 0, 1], [1, 0, 0])
+    before = list(kinematic_gripper.transformed_frames(configuration))
+    tcf_before = kinematic_gripper.frame
+
+    kinematic_gripper.reframe_base(base_frame)
+
+    after = list(kinematic_gripper.transformed_frames(configuration))
+    assert len(after) == len(before)
+    # Every link ends up where it was, read in the new base frame
+    for frame_before, frame_after in zip(before, after):
+        assert allclose(frame_after, base_frame.to_local_coordinates(frame_before), tol=1e-9)
+    assert allclose(kinematic_gripper.frame, base_frame.to_local_coordinates(tcf_before), tol=1e-9)
+
+    # The comparison above is what catches a joint left behind, or an axis rotated a
+    # second time by re-initializing the tree. This adds that the jaws still articulate.
+    closed = kinematic_gripper.zero_configuration()
+    closed.joint_values = [0.0, 0.0]
+    travel = [Point(*opened.point).distance_to_point(Point(*shut.point)) for opened, shut in zip(after, kinematic_gripper.transformed_frames(closed))]
+    assert max(travel) == pytest.approx(0.01, abs=1e-9)
+
+
+def test_base_frame_survives_serialization(cone_along_z):
+    base_frame = Frame([0.1, 0, 0], [0, 0, 1], [1, 0, 0])
+    tool = ToolModel(cone_along_z, Frame([0, 0, 0.14], [0, 1, 0], [1, 0, 0]), base_frame=base_frame)
+
+    other = ToolModel.__from_data__(tool.__data__)
+
+    assert other.frame == tool.frame
+    for item, other_item in zip(tool.root.visual, other.root.visual):
+        assert allclose(unproxied(item.origin), unproxied(other_item.origin), tol=1e-9)
+
+
 def test_from_json(cone_tool_json):
     tool = ToolModel.from_json(cone_tool_json)
     assert [link.name for link in tool.iter_links()] == ["attached_tool_link"]