diff --git a/CHANGELOG.md b/CHANGELOG.md index 31632109..b6c027c5 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/conftest.py b/conftest.py index 652166f6..ca5c2976 100644 --- a/conftest.py +++ b/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 diff --git a/src/compas_robots/model/joint.py b/src/compas_robots/model/joint.py index 7bb844db..01651001 100644 --- a/src/compas_robots/model/joint.py +++ b/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: diff --git a/src/compas_robots/model/tool.py b/src/compas_robots/model/tool.py index 27164730..329918ee 100644 --- a/src/compas_robots/model/tool.py +++ b/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. diff --git a/tests/test_tool.py b/tests/test_tool.py index b042811b..ffc8d661 100644 --- a/tests/test_tool.py +++ b/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 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"]