From afc2f8ecd286e652cd70dc5789186b7e089a41ab Mon Sep 17 00:00:00 2001 From: Lizongdi <1210855344@qq.com> Date: Thu, 28 May 2026 14:23:20 +0800 Subject: [PATCH] =?UTF-8?q?=E4=B8=8A=E4=BC=A0=E6=96=B0=E6=B5=81=E7=A8=8B?= =?UTF-8?q?=E4=B8=8B=E7=9A=84=E7=82=B9=E4=BD=8D=E5=92=8C=E4=BD=9C=E4=B8=9A?= =?UTF-8?q?=E6=B5=81=E7=A8=8B100?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- kelaifen/Kelaifen_V6.0/program_config.py | 4 +- kelaifen/Kelaifen_V6.0/trajectory_data.py | 389 +++++++++++++--------- 2 files changed, 234 insertions(+), 159 deletions(-) diff --git a/kelaifen/Kelaifen_V6.0/program_config.py b/kelaifen/Kelaifen_V6.0/program_config.py index 20dfb7e..d207089 100644 --- a/kelaifen/Kelaifen_V6.0/program_config.py +++ b/kelaifen/Kelaifen_V6.0/program_config.py @@ -2,7 +2,7 @@ import json from trajectory_program import ( program1, program1_1, program2, program3, program3_1, program4, program5, program5_1, program6, program7, program7_1, program8, - program9, program10, program11, program12 + program9, program10, program11, program12, program100 ) program_switch = { @@ -14,7 +14,7 @@ program_switch = { 4: program4, 5: program5, 500: program5_1, - 6: program6, + 6: program100, 7: program7, 700: program7_1, 8: program8, diff --git a/kelaifen/Kelaifen_V6.0/trajectory_data.py b/kelaifen/Kelaifen_V6.0/trajectory_data.py index 5741842..704d836 100644 --- a/kelaifen/Kelaifen_V6.0/trajectory_data.py +++ b/kelaifen/Kelaifen_V6.0/trajectory_data.py @@ -94,7 +94,7 @@ Pose_1 = [ [476.18, 958.93, 483.84, -3.1202, -0.030478, -0.91844], [461.77, 972.7, 485.5, -3.1202, -0.030478, -0.91844], [447.36, 986.46, 487.16, -3.1202, -0.030478, -0.91844], - [432.95, 1000.2, 488.82, -3.1202, -0.030478, -0.91844], + [432.95, 1.2, 488.82, -3.1202, -0.030478, -0.91844], [418.54, 1014, 490.48, -3.1202, -0.030478, -0.91844], [404.12, 1027.8, 492.14, -3.1202, -0.030478, -0.91844], [389.71, 1041.5, 493.8, -3.1202, -0.030478, -0.91844], @@ -317,161 +317,161 @@ Pose_2 = [ ] joint_positions_assembled_2 = [[92.442,-41.967,60.096,-110.133,90.041,11.741]] Pose_3 = [ - [1549.9, -66.747, 564.95, 3.1398, 0.034928, -1.5584], - [1549.9, -66.747, 564.95, 3.1398, 0.034929, -1.558], - [1549.9, -66.747, 564.95, 3.14, 0.034937, -1.5534], - [1549.9, -66.747, 564.95, 3.1403, 0.03495, -1.544], - [1549.9, -66.747, 564.95, 3.1408, 0.034964, -1.5301], - [1549.9, -66.747, 564.95, 3.1415, 0.034973, -1.5114], - [1549.9, -66.747, 564.95, -3.1409, 0.034966, -1.4882], - [1549.9, -66.747, 564.95, -3.1399, 0.034933, -1.4602], - [1549.9, -66.747, 564.95, -3.1388, 0.034861, -1.4276], - [1549.9, -66.747, 564.95, -3.1375, 0.034732, -1.3904], - [1549.9, -66.747, 564.95, -3.1361, 0.03453, -1.3484], - [1549.5, -66.359, 564.95, -3.1345, 0.034239, -1.3025], - [1547.7, -64.638, 564.84, -3.1329, 0.03387, -1.2559], - [1544.4, -61.541, 564.58, -3.1313, 0.033428, -1.2093], - [1539.7, -57.066, 564.14, -3.1298, 0.032914, -1.1628], - [1533.6, -51.215, 563.54, -3.1282, 0.032328, -1.1162], - [1526, -43.987, 562.77, -3.1268, 0.031672, -1.0696], - [1517, -35.383, 561.84, -3.1253, 0.030947, -1.0231], - [1506.6, -25.401, 560.74, -3.1239, 0.030156, -0.97652], - [1494.7, -14.043, 559.48, -3.1225, 0.029299, -0.92996], - [1481.4, -1.3082, 558.05, -3.1212, 0.028379, -0.88341], - [1467, 12.416, 556.45, -3.1205, 0.027896, -0.86013], - [1452.6, 26.184, 554.79, -3.1205, 0.027896, -0.86013], - [1438.2, 39.951, 553.13, -3.1205, 0.027896, -0.86013], - [1423.8, 53.719, 551.47, -3.1205, 0.027896, -0.86013], - [1409.3, 67.486, 549.81, -3.1205, 0.027896, -0.86013], - [1394.9, 81.254, 548.15, -3.1205, 0.027896, -0.86013], - [1380.5, 95.021, 546.49, -3.1205, 0.027896, -0.86013], - [1366.1, 108.79, 544.84, -3.1205, 0.027896, -0.86013], - [1351.7, 122.56, 543.17, -3.1205, 0.027896, -0.86013], - [1337.3, 136.32, 541.51, -3.1205, 0.027896, -0.86013], - [1322.9, 150.09, 539.86, -3.1205, 0.027896, -0.86013], - [1308.5, 163.86, 538.2, -3.1205, 0.027896, -0.86013], - [1294.1, 177.63, 536.54, -3.1205, 0.027896, -0.86013], - [1279.7, 191.39, 534.88, -3.1205, 0.027896, -0.86013], - [1265.2, 205.16, 533.22, -3.1205, 0.027896, -0.86013], - [1250.8, 218.93, 531.56, -3.1205, 0.027896, -0.86013], - [1236.4, 232.7, 529.9, -3.1205, 0.027896, -0.86013], - [1222, 246.46, 528.24, -3.1205, 0.027896, -0.86013], - [1204, 263.67, 526.58, -3.1205, 0.027896, -0.86013], - [1189.6, 277.44, 524.5, -3.1205, 0.027896, -0.86013], - [1175.2, 291.21, 522.84, -3.1205, 0.027896, -0.86013], - [1160.8, 304.98, 521.18, -3.1205, 0.027896, -0.86013], - [1146.3, 318.74, 519.52, -3.1205, 0.027896, -0.86013], - [1131.9, 332.51, 517.87, -3.1205, 0.027896, -0.86013], - [1117.5, 346.28, 516.21, -3.1205, 0.027896, -0.86013], - [1103.1, 360.05, 514.55, -3.1205, 0.027896, -0.86013], - [1088.7, 373.81, 512.89, -3.1205, 0.027896, -0.86013], - [1074.3, 387.58, 511.23, -3.1205, 0.027896, -0.86013], - [1059.9, 401.35, 509.57, -3.1205, 0.027896, -0.86013], - [1045.5, 415.11, 507.91, -3.1205, 0.027896, -0.86013], - [1031, 428.88, 506.25, -3.1205, 0.027896, -0.86013], - [1016.6, 442.65, 504.59, -3.1205, 0.027896, -0.86013], - [1002.2, 456.42, 502.93, -3.1205, 0.027896, -0.86013], - [987.8, 470.18, 501.27, -3.1205, 0.027896, -0.86013], - [973.39, 483.95, 499.61, -3.1205, 0.027896, -0.86013], - [958.98, 497.72, 497.95, -3.1205, 0.027896, -0.86013], - [944.57, 511.49, 496.29, -3.1205, 0.027896, -0.86013], - [930.16, 525.25, 494.63, -3.1205, 0.027896, -0.86013], - [915.75, 539.02, 492.97, -3.1205, 0.027896, -0.86013], - [901.33, 552.79, 491.31, -3.1205, 0.027896, -0.86013], - [886.92, 566.56, 489.65, -3.1205, 0.027896, -0.86013], - [872.51, 580.32, 487.99, -3.1205, 0.027896, -0.86013], - [858.1, 594.09, 486.33, -3.1205, 0.027896, -0.86013], - [843.69, 607.86, 484.67, -3.1205, 0.027896, -0.86013], - [829.27, 621.63, 483.01, -3.1205, 0.027896, -0.86013], - [814.86, 635.39, 481.35, -3.1205, 0.027896, -0.86013], - [800.45, 649.16, 479.69, -3.1205, 0.027896, -0.86013], - [786.04, 662.93, 478.03, -3.1205, 0.027896, -0.86013], - [771.63, 676.7, 476.37, -3.1205, 0.027896, -0.86013], - [757.22, 690.46, 474.72, -3.1205, 0.027896, -0.86013], - [742.8, 704.23, 473.06, -3.1205, 0.027896, -0.86013], - [728.39, 718, 471.39, -3.1205, 0.027896, -0.86013], - [713.98, 731.77, 469.74, -3.1205, 0.027896, -0.86013], - [699.57, 745.53, 468.08, -3.1205, 0.027896, -0.86013], - [685.15, 759.3, 466.42, -3.1205, 0.027896, -0.86013], - [670.74, 773.07, 464.76, -3.1205, 0.027896, -0.86013], - [656.33, 786.84, 463.1, -3.1205, 0.027896, -0.86013], - [641.92, 800.6, 461.44, -3.1205, 0.027896, -0.86013], - [627.51, 814.37, 459.78, -3.1205, 0.027896, -0.86013], - [613.1, 828.14, 458.12, -3.1205, 0.027896, -0.86013], - [598.68, 841.91, 456.46, -3.1205, 0.027896, -0.86013], - [584.27, 855.67, 454.8, -3.1205, 0.027896, -0.86013], - [569.86, 869.44, 453.14, -3.1205, 0.027896, -0.86013], - [555.45, 883.21, 451.48, -3.1205, 0.027896, -0.86013], - [541.04, 896.98, 449.82, -3.1205, 0.027896, -0.86013], - [526.62, 910.74, 448.16, -3.1205, 0.027896, -0.86013], - [512.21, 924.51, 446.5, -3.1205, 0.027896, -0.86013], - [497.8, 938.28, 444.84, -3.1205, 0.027896, -0.86013], - [483.39, 952.04, 443.18, -3.1205, 0.027896, -0.86013], - [468.98, 965.81, 441.52, -3.1205, 0.027896, -0.86013], - [454.56, 979.58, 439.86, -3.1205, 0.027896, -0.86013], - [440.15, 993.35, 438.2, -3.1205, 0.027896, -0.86013], - [425.74, 1007.1, 436.54, -3.1205, 0.027896, -0.86013], - [411.33, 1020.9, 434.88, -3.1205, 0.027896, -0.86013], - [396.92, 1034.7, 433.22, -3.1205, 0.027896, -0.86013], - [382.5, 1048.4, 431.56, -3.1205, 0.027896, -0.86013], - [368.09, 1062.2, 429.9, -3.1205, 0.027896, -0.86013], - [353.68, 1076, 428.25, -3.1205, 0.027896, -0.86013], - [339.27, 1089.7, 426.58, -3.1205, 0.027896, -0.86013], - [324.86, 1103.5, 424.93, -3.1205, 0.027896, -0.86013], - [310.45, 1117.2, 423.27, -3.1205, 0.027896, -0.86013], - [296.03, 1131, 421.61, -3.1205, 0.027896, -0.86013], - [281.62, 1144.8, 419.95, -3.1205, 0.027896, -0.86013], - [267.21, 1158.6, 418.29, -3.1205, 0.027896, -0.86013], - [252.8, 1172.3, 416.63, -3.1205, 0.027896, -0.86013], - [238.39, 1186.1, 414.97, -3.1205, 0.027896, -0.86013], - [223.97, 1199.9, 413.31, -3.1205, 0.027896, -0.86013], - [209.56, 1213.6, 411.65, -3.1205, 0.027896, -0.86013], - [195.15, 1227.4, 409.99, -3.1205, 0.027896, -0.86013], - [180.74, 1241.2, 408.33, -3.1205, 0.027896, -0.86013], - [166.33, 1254.9, 406.67, -3.1205, 0.027896, -0.86013], - [151.91, 1268.7, 405.01, -3.1205, 0.027896, -0.86013], - [137.5, 1282.5, 403.35, -3.1205, 0.027896, -0.86013], - [123.09, 1296.2, 401.69, -3.1205, 0.027896, -0.86013], - [105.08, 1313.4, 400.03, -3.1205, 0.027896, -0.86013], - [90.665, 1327.2, 397.96, -3.1205, 0.027896, -0.86013], - [76.253, 1341, 396.3, -3.1205, 0.027896, -0.86013], - [61.841, 1354.7, 394.64, -3.1205, 0.027896, -0.86013], - [47.429, 1368.5, 392.98, -3.1205, 0.027896, -0.86013], - [33.017, 1382.3, 391.32, -3.1205, 0.027896, -0.86013], - [18.605, 1396, 389.66, -3.1205, 0.027896, -0.86013], - [4.1933, 1409.8, 388, -3.1205, 0.027896, -0.86013], - [-10.219, 1423.6, 386.34, -3.1205, 0.027896, -0.86013], - [-24.631, 1437.3, 384.68, -3.1205, 0.027896, -0.86013], - [-39.042, 1451.1, 383.02, -3.1205, 0.027896, -0.86013], - [-53.454, 1464.9, 381.36, -3.1205, 0.027896, -0.86013], - [-67.866, 1478.7, 379.7, -3.1205, 0.027896, -0.86013], - [-82.278, 1492.4, 378.04, -3.1205, 0.027896, -0.86013], - [-96.69, 1506.2, 376.38, -3.1205, 0.027896, -0.86013], - [-111.1, 1520, 374.72, -3.1205, 0.027896, -0.86013], - [-125.51, 1533.7, 373.06, -3.1205, 0.027896, -0.86013], - [-139.93, 1547.5, 371.4, -3.1205, 0.027896, -0.86013], - [-154.34, 1561.3, 369.74, -3.1205, 0.027896, -0.86013], - [-168.19, 1574.5, 368.09, -3.12, 0.027526, -0.84279], - [-180.61, 1586.3, 366.55, -3.1182, 0.026015, -0.77558], - [-191.59, 1596.8, 365.18, -3.1166, 0.024474, -0.71188], - [-201.13, 1606, 363.98, -3.1152, 0.022926, -0.65167], - [-209.23, 1613.7, 362.94, -3.1139, 0.021392, -0.59495], - [-215.88, 1620, 362.07, -3.1128, 0.01989, -0.54173], - [-221.1, 1625, 361.37, -3.1119, 0.018435, -0.492], - [-224.87, 1628.6, 360.83, -3.1111, 0.017042, -0.44576], - [-227.2, 1630.9, 360.45, -3.1103, 0.01572, -0.40302], - [-228.09, 1631.7, 360.25, -3.1098, 0.014482, -0.36376], - [-228.1, 1631.7, 360.2, -3.1093, 0.013334, -0.328], - [-228.1, 1631.7, 360.2, -3.1088, 0.012284, -0.29572], - [-228.1, 1631.7, 360.2, -3.1085, 0.011336, -0.26694], - [-228.1, 1631.7, 360.2, -3.1082, 0.010495, -0.24164], - [-228.1, 1631.7, 360.2, -3.108, 0.0097651, -0.21984], - [-228.1, 1631.7, 360.2, -3.1078, 0.0091482, -0.20152], - [-228.1, 1631.7, 360.2, -3.1077, 0.0086465, -0.18669], - [-228.1, 1631.7, 360.2, -3.1076, 0.0082616, -0.17536], - [-228.1, 1631.7, 360.2, -3.1075, 0.0079945, -0.16751], - [-228.1, 1631.7, 360.2, -3.1075, 0.0078459, -0.16315], - [-228.1, 1631.7, 360.2, -3.1075, 0.0078124, -0.16216] + [1549.9, -66.747, 576.94, 3.1398, 0.034928, -1.5584], + [1549.9, -66.747, 576.94, 3.1398, 0.034929, -1.558], + [1549.9, -66.747, 576.94, 3.14, 0.034937, -1.5534], + [1549.9, -66.747, 576.94, 3.1403, 0.03495, -1.544], + [1549.9, -66.747, 576.94, 3.1408, 0.034964, -1.5301], + [1549.9, -66.747, 576.94, 3.1415, 0.034973, -1.5114], + [1549.9, -66.747, 576.94, -3.1409, 0.034966, -1.4882], + [1549.9, -66.747, 576.94, -3.1399, 0.034933, -1.4602], + [1549.9, -66.747, 576.94, -3.1388, 0.034861, -1.4276], + [1549.9, -66.747, 576.94, -3.1375, 0.034732, -1.3904], + [1549.9, -66.747, 576.94, -3.1361, 0.03453, -1.3484], + [1549.9, -66.737, 576.94, -3.1345, 0.034239, -1.3025], + [1549, -65.885, 576.84, -3.1329, 0.03387, -1.2559], + [1546.6, -63.657, 576.57, -3.1313, 0.033428, -1.2093], + [1542.9, -60.051, 576.13, -3.1298, 0.032914, -1.1628], + [1537.6, -55.069, 575.53, -3.1282, 0.032328, -1.1162], + [1531, -48.711, 574.77, -3.1268, 0.031672, -1.0696], + [1522.9, -40.975, 573.83, -3.1253, 0.030947, -1.0231], + [1513.3, -31.862, 572.74, -3.1239, 0.030156, -0.97652], + [1502.4, -21.373, 571.47, -3.1225, 0.029299, -0.92996], + [1490, -9.5074, 570.04, -3.1212, 0.028379, -0.88341], + [1476.1, 3.7255, 568.44, -3.1205, 0.027896, -0.86013], + [1461.7, 17.493, 566.79, -3.1205, 0.027896, -0.86013], + [1447.3, 31.261, 565.13, -3.1205, 0.027896, -0.86013], + [1432.9, 45.028, 563.47, -3.1205, 0.027896, -0.86013], + [1418.5, 58.796, 561.81, -3.1205, 0.027896, -0.86013], + [1404, 72.563, 560.15, -3.1205, 0.027896, -0.86013], + [1389.6, 86.331, 558.49, -3.1205, 0.027896, -0.86013], + [1375.2, 100.1, 556.83, -3.1205, 0.027896, -0.86013], + [1360.8, 113.87, 555.17, -3.1205, 0.027896, -0.86013], + [1346.4, 127.63, 553.51, -3.1205, 0.027896, -0.86013], + [1332, 141.4, 551.85, -3.1205, 0.027896, -0.86013], + [1317.6, 155.17, 550.19, -3.1205, 0.027896, -0.86013], + [1303.2, 168.94, 548.53, -3.1205, 0.027896, -0.86013], + [1288.7, 182.7, 546.87, -3.1205, 0.027896, -0.86013], + [1274.3, 196.47, 545.21, -3.1205, 0.027896, -0.86013], + [1259.9, 210.24, 543.55, -3.1205, 0.027896, -0.86013], + [1245.5, 224.01, 541.89, -3.1205, 0.027896, -0.86013], + [1231.1, 237.77, 540.23, -3.1205, 0.027896, -0.86013], + [1216.7, 251.54, 538.57, -3.1205, 0.027896, -0.86013], + [1198.7, 268.75, 536.5, -3.1205, 0.027896, -0.86013], + [1184.3, 282.52, 534.84, -3.1205, 0.027896, -0.86013], + [1169.8, 296.29, 533.18, -3.1205, 0.027896, -0.86013], + [1155.4, 310.05, 531.52, -3.1205, 0.027896, -0.86013], + [1141, 323.82, 529.86, -3.1205, 0.027896, -0.86013], + [1126.6, 337.59, 528.2, -3.1205, 0.027896, -0.86013], + [1112.2, 351.36, 526.54, -3.1205, 0.027896, -0.86013], + [1097.8, 365.12, 524.88, -3.1205, 0.027896, -0.86013], + [1083.4, 378.89, 523.22, -3.1205, 0.027896, -0.86013], + [1069, 392.66, 521.56, -3.1205, 0.027896, -0.86013], + [1054.5, 406.43, 519.9, -3.1205, 0.027896, -0.86013], + [1040.1, 420.19, 518.24, -3.1205, 0.027896, -0.86013], + [1025.7, 433.96, 516.58, -3.1205, 0.027896, -0.86013], + [1011.3, 447.73, 514.92, -3.1205, 0.027896, -0.86013], + [996.9, 461.5, 513.26, -3.1205, 0.027896, -0.86013], + [982.49, 475.26, 511.6, -3.1205, 0.027896, -0.86013], + [968.08, 489.03, 509.94, -3.1205, 0.027896, -0.86013], + [953.67, 502.8, 508.28, -3.1205, 0.027896, -0.86013], + [939.26, 516.57, 506.62, -3.1205, 0.027896, -0.86013], + [924.84, 530.33, 504.96, -3.1205, 0.027896, -0.86013], + [910.43, 544.1, 503.3, -3.1205, 0.027896, -0.86013], + [896.02, 557.87, 501.64, -3.1205, 0.027896, -0.86013], + [881.61, 571.63, 499.99, -3.1205, 0.027896, -0.86013], + [867.2, 585.4, 498.32, -3.1205, 0.027896, -0.86013], + [852.78, 599.17, 496.66, -3.1205, 0.027896, -0.86013], + [838.37, 612.94, 495.01, -3.1205, 0.027896, -0.86013], + [823.96, 626.71, 493.35, -3.1205, 0.027896, -0.86013], + [809.55, 640.47, 491.69, -3.1205, 0.027896, -0.86013], + [795.14, 654.24, 490.03, -3.1205, 0.027896, -0.86013], + [780.73, 668.01, 488.37, -3.1205, 0.027896, -0.86013], + [766.31, 681.77, 486.71, -3.1205, 0.027896, -0.86013], + [751.9, 695.54, 485.05, -3.1205, 0.027896, -0.86013], + [737.49, 709.31, 483.39, -3.1205, 0.027896, -0.86013], + [723.08, 723.08, 481.73, -3.1205, 0.027896, -0.86013], + [708.67, 736.85, 480.07, -3.1205, 0.027896, -0.86013], + [694.25, 750.61, 478.41, -3.1205, 0.027896, -0.86013], + [679.84, 764.38, 476.75, -3.1205, 0.027896, -0.86013], + [665.43, 778.15, 475.09, -3.1205, 0.027896, -0.86013], + [651.02, 791.91, 473.43, -3.1205, 0.027896, -0.86013], + [636.61, 805.68, 471.77, -3.1205, 0.027896, -0.86013], + [622.2, 819.45, 470.11, -3.1205, 0.027896, -0.86013], + [607.78, 833.22, 468.45, -3.1205, 0.027896, -0.86013], + [593.37, 846.99, 466.79, -3.1205, 0.027896, -0.86013], + [578.96, 860.75, 465.13, -3.1205, 0.027896, -0.86013], + [564.55, 874.52, 463.47, -3.1205, 0.027896, -0.86013], + [550.14, 888.29, 461.81, -3.1205, 0.027896, -0.86013], + [535.72, 902.05, 460.15, -3.1205, 0.027896, -0.86013], + [521.31, 915.82, 458.49, -3.1205, 0.027896, -0.86013], + [506.9, 929.59, 456.83, -3.1205, 0.027896, -0.86013], + [492.49, 943.36, 455.17, -3.1205, 0.027896, -0.86013], + [478.08, 957.12, 453.51, -3.1205, 0.027896, -0.86013], + [463.67, 970.89, 451.86, -3.1205, 0.027896, -0.86013], + [449.25, 984.66, 450.19, -3.1205, 0.027896, -0.86013], + [434.84, 998.43, 448.54, -3.1205, 0.027896, -0.86013], + [420.43, 1012.2, 446.88, -3.1205, 0.027896, -0.86013], + [406.02, 1026, 445.22, -3.1205, 0.027896, -0.86013], + [391.61, 1039.7, 443.56, -3.1205, 0.027896, -0.86013], + [377.19, 1053.5, 441.9, -3.1205, 0.027896, -0.86013], + [362.78, 1067.3, 440.24, -3.1205, 0.027896, -0.86013], + [348.37, 1081, 438.58, -3.1205, 0.027896, -0.86013], + [333.96, 1094.8, 436.92, -3.1205, 0.027896, -0.86013], + [319.55, 1108.6, 435.26, -3.1205, 0.027896, -0.86013], + [305.13, 1122.3, 433.6, -3.1205, 0.027896, -0.86013], + [290.72, 1136.1, 431.94, -3.1205, 0.027896, -0.86013], + [276.31, 1149.9, 430.28, -3.1205, 0.027896, -0.86013], + [261.9, 1163.6, 428.62, -3.1205, 0.027896, -0.86013], + [247.49, 1177.4, 426.96, -3.1205, 0.027896, -0.86013], + [233.07, 1191.2, 425.3, -3.1205, 0.027896, -0.86013], + [218.66, 1204.9, 423.64, -3.1205, 0.027896, -0.86013], + [204.25, 1218.7, 421.98, -3.1205, 0.027896, -0.86013], + [189.84, 1232.5, 420.32, -3.1205, 0.027896, -0.86013], + [175.43, 1246.2, 418.66, -3.1205, 0.027896, -0.86013], + [161.02, 1260, 417, -3.1205, 0.027896, -0.86013], + [146.6, 1273.8, 415.34, -3.1205, 0.027896, -0.86013], + [132.19, 1287.5, 413.68, -3.1205, 0.027896, -0.86013], + [117.78, 1301.3, 412.02, -3.1205, 0.027896, -0.86013], + [99.766, 1318.5, 409.95, -3.1205, 0.027896, -0.86013], + [85.354, 1332.3, 408.29, -3.1205, 0.027896, -0.86013], + [70.942, 1346.1, 406.63, -3.1205, 0.027896, -0.86013], + [56.53, 1359.8, 404.97, -3.1205, 0.027896, -0.86013], + [42.118, 1373.6, 403.31, -3.1205, 0.027896, -0.86013], + [27.706, 1387.4, 401.65, -3.1205, 0.027896, -0.86013], + [13.295, 1401.1, 399.99, -3.1205, 0.027896, -0.86013], + [-1.1172, 1414.9, 398.33, -3.1205, 0.027896, -0.86013], + [-15.529, 1428.7, 396.67, -3.1205, 0.027896, -0.86013], + [-29.941, 1442.4, 395.01, -3.1205, 0.027896, -0.86013], + [-44.353, 1456.2, 393.35, -3.1205, 0.027896, -0.86013], + [-58.765, 1470, 391.69, -3.1205, 0.027896, -0.86013], + [-73.176, 1483.7, 390.03, -3.1205, 0.027896, -0.86013], + [-87.588, 1497.5, 388.37, -3.1205, 0.027896, -0.86013], + [-102, 1511.3, 386.71, -3.1205, 0.027896, -0.86013], + [-116.41, 1525, 385.05, -3.1205, 0.027896, -0.86013], + [-130.82, 1538.8, 383.39, -3.1205, 0.027896, -0.86013], + [-145.24, 1552.6, 381.74, -3.1205, 0.027896, -0.86013], + [-159.6, 1566.3, 380.08, -3.12, 0.027526, -0.84279], + [-172.93, 1579, 378.55, -3.1182, 0.026015, -0.77558], + [-184.82, 1590.4, 377.18, -3.1166, 0.024474, -0.71188], + [-195.27, 1600.4, 375.97, -3.1152, 0.022926, -0.65167], + [-204.28, 1609, 374.94, -3.1139, 0.021392, -0.59495], + [-211.84, 1616.2, 374.06, -3.1128, 0.01989, -0.54173], + [-217.97, 1622, 373.36, -3.1119, 0.018435, -0.492], + [-222.65, 1626.5, 372.82, -3.1111, 0.017042, -0.44576], + [-225.9, 1629.6, 372.45, -3.1103, 0.01572, -0.40302], + [-227.7, 1631.3, 372.24, -3.1098, 0.014482, -0.36376], + [-228.1, 1631.7, 372.19, -3.1093, 0.013334, -0.328], + [-228.1, 1631.7, 372.19, -3.1088, 0.012284, -0.29572], + [-228.1, 1631.7, 372.19, -3.1085, 0.011336, -0.26694], + [-228.1, 1631.7, 372.19, -3.1082, 0.010495, -0.24164], + [-228.1, 1631.7, 372.19, -3.108, 0.0097651, -0.21984], + [-228.1, 1631.7, 372.19, -3.1078, 0.0091482, -0.20152], + [-228.1, 1631.7, 372.19, -3.1077, 0.0086465, -0.18669], + [-228.1, 1631.7, 372.19, -3.1076, 0.0082616, -0.17536], + [-228.1, 1631.7, 372.19, -3.1075, 0.0079945, -0.16751], + [-228.1, 1631.7, 372.19, -3.1075, 0.0078459, -0.16315], + [-228.1, 1631.7, 372.19, -3.1075, 0.0078124, -0.16216] ] joint_positions_assembled_3 = [[-8.437,-52.830,64.769,-101.519,91.959,-9.155]] Pose_4 = [ @@ -631,4 +631,79 @@ Pose_4 = [ [1549.9, -66.747, 360.2, 3.1398, 0.034929, -1.558], [1549.9, -66.747, 360.2, 3.1398, 0.034928, -1.5584] ] -joint_positions_assembled_4 = [[92.553,-45.178,53.196,-97.284,97.995,15.177]] \ No newline at end of file +joint_positions_assembled_4 = [[92.553,-45.178,53.196,-97.284,97.995,15.177]] + +Pose_3_4 = [ + [-228.1, 1631.7, 370.4375, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 370.4375, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 370.4375, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 370.4375, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 370.4375, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 370.4375, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 380.6750, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 390.9125, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 401.1500, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 411.3875, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 421.6250, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 431.8625, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 442.1, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 452.3375, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 462.5750, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 472.8125, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 483.0500, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 493.2875, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 503.5250, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 513.7625, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 524.0, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 534.2375, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 544.4750, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 554.7125, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 564.9500, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 564.9500, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 564.9500, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 564.9500, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 564.9500, -3.1075, 0.0078, -0.1622], + [-228.1, 1631.7, 564.9500, -3.1075, 0.0078, -0.1622] +] + +Pose_4_3 = [ + [1549.9, -66.7470, 370.4375, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 370.4375, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 370.4375, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 370.4375, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 370.4375, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 370.4375, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 380.6750, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 390.9125, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 401.1500, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 411.3875, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 421.6250, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 431.8625, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 442.1, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 452.3375, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 462.5750, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 472.8125, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 483.0500, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 493.2875, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 503.5250, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 513.7625, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 524.0, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 534.2375, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 544.4750, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 554.7125, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584], + [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584] +] + + + + +