From c0fa3347743ba6d71b8c97e2432b2ac4f8cb7eb8 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 9 Jun 2022 10:46:10 +0200
Subject: [PATCH] Add output to python demo
---
.../hemisphere_joint_demo/app/hemisphere_joint_demo_run.py | 7 ++++++-
.../hemisphere_joint_demo/equations.py | 2 +-
2 files changed, 7 insertions(+), 2 deletions(-)
diff --git a/python/hemisphere-joint-demo/hemisphere_joint_demo/app/hemisphere_joint_demo_run.py b/python/hemisphere-joint-demo/hemisphere_joint_demo/app/hemisphere_joint_demo_run.py
index 448859e24..5b8197d37 100755
--- a/python/hemisphere-joint-demo/hemisphere_joint_demo/app/hemisphere_joint_demo_run.py
+++ b/python/hemisphere-joint-demo/hemisphere_joint_demo/app/hemisphere_joint_demo_run.py
@@ -39,6 +39,7 @@ class Data:
self.lever = 1
self.theta_0 = np.deg2rad(25)
self.radius = 2 * np.sin(self.theta_0) * self.lever
+ self.actuator_offset = np.arcsin(self.theta_0)
self.mode = self.Mode.FK
@@ -56,12 +57,16 @@ class Data:
def fk(self):
print("-" * 50)
- a1, a2 = self.actuator_pos + np.arcsin(self.theta_0)
+ a1, a2 = self.actuator_pos + self.actuator_offset
# KIT-Wrist constants
lever = self.lever
theta_0 = self.theta_0
radius = self.radius
+ print(f"Lever: {lever}")
+ print(f"theta0: {theta_0}")
+ print(f"radius: {radius}")
+ print(f"actuator_offset: {self.actuator_offset}")
print(f"(a1, a2) = ({a1}, {a2})")
do_fk_pos_azim_zenith = False
diff --git a/python/hemisphere-joint-demo/hemisphere_joint_demo/equations.py b/python/hemisphere-joint-demo/hemisphere_joint_demo/equations.py
index b1212e7c2..839988dbd 100755
--- a/python/hemisphere-joint-demo/hemisphere_joint_demo/equations.py
+++ b/python/hemisphere-joint-demo/hemisphere_joint_demo/equations.py
@@ -5,7 +5,7 @@ from numpy import pi, sin, cos, sqrt, arcsin, arctan2
from .terms import CommonTerms
-def fk(a1, a2, L, T_0):
+def fk_pos(a1, a2, L, T_0):
"""
From:
https://colab.research.google.com/drive/11faUc8pS1yWxFrnmt05VqpDsqOwEi_dg#scrollTo=za3fZw9Rq5d9&line=1&uniqifier=1
--
GitLab