diff --git a/data/robot/robot.json b/data/robot/robot.json index 2c27a5c..4b90a63 100644 --- 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", diff --git a/data/simulation/debug/markers.json b/data/simulation/debug/markers.json index f4fe080..9775064 100644 --- 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 ] } ] \ No newline at end of file diff --git a/data/simulation/debug/render.png b/data/simulation/debug/render.png index e4716f8..daccf25 100644 Binary files a/data/simulation/debug/render.png and b/data/simulation/debug/render.png differ diff --git a/setup/generateSets/render_robot.py b/setup/generateSets/render_robot.py index da945bf..a60bea3 100644 --- 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