Hand Marker & Rotation

2026-05-30 15:19:31 +02:00
parent 261691b8d2
commit b3cf7adc95
4 changed files with 371 additions and 197 deletions
--- a/data/robot/robot.json
+++ b/data/robot/robot.json
@@ -109,10 +109,10 @@
    "e": 1
  },
  "defaultPosition": {
-    "x": 140,
-    "y": 46,
-    "z": -70,
-    "a": 120,
+    "x": 180,
+    "y": 86,
+    "z": -120,
+    "a": -60,
    "b": 22,
    "c": 91,
    "e": 10
@@ -493,6 +493,11 @@
        "radius": 4,
        "color": [0.20, 0.80, 0.20]
      },
+      "markers":[      
+        {"id":40,"position":[12,-24,-17.1],"normal":[-10.98,0,-23.56]},
+        {"id":41,"position":[1.5,-2.2,25.8],"normal":[0,-25.6,9.5]},
+        {"id":42,"position":[13.9,-40,0],"normal":[1,-0.35,0.40], "spin": 27}
+      ],
      "model": [
        {
          "stlFile": "surfaces/Finger.stl",
@@ -521,6 +526,11 @@
        "radius": 4,
        "color": [0.20, 0.80, 0.20]
      },
+      "markers":[      
+        {"id":43,"position":[-12,-24,17.1],"normal":[10.98,0,23.56], "spin":90 },
+        {"id":44,"position":[-1.5,-2.2,-25.8],"normal":[0,-25.6,-9.5], "spin":90},
+        {"id":45,"position":[-13.9,-40,0],"normal":[-1,-0.35,-0.40], "spin": -27}
+      ],
      "model": [
        {
          "stlFile": "surfaces/Finger.stl",
--- a/data/simulation/debug/markers.json
+++ b/data/simulation/debug/markers.json
@@ -238,25 +238,25 @@
    "id": 198,
    "link": "Arm1",
    "position_m": [
-      0.25,
-      0.022391222417354584,
-      0.20075473189353943
+      0.2900000214576721,
+      0.13225246965885162,
+      0.22308659553527832
    ],
    "position_mm": [
-      250.0,
-      22.391222417354584,
-      200.75473189353943
+      290.0000214576721,
+      132.25246965885162,
+      223.08659553527832
    ],
    "rotation_quaternion": [
-      0.9205048084259033,
-      -0.39073115587234497,
+      0.7313537001609802,
+      -0.6819983720779419,
      0.0,
      0.0
    ],
    "normal": [
      0.0,
-      0.7193398475646973,
-      0.6946583390235901
+      0.9975640773773193,
+      0.06975647807121277
    ]
  },
  {
@@ -264,25 +264,25 @@
    "id": 229,
    "link": "Arm1",
    "position_m": [
-      0.25,
-      -0.040128037333488464,
-      0.26549533009529114
+      0.2900000214576721,
+      0.1259743720293045,
+      0.3128673732280731
    ],
    "position_mm": [
-      250.0,
-      -40.128037333488464,
-      265.49533009529114
+      290.0000214576721,
+      125.9743720293045,
+      312.8673732280731
    ],
    "rotation_quaternion": [
-      0.9205048084259033,
-      -0.39073115587234497,
+      0.7313537001609802,
+      -0.6819983720779419,
      0.0,
      0.0
    ],
    "normal": [
      0.0,
-      0.7193398475646973,
-      0.6946583390235901
+      0.9975640773773193,
+      0.06975647807121277
    ]
  },
  {
@@ -290,25 +290,25 @@
    "id": 242,
    "link": "Arm1",
    "position_m": [
-      0.25,
-      -0.09120115637779236,
-      0.21617457270622253
+      0.2900000214576721,
+      0.05514732748270035,
+      0.307914674282074
    ],
    "position_mm": [
-      250.0,
-      -91.20115637779236,
-      216.17457270622253
+      290.0000214576721,
+      55.14732748270035,
+      307.914674282074
    ],
    "rotation_quaternion": [
-      0.27628862857818604,
-      0.6508952379226685,
-      0.6508952975273132,
-      -0.27628862857818604
+      0.4822455942630768,
+      0.5171451568603516,
+      0.5171452164649963,
+      -0.4822455644607544
    ],
    "normal": [
-      2.9802318834981634e-08,
-      -0.7193397879600525,
-      -0.6946584582328796
+      8.940698137394065e-08,
+      -0.9975640773773193,
+      -0.06975672394037247
    ]
  },
  {
@@ -316,25 +316,25 @@
    "id": 243,
    "link": "Arm1",
    "position_m": [
-      0.25,
-      -0.09032496064901352,
-      0.266371488571167
+      0.2900000214576721,
+      0.08808448910713196,
+      0.34580451250076294
    ],
    "position_mm": [
-      250.0,
-      -90.32496064901352,
-      266.371488571167
+      290.0000214576721,
+      88.08448910713196,
+      345.80451250076294
    ],
    "rotation_quaternion": [
-      0.9271838665008545,
-      0.3746066093444824,
+      0.9993908405303955,
+      0.03489954397082329,
      0.0,
      0.0
    ],
    "normal": [
      0.0,
-      -0.6946583986282349,
-      0.7193398475646973
+      -0.06975656747817993,
+      0.9975640773773193
    ]
  },
  {
@@ -342,25 +342,25 @@
    "id": 244,
    "link": "Ellbow",
    "position_m": [
-      0.37549999356269836,
-      -0.06566459685564041,
-      0.24083495140075684
+      0.4155000150203705,
+      0.09056085348129272,
+      0.31039100885391235
    ],
    "position_mm": [
-      375.49999356269836,
-      -65.66459685564041,
-      240.83495140075684
+      415.5000150203705,
+      90.56085348129272,
+      310.39100885391235
    ],
    "rotation_quaternion": [
-      0.6916548013687134,
-      0.1470157951116562,
-      0.6916548013687134,
-      0.1470157951116562
+      0.676209568977356,
+      0.20673811435699463,
+      0.676209568977356,
+      0.20673811435699463
    ],
    "normal": [
      1.0,
      0.0,
-      -1.6763806343078613e-08
+      2.607702853651972e-08
    ]
  },
  {
@@ -368,25 +368,25 @@
    "id": 245,
    "link": "Ellbow",
    "position_m": [
-      0.3400000035762787,
-      -0.0512254424393177,
-      0.20840409398078918
+      0.3800000250339508,
+      0.11041221022605896,
+      0.28096017241477966
    ],
    "position_mm": [
-      340.0000035762787,
-      -51.2254424393177,
-      208.40409398078918
+      380.0000250339508,
+      110.41221022605896,
+      280.96017241477966
    ],
    "rotation_quaternion": [
-      0.14701582491397858,
-      -0.6916547417640686,
-      -0.6916548013687134,
-      -0.1470157504081726
+      0.2067381590604782,
+      -0.676209568977356,
+      -0.676209568977356,
+      -0.20673811435699463
    ],
    "normal": [
-      -1.1920930376163597e-07,
-      0.40673673152923584,
-      -0.9135454893112183
+      -5.960463766996327e-08,
+      0.5591931939125061,
+      -0.8290373086929321
    ]
  },
  {
@@ -394,25 +394,25 @@
    "id": 246,
    "link": "Ellbow",
    "position_m": [
-      0.3400000035762787,
-      -0.08010375499725342,
-      0.2732658088207245
+      0.3800000250339508,
+      0.07070949673652649,
+      0.33982184529304504
    ],
    "position_mm": [
-      340.0000035762787,
-      -80.10375499725342,
-      273.2658088207245
+      380.0000250339508,
+      70.70949673652649,
+      339.82184529304504
    ],
    "rotation_quaternion": [
-      0.9781476259231567,
-      0.20791172981262207,
+      0.9563047289848328,
+      0.29237180948257446,
      0.0,
      0.0
    ],
    "normal": [
      0.0,
-      -0.40673667192459106,
-      0.9135453701019287
+      -0.5591930747032166,
+      0.8290374279022217
    ]
  },
  {
@@ -420,25 +420,25 @@
    "id": 247,
    "link": "Ellbow",
    "position_m": [
-      0.30250000953674316,
-      -0.08010375499725342,
-      0.2732658088207245
+      0.3425000309944153,
+      0.07070949673652649,
+      0.33982184529304504
    ],
    "position_mm": [
-      302.50000953674316,
-      -80.10375499725342,
-      273.2658088207245
+      342.5000309944153,
+      70.70949673652649,
+      339.82184529304504
    ],
    "rotation_quaternion": [
-      0.9781476259231567,
-      0.20791172981262207,
+      0.9563047289848328,
+      0.29237180948257446,
      0.0,
      0.0
    ],
    "normal": [
      0.0,
-      -0.40673667192459106,
-      0.9135453701019287
+      -0.5591930747032166,
+      0.8290374279022217
    ]
  },
  {
@@ -446,25 +446,25 @@
    "id": 120,
    "link": "Arm2",
    "position_m": [
-      0.34918853640556335,
-      -0.1819295436143875,
-      0.22660782933235168
+      0.37081149220466614,
+      0.016884557902812958,
+      0.21933193504810333
    ],
    "position_mm": [
-      349.18853640556335,
-      -181.9295436143875,
-      226.60782933235168
+      370.81149220466614,
+      16.884557902812958,
+      219.33193504810333
    ],
    "rotation_quaternion": [
-      0.9697794318199158,
-      0.20613299310207367,
-      0.1276739090681076,
-      0.027137938886880875
+      0.12482281029224396,
+      0.03816218301653862,
+      -0.9481233954429626,
+      -0.28987058997154236
    ],
    "normal": [
-      0.2588190734386444,
-      -0.39287739992141724,
-      0.8824170827865601
+      -0.25881901383399963,
+      0.5401390790939331,
+      -0.8007885813713074
    ]
  },
  {
@@ -472,25 +472,25 @@
    "id": 122,
    "link": "Arm2",
    "position_m": [
-      0.3577499985694885,
-      -0.15547701716423035,
-      0.16719448566436768
+      0.36225003004074097,
+      -0.019483134150505066,
+      0.27324920892715454
    ],
    "position_mm": [
-      357.7499985694885,
-      -155.47701716423035,
-      167.19448566436768
+      362.25003004074097,
+      -19.483134150505066,
+      273.24920892715454
    ],
    "rotation_quaternion": [
-      0.2531631588935852,
-      0.05381150171160698,
-      0.9448180794715881,
-      0.20082731544971466
+      0.9237194657325745,
+      0.2824094891548157,
+      -0.24750985205173492,
+      -0.07567139714956284
    ],
    "normal": [
-      0.49999988079071045,
-      0.35224437713623047,
-      -0.791153609752655
+      -0.49999988079071045,
+      -0.4842754304409027,
+      0.7179675102233887
    ]
  },
  {
@@ -498,25 +498,25 @@
    "id": 218,
    "link": "Arm2",
    "position_m": [
-      0.32225000858306885,
-      -0.18048636615276337,
-      0.2233663946390152
+      0.39775002002716064,
+      0.014900431036949158,
+      0.22227352857589722
    ],
    "position_mm": [
-      322.25000858306885,
-      -180.48636615276337,
-      223.3663946390152
+      397.75002002716064,
+      14.900431036949158,
+      222.27352857589722
    ],
    "rotation_quaternion": [
-      0.9448180198669434,
-      0.20082733035087585,
-      -0.2531631886959076,
-      -0.053811509162187576
+      0.24750986695289612,
+      0.07567141205072403,
+      0.9237194657325745,
+      0.2824094891548157
    ],
    "normal": [
-      -0.5,
-      -0.35224440693855286,
-      0.791153609752655
+      0.4999999403953552,
+      0.4842754006385803,
+      -0.7179675102233887
    ]
  },
  {
@@ -524,25 +524,25 @@
    "id": 113,
    "link": "Arm2",
    "position_m": [
-      0.3135862350463867,
-      -0.2257271409034729,
-      0.1528773009777069
+      0.4064137935638428,
+      -0.06885166466236115,
+      0.2212606817483902
    ],
    "position_mm": [
-      313.5862350463867,
-      -225.7271409034729,
-      152.8773009777069
+      406.4137935638428,
+      -68.85166466236115,
+      221.2606817483902
    ],
    "rotation_quaternion": [
-      0.48907387256622314,
-      0.10395587980747223,
-      -0.8471006751060486,
-      -0.1800568401813507
+      0.828184187412262,
+      0.253201425075531,
+      0.47815239429473877,
+      0.14618593454360962
    ],
    "normal": [
-      -0.866025447845459,
-      0.20336830615997314,
-      -0.4567725956439972
+      0.866025447845459,
+      -0.2795965075492859,
+      0.4145187437534332
    ]
  },
  {
@@ -550,25 +550,25 @@
    "id": 101,
    "link": "Arm2",
    "position_m": [
-      0.34918853640556335,
-      -0.2458777278661728,
-      0.19813625514507294
+      0.37081149220466614,
+      -0.04114805907011032,
+      0.18018841743469238
    ],
    "position_mm": [
-      349.18853640556335,
-      -245.8777278661728,
-      198.13625514507294
+      370.81149220466614,
+      -41.14805907011032,
+      180.18841743469238
    ],
    "rotation_quaternion": [
-      0.9697794318199158,
-      0.20613299310207367,
-      0.1276739090681076,
-      0.027137938886880875
+      0.12482281029224396,
+      0.03816218301653862,
+      -0.9481233954429626,
+      -0.28987058997154236
    ],
    "normal": [
-      0.2588190734386444,
-      -0.39287739992141724,
-      0.8824170827865601
+      -0.25881901383399963,
+      0.5401390790939331,
+      -0.8007885813713074
    ]
  },
  {
@@ -576,25 +576,25 @@
    "id": 102,
    "link": "Arm2",
    "position_m": [
-      0.3742921054363251,
-      -0.2281925529241562,
-      0.15841472148895264
+      0.3457079231739044,
+      -0.0654621347784996,
+      0.21623548865318298
    ],
    "position_mm": [
-      374.2921054363251,
-      -228.1925529241562,
-      158.41472148895264
+      345.7079231739044,
+      -65.4621347784996,
+      216.23548865318298
    ],
    "rotation_quaternion": [
-      0.595458447933197,
-      0.12656863033771515,
-      0.7760167121887207,
-      0.1649474799633026
+      0.7586875557899475,
+      0.2319541722536087,
+      -0.5821614265441895,
+      -0.17798471450805664
    ],
    "normal": [
-      0.9659258127212524,
-      0.10527129471302032,
-      -0.23644313216209412
+      -0.9659258723258972,
+      -0.1447298228740692,
+      0.21457071602344513
    ]
  },
  {
@@ -602,25 +602,25 @@
    "id": 124,
    "link": "Arm2",
    "position_m": [
-      0.3577499985694885,
-      -0.2532263696193695,
-      0.12367365509271622
+      0.36225003004074097,
+      -0.1081901341676712,
+      0.21341554820537567
    ],
    "position_mm": [
-      357.7499985694885,
-      -253.2263696193695,
-      123.67365509271622
+      362.25003004074097,
+      -108.1901341676712,
+      213.41554820537567
    ],
    "rotation_quaternion": [
-      0.2531631588935852,
-      0.05381150171160698,
-      0.9448180794715881,
-      0.20082731544971466
+      0.9237194657325745,
+      0.2824094891548157,
+      -0.24750985205173492,
+      -0.07567139714956284
    ],
    "normal": [
-      0.49999988079071045,
-      0.35224437713623047,
-      -0.791153609752655
+      -0.49999988079071045,
+      -0.4842754304409027,
+      0.7179675102233887
    ]
  },
  {
@@ -628,25 +628,181 @@
    "id": 219,
    "link": "Arm2",
    "position_m": [
-      0.32225000858306885,
-      -0.2782357335090637,
-      0.17984557151794434
+      0.39775002002716064,
+      -0.07380656898021698,
+      0.16243985295295715
    ],
    "position_mm": [
-      322.25000858306885,
-      -278.2357335090637,
-      179.84557151794434
+      397.75002002716064,
+      -73.80656898021698,
+      162.43985295295715
    ],
    "rotation_quaternion": [
-      0.9448180198669434,
-      0.20082733035087585,
-      -0.2531631886959076,
-      -0.053811509162187576
+      0.24750986695289612,
+      0.07567141205072403,
+      0.9237194657325745,
+      0.2824094891548157
    ],
    "normal": [
-      -0.5,
-      -0.35224440693855286,
-      0.791153609752655
+      0.4999999403953552,
+      0.4842754006385803,
+      -0.7179675102233887
+    ]
+  },
+  {
+    "name": "FingerA_marker_40",
+    "id": 40,
+    "link": "FingerA",
+    "position_m": [
+      0.3903598487377167,
+      -0.16245591640472412,
+      0.12312574684619904
+    ],
+    "position_mm": [
+      390.3598487377167,
+      -162.45591640472412,
+      123.12574684619904
+    ],
+    "rotation_quaternion": [
+      0.00047839104081504047,
+      -0.177643820643425,
+      -0.9121214151382446,
+      -0.36942800879478455
+    ],
+    "normal": [
+      0.13038049638271332,
+      0.6740963459014893,
+      -0.7270454168319702
+    ]
+  },
+  {
+    "name": "FingerA_marker_41",
+    "id": 41,
+    "link": "FingerA",
+    "position_m": [
+      0.3667393624782562,
+      -0.16298790276050568,
+      0.16668358445167542
+    ],
+    "position_mm": [
+      366.7393624782562,
+      -162.98790276050568,
+      166.68358445167542
+    ],
+    "rotation_quaternion": [
+      0.5471516251564026,
+      0.7730997800827026,
+      -0.3062758445739746,
+      -0.09558753669261932
+    ],
+    "normal": [
+      -0.48295605182647705,
+      -0.787453293800354,
+      -0.3829762637615204
+    ]
+  },
+  {
+    "name": "FingerA_marker_42",
+    "id": 42,
+    "link": "FingerA",
+    "position_m": [
+      0.37803414463996887,
+      -0.18306580185890198,
+      0.12539319694042206
+    ],
+    "position_mm": [
+      378.0341446399689,
+      -183.06580185890198,
+      125.39319694042206
+    ],
+    "rotation_quaternion": [
+      0.7558663487434387,
+      0.6153563261032104,
+      0.1284194439649582,
+      0.1830604076385498
+    ],
+    "normal": [
+      0.41943061351776123,
+      -0.8832373023033142,
+      0.20969009399414062
+    ]
+  },
+  {
+    "name": "FingerB_marker_43",
+    "id": 43,
+    "link": "FingerB",
+    "position_m": [
+      0.33135876059532166,
+      -0.14928503334522247,
+      0.13855642080307007
+    ],
+    "position_mm": [
+      331.35876059532166,
+      -149.28503334522247,
+      138.55642080307007
+    ],
+    "rotation_quaternion": [
+      0.7705803513526917,
+      0.26156336069107056,
+      -0.26088663935661316,
+      0.5193541049957275
+    ],
+    "normal": [
+      -0.13038022816181183,
+      -0.6740962862968445,
+      0.7270455956459045
+    ]
+  },
+  {
+    "name": "FingerB_marker_44",
+    "id": 44,
+    "link": "FingerB",
+    "position_m": [
+      0.3688197433948517,
+      -0.12042809277772903,
+      0.12374269217252731
+    ],
+    "position_mm": [
+      368.8197433948517,
+      -120.42809277772903,
+      123.74269217252731
+    ],
+    "rotation_quaternion": [
+      0.16055524349212646,
+      0.42056193947792053,
+      -0.8736730813980103,
+      0.1845129281282425
+    ],
+    "normal": [
+      -0.12534736096858978,
+      -0.4574548006057739,
+      -0.8803540468215942
+    ]
+  },
+  {
+    "name": "FingerB_marker_45",
+    "id": 45,
+    "link": "FingerB",
+    "position_m": [
+      0.3332028090953827,
+      -0.1501261293888092,
+      0.11452052742242813
+    ],
+    "position_mm": [
+      333.2028090953827,
+      -150.1261293888092,
+      114.52052742242813
+    ],
+    "rotation_quaternion": [
+      0.17917531728744507,
+      0.4258763790130615,
+      -0.7947980761528015,
+      -0.39347371459007263
+    ],
+    "normal": [
+      -0.6199586987495422,
+      0.4728512465953827,
+      -0.6261492967605591
    ]
  }
 ]
--- a/data/simulation/debug/render.png
+++ b/data/simulation/debug/render.png
--- a/setup/generateSets/render_robot.py
+++ b/setup/generateSets/render_robot.py
@@ -752,12 +752,20 @@ for link_name, link_info in links_def.items():
            safe_parent(marker_obj, link_frame, keep_world=False)

            marker_obj.rotation_mode = "QUATERNION"
+            #alt: base_quat = normal_to_quaternion(normal)
+
+            normal = mathutils.Vector(m.get("normal", [0, 0, 1]))
+            if normal.length == 0:
+                normal = mathutils.Vector((0, 0, 1))
+            normal.normalize()
+
            base_quat = normal_to_quaternion(normal)

            spin_quat = mathutils.Quaternion(
-                mathutils.Vector(normal).normalized(),
+                mathutils.Vector((0, 0, 1)),
                math.radians(marker_spin_deg)
            )
+            
            marker_obj.rotation_quaternion = (
                base_quat @ spin_quat
            )
@@ -791,7 +799,7 @@ for link_name, link_info in links_def.items():
            # BACKING PLATE (white PLA behind marker)
            # --------------------------------------------------------

-            plate_side_mm = 28.0
+            plate_side_mm = 26.5
            plate_thickness_mm = 1.0
            gap_mm = 0.2   # kleiner Abstand gegen Z-Fighting