diff --git a/python/hemisphere-joint-demo/CMakeLists.txt b/python/hemisphere-joint-demo/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/python/hemisphere-joint-demo/README.rst b/python/hemisphere-joint-demo/README.rst new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/python/hemisphere-joint-demo/hemisphere-joint-demo/__init__.py b/python/hemisphere-joint-demo/hemisphere-joint-demo/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/python/hemisphere-joint-demo/hemisphere-joint-demo/cornelius_joint_mecha_demo.py b/python/hemisphere-joint-demo/hemisphere-joint-demo/cornelius_joint_mecha_demo.py new file mode 100755 index 0000000000000000000000000000000000000000..5121adce413d8b9187a98331d4760d6cd7d47ea6 --- /dev/null +++ b/python/hemisphere-joint-demo/hemisphere-joint-demo/cornelius_joint_mecha_demo.py @@ -0,0 +1,131 @@ +import os +import numpy as np +import transforms3d as tf3d + +from typing import Optional + +from armarx import arviz as viz +from armarx import remote_gui as rg + +from armarx.remote_gui.widgets.ndarray import NdArrayWidget + + +class Data: + + def __init__(self): + super().__init__() + + self.linear_joints = np.zeros(2) + self.eef_pos = np.zeros(3) + + self.links = np.array([ + [0, 0, 5], + [0, 0, 5], + ]) + + self.fk() + + def fk(self): + # Dummy. + if False: + eef = np.zeros(3) + + eef += tf3d.axangles.axangle2mat((1, 0, 0), self.linear_joints[0]) @ self.links[0] + eef += tf3d.axangles.axangle2mat((1, 0, 0), self.linear_joints[1]) @ self.links[1] + + self.eef_pos = eef + self.eef_pos[:] = [result[PE_xs], result[PE_ys], result[PE_zs]] + + def ik(self): + pass + + +class Visu: + + def __init__(self, data: Data): + self.data = data + + def visualize(self, stage: viz.Stage): + self.vis_linear_joints(stage.layer("Linear Joints")) + self.vis_eef_pos(stage.layer("EEF Pos")) + stage.origin_layer(scale=1e-3) + + def vis_linear_joints(self, layer: viz.Layer): + ground_radius = 5 + for i, value in enumerate(self.data.linear_joints): + angle = i * np.pi/2 + start = ground_radius * np.array([np.cos(angle), np.sin(angle), 0]) + if value == 0: + value = 1e-3 + end = start + value * 10 * np.array([0, 0, 1]) + layer.add(viz.Arrow(f"linear joint {i}", from_to=(start, end), + width=0.2, color=(255, 192, 0))) + + def vis_eef_pos(self, layer: viz.Layer): + layer.add(viz.Sphere("eef pos", position=self.data.eef_pos, radius=1, color=(0, 0, 255))) + layer.add(viz.Pose("eef pose", position=self.data.eef_pos, scale=1e-3)) + + +class WidgetsTab(rg.Tab): + + def __init__( + self, + data: Data, + visu: Visu, + tag=None, + arviz: Optional[viz.Client] = None): + super().__init__(tag or TAG) + + self.data = data + self.visu = visu + self.arviz = arviz or viz.Client(tag) + + self.linear_joints = NdArrayWidget(data.linear_joints, row_vector=True, range_min=0, range_max=10) + self.eef_pos = NdArrayWidget(data.eef_pos, row_vector=True, range_min=-100, range_max=100) + + def create_widget_tree(self) -> rg.GridLayout: + layout = rg.GridLayout() + row = 0 + + layout.add(rg.Label("Linear Joints:"), (row, 0)) + layout.add(self.linear_joints.create_tree(), (row, 1)) + row += 1 + + layout.add(rg.Label("EEF Pos:"), (row, 0)) + layout.add(self.eef_pos.create_tree(), (row, 1)) + row += 1 + + return layout + + def update(self): + + if self.linear_joints.has_any_changed(): + self.data.linear_joints = self.linear_joints.get_array() + self.data.fk() + + if self.eef_pos.has_any_changed(): + self.data.eef_pos = self.eef_pos.get_array() + self.data.ik() + + with self.arviz.begin_stage(commit_on_exit=True) as stage: + self.visu.visualize(stage) + + +if __name__ == "__main__": + TAG = os.path.basename(__file__) + + data = Data() + visu = Visu(data) + arviz = viz.Client(TAG) + + with arviz.begin_stage(commit_on_exit=True) as stage: + visu.visualize(stage) + + rg_client = rg.Client() + + widgets_tab = WidgetsTab(data=data, visu=visu, tag=TAG) + rg_client.add_tab(widgets_tab) + rg_client.receive_updates() + rg_client.receive_updates() + + rg_client.update_loop(lambda: widgets_tab.update(), block=False) diff --git a/python/hemisphere-joint-demo/poetry.toml b/python/hemisphere-joint-demo/poetry.toml new file mode 100644 index 0000000000000000000000000000000000000000..ab1033bd37224ee84b5862fb25f094db73809b74 --- /dev/null +++ b/python/hemisphere-joint-demo/poetry.toml @@ -0,0 +1,2 @@ +[virtualenvs] +in-project = true diff --git a/python/hemisphere-joint-demo/pyproject.toml b/python/hemisphere-joint-demo/pyproject.toml new file mode 100644 index 0000000000000000000000000000000000000000..7c9d0cb1f97dbfac02f0f0582db2d24838296793 --- /dev/null +++ b/python/hemisphere-joint-demo/pyproject.toml @@ -0,0 +1,25 @@ +[tool.poetry] +name = "hemisphere-joint-demo" +version = "0.1.0" +description = "" +authors = ["Rainer Kartmann <rainer dot kartmann at kit dot edu>"] + +[tool.poetry.dependencies] +python = "^3.6" +armarx-dev = { path = "../../../armarx/python3-armarx/", develop=true} # "^0.16.4" +zeroc-ice = "3.7.0" +numpy = "^1.19.5" +jupyter = "^1.0.0" + +[tool.poetry.dev-dependencies] +pytest = "^5.2" +ipython = "5.5" + +[build-system] +requires = ["poetry-core@https://github.com/python-poetry/poetry-core/archive/325312c016d69189ac93c945ba0c1b69296c5e54.zip"] +build-backend = "poetry.core.masonry.api" + +[[tool.poetry.source]] +name = "h2t" +url = "https://pypi.humanoids.kit.edu/simple/" + diff --git a/python/hemisphere-joint-demo/tests/__init__.py b/python/hemisphere-joint-demo/tests/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391