docs/source/background-math.rst

@@ -11,4 +11,5 @@ Background Math and Geometry
    background-math/studys-kinematics
    background-math/joint-angle-to-t
    background-math/comb-optim
-   background-math/motion-interp-4p
\ No newline at end of file
+   background-math/motion-interp-4p
+   background-math/affine-metric
\ No newline at end of file

docs/source/background-math/affine-metric.rst

+.. _affine-metric:
+
+Affine Metric and Motion Covering
+=================================
+
+For faster collision detection, the motion curve of a mechanism can be interpolated
+by Bezier curves. The Bezier curves are defined by control points, which themselves
+are 8-dimensional vectors in :math:`SE(3)`. As known, there exists no meaningful
+metric in Special Eucledian Group :math:`SE(3)`,
+which is the group of rigid body motions in 3D space. In another words, the question,
+"What is the distance between two poses?", is difficult/impossible to answer
+since translation and rotation are not comparable.
+
+Affine Metric Definition
+------------------------
+
+Hofer introduced in his dissertation :footcite:p:`Hofer2004diss` and
+paper with :footcite:t:`Hofer2004` an affine metric that maps elements
+of :math:`SE(3)` to Affine Space :math:`\mathbb{R}^{12}`, where a poses is represented
+as a 12-dimensional vector. The mapping can be obtained from transformation matrix
+
+.. math::
+
+    \mathbf{T} = \begin{bmatrix} 1 & 0 \\ \mathbf{t} & \mathbf{R} \end{bmatrix} =
+    \begin{bmatrix} 1 & 0 & 0 & 0 \\ \mathbf{t} & \mathbf{n} & \mathbf{o} & \mathbf{a}
+    \end{bmatrix}
+
+where :math:`R` is a 3x3 rotation matrix with orthogonal vectors called normal,
+orthogonal, and approach, and :math:`t` is a 3x1 translation vector. Keep in mind
+that this is a european standard of writing transformation matrices, where the
+translation vector is written in the first column.
+
+A point :math:`\mathbf{a}` in :math:`\mathbb{R}^{12}` is obtained as:
+
+.. math::
+
+    \mathbf{a(\mathbf{T})} = \begin{bmatrix} \mathbf{t} \\ \mathbf{n} \\
+    \mathbf{o} \\ \mathbf{a} \end{bmatrix}
+
+If the transformation matrix is not normalized, the affine metric may be seen as
+projective space :math:`\mathbb{PR}^{12}` with 13 dimensions, where the
+first element is the homogeneous coordinate.
+
+To map between poses, the package uses classes :class:`.TransfMatrix`,
+:class:`.DualQuaternion`, and :class:`.PointHomogeneous` (which can take n-dimensional
+vectors as input).
+
+The affine metric is used in the package to calculate the distance between poses.
+But first, the metric itself has to be defined. To be locally appropriate,
+the connection points between joints and links of a mechanism are used to define
+it via :class:`.AffineMetric` object.
+
+
+
+
+
+Motion Interpolation and Bezier Splitting
+-----------------------------------------
+
+The rational motion curve can be reparametrized by Bezier curves using Bernstein
+polynomials.
+
+
+Ball Covering of Bezier Curves
+------------------------------
+
+When the metric is defined, the distance between two poses (Bezier curve control points)
+can be calculated. This is applied in covering the orbits of mechanism points.
+
+
+:footcite:t:`Schroecker2005`
+:footcite:t:`Schroecker2014`
+
+.. footbibliography::

docs/source/refs.bib

@@ -126,6 +126,56 @@
   pages = {261–275}
 }
 
+@book{Hofer2004diss,
+  title={Variational Motion Design in the Presence of Obstacles},
+  author={Hofer, Michael},
+  year={2004},
+  url = {https://hdl.handle.net/20.500.12708/9613},
+  publisher={TU Wien}
+}
+
+@inbook{Hofer2004,
+  title = {Variational Motion Design},
+  ISBN = {9781402022494},
+  url = {http://dx.doi.org/10.1007/978-1-4020-2249-4_39},
+  DOI = {10.1007/978-1-4020-2249-4_39},
+  booktitle = {On Advances in Robot Kinematics},
+  publisher = {Springer Netherlands},
+  author = {Pottmann,  Helmut and Hofer,  Michael and Ravani,  Bahram},
+  year = {2004},
+  pages = {361–370}
+}
+
+@article{Schroecker2005,
+  title = {Curvatures and Tolerances in the Euclidean Motion Group},
+  volume = {47},
+  ISSN = {1420-9012},
+  url = {http://dx.doi.org/10.1007/BF03323019},
+  DOI = {10.1007/bf03323019},
+  number = {1–2},
+  journal = {Results in Mathematics},
+  publisher = {Springer Science and Business Media LLC},
+  author = {Schr\"{o}cker,  Hans-Peter and Wallner,  Johannes},
+  year = {2005},
+  month = mar,
+  pages = {132–146}
+}
+
+@article{Schroecker2014,
+  title = {Guaranteed Collision Detection with Toleranced Motions},
+  volume = {31},
+  ISSN = {0167-8396},
+  url = {http://dx.doi.org/10.1016/j.cagd.2014.08.001},
+  DOI = {10.1016/j.cagd.2014.08.001},
+  number = {7–8},
+  journal = {Computer Aided Geometric Design},
+  publisher = {Elsevier BV},
+  author = {Schr\"{o}cker,  Hans-Peter and Weber,  Matthias J.},
+  year = {2014},
+  month = oct,
+  pages = {602–612}
+}
+
 @book{study1901geometrie,
   title={Geometrie der Dynamen},
   author={Study, E.},
@@ -134,5 +184,3 @@
   publisher={B. G. Teubner},
   pages={603}
 }
-
-

python/examples/interp_2poses.py

+# Quadratic interpolation of 2 poses with an optimized 3rd pose
+
+from rational_linkages import (DualQuaternion, Plotter, MotionInterpolation,
+                               TransfMatrix, RationalMechanism)
+
+
+p2 = DualQuaternion()
+p0 = TransfMatrix.from_rpy_xyz([0.5, 0, 0], [1, 0, 0])
+p1 = TransfMatrix.from_rpy_xyz([0, 0, 0.5], [0, 1, 0])
+
+p0 = TransfMatrix.from_rpy_xyz([180, 0, 0], [0.1, 0.2, 0.1], unit='deg')
+p1 = TransfMatrix.from_rpy_xyz([-90, 0, 90], [0.15, -0.2, 0.2], unit='deg')
+
+
+
+p0 = TransfMatrix.from_rpy_xyz([0, 0, 0], [0.1, 0.2, 0.1], unit='deg')
+p1 = TransfMatrix.from_rpy_xyz([0, 0, 90], [0.15, -0.2, 0.2], unit='deg')
+
+p0 = DualQuaternion(p0.matrix2dq())
+p1 = DualQuaternion(p1.matrix2dq())
+
+interpolated_curve = MotionInterpolation.interpolate([p0, p1])
+m = RationalMechanism(interpolated_curve.factorize())
+
+p = Plotter(interactive=True, steps=500, arrows_length=0.05)
+p.plot(p0)
+p.plot(p1)
+
+p.plot(interpolated_curve, interval='closed', label='interpolated curve')
+p.plot(m)
+
+p.show()

python/examples/limancon2bezier_split.py

@@ -24,7 +24,7 @@ p = Plotter(interactive=False, arrows_length=0.5, joint_range_lim=2, steps=200)
 bezier_segments = [b_left, b_right, b_left_inv, b_right_inv]
 
 for bezier_curve in bezier_segments:
-    p.plot(bezier_curve, interval=(-1, 1), plot_control_points=True)
+    p.plot(bezier_curve.curve, interval=(-1, 1), plot_control_points=True)
     p.plot(bezier_curve.ball)
 
 p.show()

python/examples/metric.py

@@ -9,70 +9,43 @@ from rational_linkages import (
     AffineMetric,
     PointHomogeneous,
     DualQuaternion,
+    CollisionAnalyser,
 )
-from rational_linkages.models import bennett_ark24, collisions_free_6r, plane_fold_6r
+from rational_linkages.models import bennett_ark24, collisions_free_6r, plane_fold_6r, interp_4poses_6r
+
+from time import time
 
 if __name__ == '__main__':
-    m = RationalMechanism.from_saved_file("johannes-interp.pkl")
+    m = interp_4poses_6r()
     #m = collisions_free_6r()
-    # m = plane_fold_6r()
-    #m = bennett_ark24()
+    #m = plane_fold_6r()
+    m = bennett_ark24()
     m.update_segments()
 
-    #m.collision_check(parallel=True, only_links=True)
-
-    c = m.curve()
-    t = sp.symbols('t')
+    m._relative_motions = None
+    m._metric = None
 
-    mechanism_points = m.points_at_parameter(0, inverted_part=True, only_links=False)
-    metric = AffineMetric(c, mechanism_points)
+    start_time = time()
+    ca = CollisionAnalyser(m)
+    print(f'{time() - start_time:.3f} sec for generating Bezier segments')
+    start_time = time()
+    # orbits = ca.get_points_orbits()
 
-    p = Plotter(interactive=True, arrows_length=0.1, joint_range_lim=2, steps=200)
+    s = 't_12'
+    o0, o1 = ca.get_segment_orbit(s)
+    print(f'{time() - start_time:.3f} sec for generating orbits')
 
-    # bezier_segments = c.split_in_beziers(metric=metric, min_splits=20)
-    lower_c = m.factorizations[0].get_symbolic_factors()
 
-    mflc = RationalCurve([sp.Poly(pol, t) for pol in (lower_c[0] * lower_c[1])])
-    bezier_segments = mflc.split_in_beziers(metric=metric, min_splits=30)
 
+    p = Plotter(interactive=True, arrows_length=0.1, joint_range_lim=2, steps=300)
     p.plot(m)
 
-    s = 4
-    p0 = 3
-    p1 = 4
-    p.plot(m.segments[s])
-    p.plot(mechanism_points[p0])
-    p.plot(mechanism_points[p1])
-
-    p.plot(bezier_segments[0], plot_control_points=True)
-    p.plot(bezier_segments[1], plot_control_points=True)
-
-    for segment in bezier_segments:
-        mechanism_points[p0].get_point_orbit(
-            segment.ball.center,
-            segment.ball.radius,
-            metric)
-        p.plot(mechanism_points[p0].orbit)
+    p.plot(ca.segments[s])
 
-    for segment in bezier_segments:
-        mechanism_points[p1].get_point_orbit(
-            segment.ball.center,
-            segment.ball.radius,
-            metric)
-        p.plot(mechanism_points[p1].orbit)
+    for orbit in o0:
+        p.plot(orbit)
+    for orbit in o1:
+        p.plot(orbit)
 
-    # segment = bezier_segments[0]
-    # mechanism_points[2].get_point_orbit(
-    #     segment.ball.center,
-    #     segment.ball.radius,
-    #     metric)
-    # p.plot(mechanism_points[2].orbit)
-    #
-    # mechanism_points[3].get_point_orbit(
-    #     segment.ball.center,
-    #     segment.ball.radius,
-    #     metric)
-    # p.plot(mechanism_points[3].orbit)
-    # p.plot(segment, plot_control_points=True)
     p.show()
 

python/rational_linkages/CollisionAnalyser.py

 from .RationalMechanism import RationalMechanism
 from .RationalCurve import RationalCurve
-from .RationalBezier import RationalBezier
 from .MiniBall import MiniBall
+from .DualQuaternion import DualQuaternion
+from .PointHomogeneous import PointOrbit
 
 import numpy
+import sympy
 
 
 class CollisionAnalyser:
-    def __init__(self):
-        pass
+    def __init__(self, mechanism: RationalMechanism):
+        self.mechanism = mechanism
+        self.mechanism_points = mechanism.points_at_parameter(0,
+                                                              inverted_part=True,
+                                                              only_links=False)
+        self.metric = mechanism.metric
+
+        self.segments = {}
+        for segment in mechanism.segments:
+            self.segments[segment.id] = segment
+
+        self.motions = self.get_motions()
+        self.bezier_splits = self.get_bezier_splits(100)
+
+    def get_bezier_splits(self, min_splits: int = 0) -> list:
+        """
+        Split the relative motions of the mechanism into bezier curves.
+        """
+        return [motion.split_in_beziers(min_splits) for motion in self.motions]
+
+    def get_motions(self):
+        """
+        Get the relative motions of the mechanism represented as rational curves.
+        """
+        sequence = DualQuaternion()
+        branch0 = [sequence := sequence * factor for factor in
+                   self.mechanism.factorizations[0].factors_with_parameter]
+
+        sequence = DualQuaternion()
+        branch1 = [sequence := sequence * factor for factor in
+                   self.mechanism.factorizations[1].factors_with_parameter]
+
+        relative_motions = branch0 + branch1[::-1]
+
+        t = sympy.symbols('t')
+
+        motions = []
+        for motion in relative_motions:
+            motions.append(RationalCurve([sympy.Poly(c, t) for c in motion],
+                                         metric=self.metric))
+        return motions
+
+    def get_points_orbits(self):
+        """
+        Get the orbits of the mechanism points.
+        """
+        from time import time
+        start_time = time()
+        points = [p.coordinates_normalized for p in self.mechanism_points]
+
+        return [PointOrbit(*point.get_point_orbit(metric=self.metric))
+                for point in self.mechanism_points]
+
+    def get_segment_orbit(self, segment_id: str):
+        """
+        Get the orbit of a segment.
+        """
+        segment = self.segments[segment_id]
+
+        if segment.type == 'l' or segment.type == 't' or segment.type == 'b':
+            if segment.factorization_idx == 0:
+                split_idx = segment.idx - 1
+                p0_idx = 2 * segment.idx - 1
+                p1_idx = 2 * segment.idx
+            else:
+                split_idx = -1 * segment.idx
+                p0_idx = -2 * segment.idx - 1
+                p1_idx = -2 * segment.idx
+        else:  # type == 'j'
+            if segment.factorization_idx == 0:
+                split_idx = segment.idx - 1
+                p0_idx = 2 * segment.idx
+                p1_idx = 2 * segment.idx + 1
+            else:
+                split_idx = -1 * segment.idx
+                p0_idx = -2 * segment.idx - 1
+                p1_idx = -2 * segment.idx - 2
+
+        p0 = self.mechanism_points[p0_idx]
+        p1 = self.mechanism_points[p1_idx]
+        rel_bezier_splits = self.bezier_splits[split_idx]
+
+        orbits0 = [PointOrbit(*p0.get_point_orbit(acting_center=split.ball.center,
+                                                  acting_radius=split.ball.radius_squared,
+                                                  metric=self.metric))
+                   for split in rel_bezier_splits]
+        orbits1 = [PointOrbit(*p1.get_point_orbit(acting_center=split.ball.center,
+                                                  acting_radius=split.ball.radius_squared,
+                                                  metric=self.metric))
+                   for split in rel_bezier_splits]
+
+        return orbits0, orbits1
 
     def check_two_objects(self, obj0, obj1):
         """

python/rational_linkages/Linkage.py

@@ -193,9 +193,10 @@ class LineSegment:
         self.type = linkage_type
         self.factorization_idx = f_idx
         self.idx = idx
+        self.id = f"{self.type}_{self.factorization_idx}{self.idx}"
 
     def __repr__(self):
-        return f"{self.type}_{self.factorization_idx}{self.idx}"
+        return self.id
 
     def is_point_in_segment(self, point: PointHomogeneous, t_val: float) -> bool:
         """

python/rational_linkages/MiniBall.py

@@ -2,52 +2,67 @@ import numpy as np
 from scipy.optimize import minimize
 
 from .PointHomogeneous import PointHomogeneous
+from .MiniBall2 import get_bounding_ball
 
 
 class MiniBall:
     def __init__(self,
                  points: list[PointHomogeneous],
-                 metric: "AffineMetric" = None):
+                 metric: "AffineMetric" = None,
+                 method: str = 'welzl'):
         """
         Initialize the MiniBall class
 
         :param list[PointHomogeneous] points: array of points in the space
         :param AffineMetric metric: alternative metric to be used for the ball
+        :param str method: method to be used for finding the smallest ball
         """
         self.points = points
 
         self.number_of_points = len(self.points)
         self.dimension = self.points[0].coordinates.size
 
-        self.metric = metric
-
-        if metric is None:
-            self.metric = 'euclidean'
-            self.metric_obj = None
+        if metric is None or metric == 'euclidean':
+            self.metric_type = 'euclidean'
         else:
-            metric = 'hofer'
-            self.metric_obj = metric
+            from .AffineMetric import AffineMetric
+            if isinstance(metric, AffineMetric):
+                self.metric_type = 'hofer'
+            else:
+                ValueError("Invalid metric type.")
+
+        self.metric = metric
 
         self.center = np.zeros(self.dimension)
-        self.radius = 10.0
+        self.radius_squared = 10.0
 
-        self.optimization_results = self.get_ball()
-        self.center = PointHomogeneous(self.optimization_results.x[:-1])
-        self.radius = self.optimization_results.x[-1]
+        self.center, self.radius_squared = self.get_ball(method='welzl')
 
-    def get_ball(self):
+    def get_ball(self, method: str = 'minimize'):
         """
         Find the smallest ball containing all given points in Euclidean metric
         """
+        if method == 'minimize':
+            result = self.get_ball_minimize()
+            center = result.x[:-1]
+            radius_squared = np.square(result.x[-1])
+        elif method == 'welzl':
+            points = np.array([point.coordinates_normalized for point in self.points])
+            center, radius_squared = get_bounding_ball(points, metric=self.metric)
+        else:
+            raise ValueError("Invalid method.")
+
+        return PointHomogeneous(center), radius_squared
 
+    def get_ball_minimize(self):
         def objective_function(x):
             """
             Objective function to minimize the squared radius r^2 of the ball
             """
-            return x[-1] ** 2
+            return np.square(x[-1])
 
         # Prepare constraint equations based on the metric
-        if self.metric == "hofer":
+        if self.metric_type == "hofer":
             def constraint_equations(x):
                 """
                 For Hofer metric, constraint equations must satisfy the ball by:
@@ -57,11 +72,9 @@ class MiniBall:
                 constraints = np.zeros(self.number_of_points)
 
                 for i in range(self.number_of_points):
-                    squared_distance = sum(self.metric_obj.squared_distance_pr12_points(
-                        self.points[i].normalize(), x[j])
-                        for j in range(self.dimension)
-                    )
-                    constraints[i] = x[-1] ** 2 - squared_distance
+                    squared_distance = self.metric.squared_distance_pr12_points(
+                        self.points[i].normalize(), x[:-1])
+                    constraints[i] = np.square(x[-1]) - squared_distance
                 return constraints
         else:
             def constraint_equations(x):
@@ -76,10 +89,10 @@ class MiniBall:
                     # in case of Euclidean metric, the normalized point has to be taken
                     # in account
                     squared_distance = sum(
-                        (self.points[i].normalize()[j] - x[j]) ** 2
+                        np.square((self.points[i].normalize()[j] - x[j]))
                         for j in range(self.dimension)
                     )
-                    constraints[i] = x[-1] ** 2 - squared_distance
+                    constraints[i] = np.square(x[-1]) - squared_distance
                 return constraints
 
         # Prepare inequality constraint dictionary
@@ -88,7 +101,7 @@ class MiniBall:
         # Initialize optimization variables
         initial_guess = np.zeros(self.dimension + 1)
         initial_guess[0] = 1.0
-        initial_guess[-1] = self.radius
+        initial_guess[-1] = self.radius_squared
 
         # Perform optimization
         result = minimize(objective_function, initial_guess, constraints=ineq_con)

python/rational_linkages/MiniBall2.py

+# This code originates from Alexandre Devert and his miniball package
+# repo: https://github.com/marmakoide/miniball
+# pypi: https://pypi.org/project/miniball/
+# license: MIT
+#
+# The code has been modified to handle other than Euclidean metrics.
+
+import numpy as np
+from .AffineMetric import AffineMetric
+
+
+def get_circumsphere(points: np.ndarray, metric: AffineMetric = None):
+    """
+    Computes the circumsphere of a set of points
+
+    :param np.ndarray points: array of points in the space
+    :param AffineMetric metric: alternative metric to be used for the ball
+
+    :return: center and the squared radius of the circumsphere
+    :rtype: (nd.array, float)
+    """
+    # calculate vectors from the first point to all other points (redefine origin)
+    u = points[1:] - points[0]
+
+    # calculate squared distances from the first point to all other points
+    b = np.sqrt(np.sum(np.square(u), axis=1))
+
+    # normalize the vectors and halve the lengths
+    u /= b[:, None]
+    b /= 2
+
+    # solve the linear system to find the center of the circumsphere
+    center = np.dot(np.linalg.solve(np.inner(u, u), b), u)
+
+    # length of "center" vector is the radius of the circumsphere
+    if metric is None or metric == 'euclidean':
+        radius_squared = np.square(center).sum()
+    else:
+        radius_squared = metric.squared_distance_pr12_points(
+            np.array([1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]),
+            center)
+
+    # move the center back to the original coordinate system
+    center += points[0]
+
+    return center, radius_squared
+
+
+def get_bounding_ball(points: np.ndarray,
+                      metric: 'AffineMetric' = None,
+                      epsilon: float = 1e-7,
+                      rng=np.random.default_rng()):
+    """
+    Computes the smallest bounding ball of a set of points
+
+    :param nd.array points: array of points in the space
+    :param AffineMetric metric: alternative metric to be used for the ball
+    :param float epsilon: tolerance used when testing if a set of point belongs to
+        the same sphere, default is 1e-7
+    :param numpy.random.Generator rng: pseudo-random number generator used internally,
+        default is the default one provided by numpy
+
+    :return: center and the squared radius of the circumsphere
+    :rtype: (nd.array, float)
+    """
+    def circle_contains(ball, point):
+        center, radius_squared = ball
+
+        if metric is None or metric == 'euclidean':
+            return np.sum(np.square(point - center)) <= radius_squared
+        else:
+            return metric.squared_distance_pr12_points(point, center) <= radius_squared
+
+    def get_boundary(subset):
+        if len(subset) == 0:
+            return np.zeros(points.shape[1]), 0.0
+        if len(subset) <= points.shape[1] + 1:
+            return get_circumsphere(points[subset], metric=metric)
+        center, radius_squared = get_circumsphere(points[subset[: points.shape[1] + 1]], metric=metric)
+        if np.all(np.abs(np.sum(np.square(points[subset] - center), axis=1) - radius_squared) < epsilon):
+            return center, radius_squared
+
+    class Node:
+        def __init__(self, subset, remaining):
+            self.subset = subset
+            self.remaining = remaining
+            self.ball = None
+            self.pivot = None
+            self.left = None
+            self.right = None
+
+    def traverse(node):
+        stack = [node]
+        while stack:
+            node = stack.pop()
+            if not node.remaining or len(node.subset) >= points.shape[1] + 1:
+                node.ball = get_boundary(node.subset)
+            elif node.left is None:
+                pivot_index = rng.integers(len(node.remaining))
+                node.pivot = node.remaining[pivot_index]
+                new_remaining = node.remaining[:pivot_index] + node.remaining[pivot_index + 1:]
+                node.left = Node(node.subset, new_remaining)
+                stack.extend([node, node.left])
+            elif node.right is None:
+                if circle_contains(node.left.ball, points[node.pivot]):
+                    node.ball = node.left.ball
+                else:
+                    node.right = Node(node.subset + [node.pivot], node.left.remaining)
+                    stack.extend([node, node.right])
+            else:
+                node.ball = node.right.ball
+                node.left = node.right = None
+
+    points = points.astype(float, copy=False)
+    root = Node([], list(range(points.shape[0])))
+    traverse(root)
+    return root.ball

python/rational_linkages/PointHomogeneous.py

@@ -67,8 +67,6 @@ class PointHomogeneous:
         #if len(self.coordinates_normalized) == 4:  # point in PR3
         #    self.as_dq_array = self.point2dq_array()
 
-        self.orbit = None
-
     @classmethod
     def at_origin_in_2d(cls):
         """
@@ -289,10 +287,11 @@ class PointHomogeneous:
         """
         Get point orbit
 
-
+        Equation from Schroecker and Webber, Guaranteed collision detection with
+        toleranced motions, 2014, eq. 4.
 
         :param PointHomogeneous acting_center: center of the acting ball
-        :param float acting_radius: radius of the orbit ball
+        :param float acting_radius: squared radius of the orbit ball
         :param AffineMetric metric: metric of the curve
 
         :return: point center and radius squared
@@ -301,23 +300,17 @@ class PointHomogeneous:
         point_center = acting_center.point2matrix() @ self.coordinates_normalized
 
         coords_3d = self.normalized_in_3d()
-        radius_squared = acting_radius ** 2 * (1/metric.total_mass + np.sum([(coord ** 2 / metric.inertia_eigen_vals[i]) for i, coord in enumerate(coords_3d)]))
 
+        radius_squared = acting_radius * (1/metric.total_mass + np.sum([(coord ** 2 / metric.inertia_eigen_vals[i]) for i, coord in enumerate(coords_3d)]))
         radius = np.sqrt(radius_squared)
-        self.set_point_orbit(point_center, radius)
-        return point_center, radius
 
-    def set_point_orbit(self, orbit_center: "PointHomogeneous", orbit_radius: float):
-        """
-        Set the orbit of the point
-        """
-        self.orbit = PointOrbit(orbit_center, orbit_radius)
+        return point_center, radius
 
 
 class PointOrbit:
     def __init__(self, point_center, radius):
         """
-
+        Orbit of a point (its covering ball)
         """
         if not isinstance(point_center, PointHomogeneous):
             self.center = PointHomogeneous(point_center)

python/rational_linkages/RationalBezier.py

@@ -50,7 +50,7 @@ class RationalBezier(RationalCurve):
         Get the smallest ball enclosing the control points of the curve
         """
         if self._ball is None:
-            self._ball = MiniBall(self.control_points)
+            self._ball = MiniBall(self.control_points, metric=self.metric)
         return self._ball
 
     def get_coeffs_from_control_points(self,
@@ -127,9 +127,9 @@ class RationalBezier(RationalCurve):
         return any(point.coordinates[0] < 0 for point in self.control_points)
 
 
-class BezierSegment(RationalBezier):
+class BezierSegment:
     """
-    Bezier curves that reparameterize a motion curve in split segments.
+    Bezier curves that reparameterizes a motion curve in split segments.
     """
     def __init__(self,
                  control_points: list[PointHomogeneous],
@@ -144,12 +144,21 @@ class BezierSegment(RationalBezier):
             list of two floats representing the original parameter interval of the
             motion curve
         """
-        super().__init__(control_points)
+        self.control_points = control_points
+        self.t_param_of_motion_curve = t_param
+        self._metric = metric
 
-        self.metric = metric
         self._ball = None
+        self._curve = None
 
-        self.t_param_of_motion_curve = t_param
+    @property
+    def curve(self):
+        """
+        Get the Bezier curve
+        """
+        if self._curve is None:
+            self._curve = RationalBezier(self.control_points)
+        return self._curve
 
     @property
     def ball(self):
@@ -160,14 +169,36 @@ class BezierSegment(RationalBezier):
             self._ball = MiniBall(self.control_points, metric=self.metric)
         return self._ball
 
+    @property
+    def metric(self):
+        """
+        Define a metric in R12 for the mechanism.
+
+        This metric is used for collision detection.
+        """
+        if self._metric is None:
+            return "euclidean"
+        else:
+            return self._metric
+
+    @metric.setter
+    def metric(self, metric: "AffineMetric"):
+        from .AffineMetric import AffineMetric  # inner import
+
+        if isinstance(metric, AffineMetric):
+            self._metric = metric
+        elif metric == "euclidean" or metric is None:
+            self._metric = None
+        else:
+            raise TypeError("The 'metric' property must be of type 'AffineMetric'")
+
     def split_de_casteljau(self,
                            t: float = 0.5,
-                           metric: "AffineMetric" = None) -> tuple:
+                           ) -> tuple:
         """
         Split the curve at the given parameter value t
 
         :param float t: parameter value to split the curve at
-        :param AffineMetric metric: metric to be used for the ball
 
         :return: tuple - two new Bezier curves
         :rtype: tuple
@@ -202,5 +233,21 @@ class BezierSegment(RationalBezier):
         new_t_right = (self.t_param_of_motion_curve[0],
                        [mid_t, self.t_param_of_motion_curve[1][1]])
 
-        return (BezierSegment(left_curve, metric=metric, t_param=new_t_left),
-                BezierSegment(right_curve, metric=metric, t_param=new_t_right))
+        return (BezierSegment(left_curve, t_param=new_t_left, metric=self.metric),
+                BezierSegment(right_curve, t_param=new_t_right, metric=self.metric))
+
+    def check_for_control_points_at_infinity(self):
+        """
+        Check if there is a control point at infinity
+
+        :return: bool - True if there is a control point at infinity, False otherwise
+        """
+        return any(point.is_at_infinity for point in self.control_points)
+
+    def check_for_negative_weights(self):
+        """
+        Check if there are negative weights in the control points
+
+        :return: bool - True if there are negative weights, False otherwise
+        """
+        return any(point.coordinates[0] < 0 for point in self.control_points)

python/rational_linkages/RationalCurve.py

@@ -57,7 +57,7 @@ class RationalCurve:
         curve = RationalCurve.from_coeffs(np.array([[1., 0., 2., 0., 1.], [0.5, 0., -2., 0., 1.5], [0., -1., 0., 3., 0.], [1., 0., 2., 0., 1.]]))
     """
 
-    def __init__(self, polynomials: list[sp.Poly]):
+    def __init__(self, polynomials: list[sp.Poly], metric: "AffineMetric" = None):
         """
         Initializes a RationalCurve object with the provided coefficients.
 
@@ -82,6 +82,31 @@ class RationalCurve:
         self.is_motion = self.dimension == 7
         self.is_affine_motion = self.dimension == 12
 
+        self._metric = metric
+
+    @property
+    def metric(self):
+        """
+        Define a metric in R12 for the mechanism.
+
+        This metric is used for collision detection.
+        """
+        if self._metric is None:
+            return "euclidean"
+        else:
+            return self._metric
+
+    @metric.setter
+    def metric(self, metric: "AffineMetric"):
+        from .AffineMetric import AffineMetric  # inner import
+
+        if isinstance(metric, AffineMetric):
+            self._metric = metric
+        elif metric == "euclidean" or metric is None:
+            self._metric = None
+        else:
+            raise TypeError("The 'metric' property must be of type 'AffineMetric'")
+
     @property
     def symbolic(self):
         """
@@ -453,7 +478,6 @@ class RationalCurve:
         return RationalCurve(curve_poly)
 
     def split_in_beziers(self,
-                         metric: Union[str, "AffineMetric"] = "euclidean",
                          min_splits: int = 0) -> list["BezierSegment"]:
         """
         Split the curve into Bezier curves with positive weights of control points.
@@ -461,47 +485,49 @@ class RationalCurve:
         The curve is split into Bezier curves using the De Casteljau algorithm.
 
         :param int min_splits: minimal number of splits to be performed
-        :param Union[str, AffineMetric] metric: metric for the optimization
 
         :return: list of RationalBezier objects
-        :rtype: list[RationalBezier]
+        :rtype: list[BezierSegment]
         """
-        if self.is_motion:
-            curve = self.get_curve_in_pr12()
-        else:
-            raise ValueError("The curve is not a motion curve, cannot "
-                             "split into Bezier curves")
+        if not self.is_motion:
+            raise ValueError("Not a motion curve, cannot split into Bezier curves.")
+
+        from .RationalBezier import BezierSegment  # inner import
 
-        from .RationalBezier import BezierSegment  # method import
+        curve = self.get_curve_in_pr12()
 
         # obtain Bezier curves for the curve and its reparametrized inverse part
         bezier_curve_segments = [
             # reparametrize the curve from the intervals [-1, 1]
             BezierSegment(curve.curve2bezier_control_points(reparametrization=True),
-                          metric=metric,
-                          t_param=(False, [-1.0, 1.0])),
+                          t_param=(False, [-1.0, 1.0]),
+                          metric=self.metric),
             BezierSegment(curve.inverse_curve().curve2bezier_control_points(
                 reparametrization=True),
-                          metric=metric,
-                          t_param=(True, [-1.0, 1.0]))
+                t_param=(True, [-1.0, 1.0]),
+                metric=self.metric)
         ]
 
         # split the Bezier curves until all control points have positive weights
+        # or no weights at infinity, or the minimal number of splits is reached
         while True:
             new_segments = [
                 part for b_curve in bezier_curve_segments
                 for part in (
-                    b_curve.split_de_casteljau(metric=metric) if b_curve.check_for_control_points_at_infinity() or b_curve.check_for_negative_weights() else [b_curve])
+                    b_curve.split_de_casteljau()
+                    if b_curve.check_for_control_points_at_infinity()
+                       or b_curve.check_for_negative_weights() else [b_curve])
             ]
 
+            # if all control points have positive weights and no weights at infinity,
+            # but the minimal number of splits is not reached, continue splitting
             if not any(
-                    b_curve.check_for_control_points_at_infinity() or b_curve.check_for_negative_weights()
+                    b_curve.check_for_control_points_at_infinity()
+                    or b_curve.check_for_negative_weights()
                     for b_curve in new_segments):
                 if len(new_segments) < min_splits:
-                    new_segments = [
-                        part for b_curve in new_segments
-                        for part in b_curve.split_de_casteljau(metric=metric)
-                    ]
+                    new_segments = [part for b_curve in new_segments
+                                    for part in b_curve.split_de_casteljau()]
                 else:
                     bezier_curve_segments = new_segments
                     break

python/rational_linkages/RationalMechanism.py

@@ -18,13 +18,15 @@ class RationalMechanism(RationalCurve):
     """
     Class representing rational mechanisms in dual quaternion space.
 
-    :ivar factorizations: list of MotionFactorization objects
-    :ivar num_joints: number of joints in the mechanism
-    :ivar is_linkage: True if the mechanism is a linkage, False if it is 1 branch of a
-        linkage
-    :ivar tool_frame: end effector of the mechanism
-    :ivar segments: list of LineSegment objects representing the physical realization of
-        the linkage
+    :ivar list factorizations: list of MotionFactorization objects
+    :ivar int num_joints: number of joints in the mechanism
+    :ivar bool is_linkage: True if the mechanism is a linkage, False if it is 1 branch
+        of a linkage
+    :ivar DualQuaternion tool_frame: end effector of the mechanism
+    :ivar AffineMetric metric: object representing the metric of the mechanism
+    :ivar LineSegment segments: list of LineSegment objects representing the physical
+        realization of the linkage
+
 
     :examples:
 
@@ -70,14 +72,44 @@ class RationalMechanism(RationalCurve):
         self.factorizations = factorizations
         self.num_joints = sum([f.number_of_factors for f in factorizations])
 
-        self.is_linkage = True if len(self.factorizations) == 2 else False
-
         self.tool_frame = self._determine_tool(tool)
 
-        self.segments = None
+        self.is_linkage = len(self.factorizations) == 2
 
-        if self.is_linkage:
-            self.update_segments()
+        self._segments = None
+
+    @property
+    def segments(self):
+        """
+        Return the line segments of the linkage.
+
+        Line segments are the physical realization of the linkage.
+
+        :return: list of LineSegment objects
+        :rtype: list[LineSegment]
+        """
+        if self._segments is None and self.is_linkage:
+            self._segments = self._get_line_segments_of_linkage()
+        else:
+            ValueError("Segments are available only for linkages.")
+
+        return self._segments
+
+    @property
+    def metric(self):
+        """
+        Define a metric in R12 for the mechanism.
+
+        This metric is used for collision detection.
+        """
+        if self._metric is None:
+            from .AffineMetric import AffineMetric  # inner import
+            mechanism_points = self.points_at_parameter(0,
+                                                        inverted_part=True,
+                                                        only_links=False)
+            self._metric = AffineMetric(self.curve(), mechanism_points)
+
+        return self._metric
 
     @classmethod
     def from_saved_file(cls, filename: str):
@@ -663,7 +695,8 @@ class RationalMechanism(RationalCurve):
 
         return solutions, intersection_points
 
-    def get_intersection_points(self, l0: NormalizedLine, l1: NormalizedLine,
+    @staticmethod
+    def get_intersection_points(l0: NormalizedLine, l1: NormalizedLine,
                                 t_params: list[float]):
         """
         Return the intersection points of two lines.
@@ -675,7 +708,6 @@ class RationalMechanism(RationalCurve):
         :return: list of intersection points
         :rtype: list[PointHomogeneous]
         """
-        from .PointHomogeneous import PointHomogeneous
         intersection_points = [PointHomogeneous()] * len(t_params)
 
         for i, t_val in enumerate(t_params):
@@ -690,12 +722,6 @@ class RationalMechanism(RationalCurve):
 
         return intersection_points
 
-    def update_segments(self):
-        """
-        Update the line segments of the linkage.
-        """
-        self.segments = self._get_line_segments_of_linkage()
-
     def _get_line_segments_of_linkage(self) -> list:
         """
         Return the line segments of the linkage.
@@ -707,7 +733,7 @@ class RationalMechanism(RationalCurve):
         :return: list of LineSegment objects
         :rtype: list[LineSegment]
         """
-        from .Linkage import LineSegment
+        from .Linkage import LineSegment  # inner import
 
         t = sp.Symbol("t")
 
@@ -761,7 +787,7 @@ class RationalMechanism(RationalCurve):
         """
         Perform singularity check of the mechanism.
         """
-        from .SingularityAnalysis import SingularityAnalysis
+        from .SingularityAnalysis import SingularityAnalysis  # inner import
 
         sa = SingularityAnalysis()
         return sa.check_singularity(self)
@@ -776,7 +802,7 @@ class RationalMechanism(RationalCurve):
             result of the optimization
         :rtype: list, list, float
         """
-        from .CollisionFreeOptimization import CollisionFreeOptimization
+        from .CollisionFreeOptimization import CollisionFreeOptimization  # inner import
 
         # get smallest polyline
         pts, points_params, res = CollisionFreeOptimization(self).smallest_polyline()
@@ -829,13 +855,13 @@ class RationalMechanism(RationalCurve):
         return results
 
     def points_at_parameter(self,
-                            t: float,
+                            t_param: float,
                             inverted_part: bool = False,
                             only_links: bool = False) -> list[PointHomogeneous]:
         """
         Get the points of the mechanism at the given parameter.
 
-        :param float t: parameter value
+        :param float t_param: parameter value
         :param bool inverted_part: if True, return the evaluated points for the inverted
             part of the mechanism
         :param bool only_links: if True, instead of two points per joint segment,
@@ -844,10 +870,9 @@ class RationalMechanism(RationalCurve):
         :return: list of connection points of the mechanism
         :rtype: list[PointHomogeneous]
         """
-
-        branch0 = self.factorizations[0].direct_kinematics(t,
+        branch0 = self.factorizations[0].direct_kinematics(t_param,
                                                            inverted_part=inverted_part)
-        branch1 = self.factorizations[1].direct_kinematics(t,
+        branch1 = self.factorizations[1].direct_kinematics(t_param,
                                                            inverted_part=inverted_part)
 
         points = branch0 + branch1[::-1]
@@ -856,3 +881,32 @@ class RationalMechanism(RationalCurve):
             points = [points[i] for i in range(0, len(points), 2)]
 
         return [PointHomogeneous.from_3d_point(p) for p in points]
+
+    def update_metric(self):
+        """
+        Update the metric of the mechanism.
+
+        Set to none so that the metric is recalculated when needed.
+        """
+        self._metric = None
+
+    def update_segments(self):
+        """
+        Update the line segments of the linkage.
+        """
+        self._segments = self._get_line_segments_of_linkage()
+
+    def get_relative_motions(self):
+        """
+        Get the relative motions of the mechanism.
+        """
+        sequence = DualQuaternion()
+        branch0 = [sequence := sequence * factor for factor in
+                   self.factorizations[0].factors_with_parameter]
+
+        sequence = DualQuaternion()
+        branch1 = [sequence := sequence * factor for factor in
+                   self.factorizations[1].factors_with_parameter]
+
+        relative_motions = branch0 + branch1[::-1]
+        return relative_motions

python/rational_linkages/models.py

@@ -95,3 +95,23 @@ def plane_fold_6r() -> RationalMechanism:
     with importlib.resources.path(resource_package, resource_path) as file_path:
         with open(file_path, 'rb') as f:
             return pickle.load(f)
+
+
+def interp_4poses_6r() -> RationalMechanism:
+    """
+    Returns a RationalMechanism object of a 6R mechanism that interpolates between 4 poses.
+
+    Original poses to be interpolated:
+    p0 = DualQuaternion.as_rational()
+    p1 = DualQuaternion.as_rational([0, 0, 0, 1, 1, 0, 1, 0])
+    p2 = DualQuaternion.as_rational([1, 2, 0, 0, -2, 1, 0, 0])
+    p3 = DualQuaternion.as_rational([3, 0, 1, 0, 1, 0, -3, 0])
+
+    :return: RationalMechanism object for the 6R linkage.
+    :rtype: RationalMechanism
+    """
+    resource_package = "rational_linkages.data"
+    resource_path = 'interp_4poses_6r.pkl'
+    with importlib.resources.path(resource_package, resource_path) as file_path:
+        with open(file_path, 'rb') as f:
+            return pickle.load(f)

python/tests/test_MiniBall.py

@@ -7,38 +7,46 @@ from rational_linkages import MiniBall, PointHomogeneous
 
 class TestMiniBall(TestCase):
     def test_get_ball(self):
-        # 2D Euclidean ball
-        ball = MiniBall(
-            [
-                PointHomogeneous.at_origin_in_2d(),
-                PointHomogeneous(np.array([1, 1, 0])),
-                PointHomogeneous(np.array([1, -1, 0])),
-                PointHomogeneous(np.array([1, 0.5, 0.5])),
-            ]
-        )
-
+        # 2D Euclidean
+        points = [PointHomogeneous.at_origin_in_2d(),
+                  PointHomogeneous(np.array([1, 1, 0])),
+                  PointHomogeneous(np.array([1, -1, 0])),
+                  PointHomogeneous(np.array([1, 0.5, 0.5]))]
+        ball = MiniBall(points, method='welzl')
         expected_center = PointHomogeneous.at_origin_in_2d()
-        expected_radius = np.float64(1.0)
+        expected_radius_squared = np.float64(1.0)
 
-        self.assertAlmostEqual(ball.radius, expected_radius)
+        self.assertAlmostEqual(ball.radius_squared, expected_radius_squared)
         self.assertTrue(
             np.allclose(ball.center.array(), expected_center.array(), atol=1e-06)
         )
 
-        # 3D Euclidean ball
-        ball = MiniBall(
-            [
-                PointHomogeneous(),
-                PointHomogeneous(np.array([1, 2, 0, 0])),
-                PointHomogeneous(np.array([1, 1, 1, 0])),
-                PointHomogeneous(np.array([1, 1, 0, 1])),
-            ]
+        ball = MiniBall(points, method='minimize')
+        self.assertAlmostEqual(ball.radius_squared, expected_radius_squared)
+        self.assertTrue(
+            np.allclose(ball.center.array(), expected_center.array(), atol=1e-06)
         )
 
+        # 3D Euclidean ball
+        points = [PointHomogeneous(),
+                  PointHomogeneous(np.array([1, 2, 0, 0])),
+                  PointHomogeneous(np.array([1, 1, 1, 0])),
+                  PointHomogeneous(np.array([1, 1, 0, 1]))]
+
+
         expected_center = PointHomogeneous(np.array([1, 1, 0, 0]))
-        expected_radius = np.float64(1.0)
+        expected_radius_squared = np.float64(1.0)
+
+        ball = MiniBall(points, method='welzl')
+
+        self.assertAlmostEqual(ball.radius_squared, expected_radius_squared)
+        self.assertTrue(
+            np.allclose(ball.center.array(), expected_center.array(), atol=1e-06)
+        )
+
+        ball = MiniBall(points, method='minimize')
 
-        self.assertAlmostEqual(ball.radius, expected_radius)
+        self.assertAlmostEqual(ball.radius_squared, expected_radius_squared)
         self.assertTrue(
             np.allclose(ball.center.array(), expected_center.array(), atol=1e-06)
         )