Merge pull request #16 from lanius/feature/varradius

Feature/varradius

pyproject.toml

@@ -1,6 +1,6 @@
 [tool.poetry]
 name = "tinyik"
-version = "2.1.1"
+version = "2.2.0"
 description = "A tiny inverse kinematics solver"
 license = "MIT"
 authors = ["lanius <[email protected]>"]

tests/test_visualizer.py

@@ -0,0 +1,45 @@
+import numpy as np
+
+import tinyik
+
+
+tokens = [
+    [.3, .0, .0], 'z', [.3, .0, .0], 'x', [.0, -.5, .0], 'x', [.0, -.5, .0]]
+
+
+def visualize():
+    leg = tinyik.Actuator(tokens)
+    leg.angles = np.deg2rad([30, 45, -90])
+    tinyik.visualize(leg)
+
+
+def visualize_with_target():
+    leg = tinyik.Actuator(tokens)
+    leg.angles = np.deg2rad([30, 45, -90])
+    tinyik.visualize(leg, target=[.8, .0, .8])
+
+
+large_tokens = [
+    [85., 80., 0.],
+    'z',
+    [500., 0., 0.],
+    'z',
+    [0., -500., 0.],
+]
+
+
+def large_visualize():
+    arm = tinyik.Actuator(large_tokens)
+    tinyik.visualize(arm, radius=15.)
+
+
+def large_visualize_with_target():
+    arm = tinyik.Actuator(large_tokens)
+    tinyik.visualize(arm, target=[400., -300., 0.], radius=15.)
+
+
+if __name__ == '__main__':
+    visualize()
+    visualize_with_target()
+    large_visualize()
+    large_visualize_with_target()

tinyik/visualizer.py

@@ -38,10 +38,10 @@ def rotate(axis, angle):
         ])
 
 
-def create_sphere(p, r=.1, color=None):
+def create_sphere(p, radius, color=None):
     if color is None:
         color = [.8, .8, .8]
-    geo = o3d.geometry.TriangleMesh.create_sphere(radius=r)
+    geo = o3d.geometry.TriangleMesh.create_sphere(radius=radius)
     geo.compute_vertex_normals()
     geo.paint_uniform_color(color)
     geo.transform(translate(p))
@@ -51,10 +51,12 @@ def create_sphere(p, r=.1, color=None):
 class GeoComponent:
 
     child = None
+    radius = .1
 
     def tip(self, link_color=None):
         return create_sphere(
             [0., 0., 0.],
+            radius=self.radius*2,
             color=[.8, .8, .8] if link_color is None else link_color)
 
     def geo(self, mat=None, link_color=None):
@@ -72,8 +74,9 @@ def geo(self, mat=None, link_color=None):
 
 class Link(GeoComponent):
 
-    def __init__(self, c):
+    def __init__(self, c, radius):
         self.c = c
+        self.radius = radius
 
     def base_geo(self, link_color=None):
         norm = np.linalg.norm(self.c.coord)
@@ -82,7 +85,7 @@ def base_geo(self, link_color=None):
         axis = cross / np.linalg.norm(cross)
         angle = np.arccos(np.dot(base, self.c.coord) / (norm ** 2))
         geo = o3d.geometry.TriangleMesh.create_cylinder(
-            radius=.05, height=norm)
+            radius=self.radius, height=norm)
         geo.compute_vertex_normals()
         geo.paint_uniform_color(
             [.8, .8, .8] if link_color is None else link_color)
@@ -95,12 +98,14 @@ def mat(self):
 
 class Joint(GeoComponent):
 
-    def __init__(self, c):
+    def __init__(self, c, radius):
         self.c = c
+        self.radius = radius
         self.angle = 0.
 
     def base_geo(self, _=None):
-        geo = o3d.geometry.TriangleMesh.create_cylinder(radius=.1, height=.2)
+        geo = o3d.geometry.TriangleMesh.create_cylinder(
+            radius=self.radius*2, height=self.radius*4)
         geo.compute_vertex_normals()
         geo.paint_uniform_color([.2, .2, .9])
         rx = {
@@ -115,16 +120,16 @@ def mat(self):
         return self.c.matrix(self.angle)
 
 
-def visualize(actuator, target=None):
+def build_geos(actuator, target=None, radius=.05):
     root = None
     p = None
     joints = []
     for c in actuator.components:
         if hasattr(c, 'axis'):
-            gc = Joint(c)
+            gc = Joint(c, radius)
             joints.append(gc)
         else:
-            gc = Link(c)
+            gc = Link(c, radius)
 
         if root is None:
             root = gc
@@ -142,9 +147,14 @@ def visualize(actuator, target=None):
         for j, a in zip(joints, actuator.angles):
             j.angle = a
         geos += root.geo()
-        geos += [create_sphere(target, r=.12, color=[.8, .2, .2])]
+        geos += [create_sphere(target, radius=radius*2.4, color=[.8, .2, .2])]
     else:
         geos = root.geo()
 
+    return geos
+
+
+def visualize(actuator, target=None, radius=.05):
+    geos = build_geos(actuator, target, radius)
     o3d.visualization.draw_geometries(
         geos, window_name='tinyik vizualizer', width=640, height=480)