Allow to set a robot's base (RCF)

CHANGELOG.md

@@ -10,6 +10,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 ### Added
 
 * Added `RobotModel.remove_link`, `RobotModel.remove_joint`, `RobotModel.to_urdf_string`, and `RobotModel.ensure_geometry`.
+* Added the ability to set the robot coordinate frame (RCF) via the `RobotModel.rcf` property.
 
 ### Changed
 

src/compas/robots/model/joint.py

@@ -509,7 +509,12 @@ def transformed(self, transformation):
         """
         xyz = transform_vectors(
             [[self.x, self.y, self.z]], transformation.matrix)
-        return Vector(xyz[0][0], xyz[0][1], xyz[0][2])
+
+        axis = Axis()
+        axis.x = xyz[0][0]
+        axis.y = xyz[0][1]
+        axis.z = xyz[0][2]
+        return axis
 
     @property
     def vector(self):
@@ -608,10 +613,28 @@ def __init__(self, name, type, parent, child, origin=None, axis=None, calibratio
         self.attr = kwargs
         self.child_link = None
         self.position = 0
-        # The following are world-relative frames representing the origin and the axis, which change with
-        # the joint state, while `origin` and `axis` above are parent-relative and static.
-        self.current_origin = self.origin.copy()
-        self.current_axis = self.axis.copy()
+
+    @property
+    def origin(self):
+        return self._origin
+
+    @property
+    def axis(self):
+        return self._axis
+
+    @origin.setter
+    def origin(self, value):
+        self._origin = value
+        # The following is a world-relative frame representing the origin, which change with
+        # the joint state, while `origin` is parent-relative and static.
+        self.current_origin = value.copy()
+
+    @axis.setter
+    def axis(self, value):
+        self._axis = value
+        # The following is a world-relative axis which change with
+        # the joint state, while `axis` is parent-relative and static.
+        self.current_axis = value.copy()
 
     def get_urdf_element(self):
         attributes = {
@@ -699,7 +722,7 @@ def transform(self, transformation):
         None
         """
         self.current_origin.transform(transformation)
-        self.current_axis.transform(transformation)
+        self.current_axis = self.axis.transformed(self.current_transformation)
 
     def _create(self, transformation):
         """Internal method to initialize the transformation tree.
@@ -714,7 +737,7 @@ def _create(self, transformation):
         None
         """
         self.current_origin = self.origin.transformed(transformation)
-        self.current_axis.transform(self.current_transformation)
+        self.current_axis = self.axis.transformed(self.current_transformation)
 
     def calculate_revolute_transformation(self, position):
         """Returns a transformation of a revolute joint.

src/compas/robots/model/robot.py

@@ -42,19 +42,30 @@ class RobotModel(Base):
     In line with URDF limitations, only tree structures can be represented by
     this model, ruling out all parallel robots.
 
+    Parameters
+    ----------
+    name : str
+        Unique name of the robot.
+    joints : list of :class:`compas.robots.Joint`
+        List of joint elements.
+    links : list of :class:`compas.robots.Link`
+        List of links of the robot.
+    materials : list of :class:`compas.robots.Material`
+        List of global materials.
+
     Attributes
     ----------
-    name:
+    name : str
         Unique name of the robot.
-    joints:
+    joints : list of :class:`compas.robots.Joint`
         List of joint elements.
-    links:
+    links : list of :class:`compas.robots.Link`
         List of links of the robot.
-    materials:
+    materials : list of :class:`compas.robots.Material`
         List of global materials.
-    root:
+    root : :class:`compas.robots.Link`
         Root link of the model.
-    attr:
+    attr : dict
         Non-standard attributes.
     """
 
@@ -66,8 +77,9 @@ def __init__(self, name, joints=[], links=[], materials=[], **kwargs):
         self.materials = list(materials)
         self.attr = kwargs
         self.root = None
+        self._rcf = None
         self._rebuild_tree()
-        self._create(self.root, Transformation())
+        self._create(self.root, self._root_transformation)
         self._scale_factor = 1.
 
     def get_urdf_element(self):
@@ -541,6 +553,9 @@ def get_end_effector_link_name(self):
     def get_base_link_name(self):
         """Returns the name of the base link.
 
+        The base link is defined as the parent link of the first
+        configurable joint of the kinematic chain.
+
         Returns
         -------
         str
@@ -550,8 +565,78 @@ def get_base_link_name(self):
         >>> robot.get_base_link_name()
         'world'
         """
+        try:
+            for joint in self.iter_joint_chain():
+                if joint.is_configurable():
+                    return joint.parent.link
+        except:  # noqa: E722
+            pass
+
+        # If we cannot get the joint chain, assume ordered joints
+        # as a more or less decent default
         joints = self.get_configurable_joints()
-        return joints[0].parent.link
+        if joints and joints[0].parent:
+            return joints[0].parent.link
+
+        return None
+
+    @property
+    def rcf(self):
+        """Robot Coordinate Frame.
+
+        The RCF can be set either as a fixed joint in the model, or as an explicit frame
+        used to reposition the model. If both are used, the former takes precedence over the latter.
+
+        Returns
+        -------
+        :class:`compas.geometry.Frame`
+            Robot coordinate frame, usually placed at the base of the robot and attached to a ``world`` link.
+        """
+        return Frame.from_transformation(self._root_transformation)
+
+    @rcf.setter
+    def rcf(self, frame):
+        # RCF can be set in two ways:"
+        # 1) as a fixed frame in front of the configurable joint chain (this is the URDF way)
+        # 2) or independently of the model of joints, as a reference frame
+        # The 1) takes precedence over the 2) if both are used.
+        if not self.root or not self._joints:
+            self._rcf = frame.copy()
+            return
+
+        base_link = self.get_link_by_name(self.get_base_link_name())
+
+        # if a fixed joint exists, set its origin based on the specified frame
+        if base_link and base_link.parent_joint and base_link.parent_joint.type == Joint.FIXED:
+            base_link.parent_joint.origin = Origin(frame.point, frame.xaxis, frame.yaxis)
+        else:
+            # else, store the frame as a memory reference only
+            self._rcf = frame.copy()
+
+        self._create(self.root, self._root_transformation)
+
+    @property
+    def _root_transformation(self):
+        """Internal: root transformation used as base of the kinematic chain.
+
+        Returns
+        -------
+        :class:`compas.geometry.Transformation`
+            A transformation describing the root of the chain.
+        """
+        # Uninitialized robot, default to identity matrix
+        if not self.root or not self._joints:
+            return Transformation()
+
+        base_link = self.get_link_by_name(self.get_base_link_name())
+
+        # if there's no fixed joint and there is an RCF is explicitly set, return it as a transform
+        ref_frame = Frame.worldXY()
+
+        if (not base_link or not base_link.parent_joint or base_link.parent_joint.type != Joint.FIXED) and self._rcf:
+            ref_frame = self._rcf
+
+        return Transformation.from_frame(ref_frame)
 
     def load_geometry(self, *resource_loaders, **kwargs):
         """Load external geometry resources, such as meshes.
@@ -906,10 +991,12 @@ def add_link(self, name, visual_meshes=None, visual_color=None, collision_meshes
 
         link = Link(name, visual=visuals, collision=collisions, **kwargs)
         self.links.append(link)
+
         # Must build the tree structure, if adding the first link to an empty robot
         if len(self.links) == 1:
             self._rebuild_tree()
-            self._create(self.root, Transformation())
+            self._create(self.root, self._root_transformation)
+
         return link
 
     def remove_link(self, name):
@@ -999,7 +1086,7 @@ def add_joint(self, name, type, parent_link, child_link, origin=None, axis=None,
         self._joints[joint.name] = joint
         self._adjacency[joint.name] = [child_link.name]
 
-        self._create(self.root, Transformation())
+        self._create(self.root, self._root_transformation)
 
         return joint
 

src/compas_ghpython/utilities/drawing.py

@@ -367,7 +367,7 @@ def draw_mesh(vertices, faces, color=None, vertex_normals=None, texture_coordina
 
     Returns
     -------
-    list of :class:`Rhino.Geometry.Mesh`
+    :class:`Rhino.Geometry.Mesh`
 
     """
     mesh = Mesh()

tests/compas/robots/test_model.py

@@ -1,9 +1,11 @@
-import re
 import os
+import re
 
 import pytest
 
 from compas.files import URDF
+from compas.geometry import Frame
+from compas.geometry import Transformation
 from compas.robots import Box
 from compas.robots import Cylinder
 from compas.robots import Joint
@@ -134,6 +136,51 @@ def test_ur5_urdf_to_string(ur5_file):
     assert len(list(filter(lambda i: i.type == Joint.REVOLUTE, r.joints))) == 6
 
 
+def test_forward_kinematics(ur5_file):
+    r = RobotModel.from_urdf_file(ur5_file)
+    f = r.forward_kinematics(dict())
+
+    ftip = Frame((0.817, 0.191, -0.005), (-0.000, 1.000, 0.000), (1.000, 0.000, 0.000))
+    assert str(f) == str(ftip)
+
+    fbase = r.forward_kinematics(dict(), link_name='base_link')
+    assert str(fbase) == str(Frame.worldXY())
+
+
+def test_rcf_on_fixed_joints(ur5_file):
+    r = RobotModel.from_urdf_file(ur5_file)
+    fbase = r.forward_kinematics(dict(), link_name='base_link')
+    assert str(fbase) == str(Frame.worldXY())
+
+    f = r.forward_kinematics(dict())
+    ftip = Frame((0.817, 0.191, -0.005), (-0.000, 1.000, 0.000), (1.000, 0.000, 0.000))
+    assert str(f) == str(ftip)
+
+    frcf = Frame((1.72, 2.25, 0.53), (0.000, 1.000, 0.000), (1.000, -0.000, 0.000))
+    r.rcf = frcf
+    fbase = r.forward_kinematics(dict(), link_name='base_link')
+    assert str(fbase) == str(frcf)
+
+    f = r.forward_kinematics(dict())
+    fexpected = Frame.from_transformation(Transformation.from_frame(frcf) * Transformation.from_frame(ftip))
+    assert str(f.point) == str(fexpected.point)
+
+
+def test_rcf_without_fixed_joint():
+    model = RobotModel("robot")
+    link0 = model.add_link("link0")
+    link1 = model.add_link("link1")
+    link2 = model.add_link("link2")
+    tip = model.add_link("tip")
+
+    model.add_joint("joint1", Joint.CONTINUOUS, link0, link1, origin=Frame((20, 0, 0), (1, 0, 0), (0, 1, 0)))
+    model.add_joint("joint2", Joint.CONTINUOUS, link1, link2, origin=Frame((20, 0, 0), (1, 0, 0), (0, 1, 0)))
+    model.add_joint("joint3", Joint.CONTINUOUS, link2, tip, origin=Frame((11, 0, 0), (1, 0, 0), (0, 1, 0)))
+    assert str(model.forward_kinematics(dict(), 'tip').point) == 'Point(51.000, 0.000, 0.000)'
+
+    model.rcf = Frame((10, 0, 0), (1, 0, 0), (0, 1, 0))
+    assert str(model.forward_kinematics(dict(), 'tip').point) == 'Point(61.000, 0.000, 0.000)'
+
 def test_ur5_urdf_data(ur5_file):
     r_original = RobotModel.from_urdf_file(ur5_file)
     r = RobotModel.from_data(r_original.data)
@@ -620,8 +667,9 @@ def test_ensure_geometry(urdf_file, urdf_file_with_shapes_only):
     import os
     from zipfile import ZipFile
     try:
-        from StringIO import StringIO as ReaderIO
         from urllib import urlopen
+
+        from StringIO import StringIO as ReaderIO
     except ImportError:
         from io import BytesIO as ReaderIO
         from urllib.request import urlopen