From ea1416a776715030ec42bfd701654f6eb08fa695 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 14 Nov 2021 13:55:58 -0500 Subject: [PATCH 1/4] Fixed spelling --- python/tests/prototype/chain.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/tests/prototype/chain.py b/python/tests/prototype/chain.py index bad246f3..50817156 100644 --- a/python/tests/prototype/chain.py +++ b/python/tests/prototype/chain.py @@ -21,7 +21,7 @@ def compose(aSbj: Tuple[Pose3, np.ndarray], bSck: Tuple[Pose3, np.ndarray]): aTb, bAj = aSbj bTc, cAk = bSck assert bAj.shape[0] == 6 and cAk.shape[0] == 6,\ - f"Jaocobains should have 6 rows, sapes are {bAj.shape} and {cAk.shape}" + f"Jaocobians should have 6 rows, shapes are {bAj.shape}, {cAk.shape}" # Compose poses: aTc = aTb.compose(bTc) From 32aeee482aefcaf2ec8722fc9cf6d393dc101509 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 15 Nov 2021 22:11:11 -0500 Subject: [PATCH 2/4] Notebook for Panda --- python/tests/panda.ipynb | 1685 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 1685 insertions(+) create mode 100644 python/tests/panda.ipynb diff --git a/python/tests/panda.ipynb b/python/tests/panda.ipynb new file mode 100644 index 00000000..d2866ef7 --- /dev/null +++ b/python/tests/panda.ipynb @@ -0,0 +1,1685 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Panda IK\n", + "Do Panda IK with shoulder partitioning, using a roadmap IK module for the 3-DOF arm." + ] + }, + { + "cell_type": "code", + "execution_count": 16, + "metadata": {}, + "outputs": [], + "source": [ + "from pathlib import Path\n", + "import numpy as np\n", + "from typing import Optional\n", + "\n", + "import gtdynamics as gtd\n", + "from gtsam import Pose3, Values, Rot3, Point3\n", + "\n", + "import plotly.express as px\n", + "\n", + "import roboplot # type: ignore\n", + "%matplotlib widget\n", + "\n", + "from prototype.chain import Chain" + ] + }, + { + "cell_type": "code", + "execution_count": 17, + "metadata": {}, + "outputs": [], + "source": [ + "model_file = Path(gtd.URDF_PATH) / \"panda\" / \"panda.urdf\"\n", + "base_name = \"link0\"\n", + "# Crucial to fix base link or FK gives wrong result\n", + "robot = gtd.CreateRobotFromFile(str(model_file)).fixLink(base_name)" + ] + }, + { + "cell_type": "code", + "execution_count": 18, + "metadata": {}, + "outputs": [], + "source": [ + "# Construct shoulder:\n", + "shoulder = Chain.from_robot(robot, base_name, (0, 3))\n" + ] + }, + { + "cell_type": "code", + "execution_count": 19, + "metadata": {}, + "outputs": [], + "source": [ + "# Sample a few configurations:\n", + "N = 200\n", + "theta = 2 * np.pi * (np.random.random_sample((N, 3)) - 0.5)" + ] + }, + { + "cell_type": "code", + "execution_count": 20, + "metadata": {}, + "outputs": [ + { + "data": { + "application/vnd.plotly.v1+json": { + "config": { + "plotlyServerURL": "https://plot.ly" + }, + "data": [ + { + "hovertemplate": "x=%{x}
y=%{y}
z=%{z}", + "legendgroup": "", + "marker": { + "color": "#636efa", + "symbol": "circle" + }, + "mode": "markers", + "name": "", + "scene": "scene", + "showlegend": false, + "type": "scatter3d", + "x": [ + 0.1500642655603171, + 0.03495139605267255, + 0.17657161542870764, + 0.05483517100858217, + -0.10149406667219099, + -0.07876498771569276, + -0.20126612014121192, + 0.021944552356212343, + 0.2218987573590024, + 0.21557051135920083, + -0.01531576841323546, + 0.16540605903135894, + 0.14874102793752408, + 0.09900466215720122, + 0.12514477622672626, + 0.030100582902938836, + -0.13344556221873427, + 0.0008134405736729272, + 0.17355140965562083, + 0.08191521476688898, + -0.15174486740328824, + 0.02209076892582329, + 0.24685769160133464, + 0.041359015463585294, + -0.059877940260079844, + -0.030860870718399186, + 0.11091030616417734, + -0.20829161213879324, + 0.2537126613755947, + 0.0030697060332432503, + 0.2192757107398812, + 0.05489695020901817, + 0.08391243022200068, + -0.188587370011046, + -0.03365876316146201, + -0.014344847141831568, + 0.16253717320608066, + -0.03925905817262866, + 0.05054693587313683, + -0.19261250382817335, + 0.03949205159267791, + -0.094655977055026, + 0.10084890894410468, + 0.0070430140032235605, + -0.052776917743337856, + -0.06322715275387808, + 0.22730465766451544, + -0.006599781358974252, + 0.2073820872781236, + -0.09382431482137125, + -0.041080185755639605, + 0.056145758722311465, + 0.015215855440855033, + 0.038807361658544814, + 0.2136713868349819, + -0.19285885862910065, + -0.03652152212318867, + -0.05066000792968933, + 0.01920584703061529, + 0.22015663592168308, + 0.02728198129061525, + 0.2048495190931698, + -0.005678395606938336, + 0.24029681276654954, + -0.2155571993817883, + 0.2270662612994212, + -0.18041762508030212, + -0.11599981201212396, + -0.0640032032320543, + -0.028487948850901355, + 0.029075434258580765, + 0.13128205537913978, + -0.01930499237149294, + -0.01903323262395118, + 0.18494158251258497, + -0.16312905385628293, + 0.14301478001026902, + -0.00975141744393504, + -0.13777065807948252, + 0.1664923405635369, + -0.2286728532380336, + -0.0689231496998687, + -0.09703984159265808, + -0.002878638559664373, + -0.03018870500684162, + 0.005601741098185101, + 0.07559384713588055, + 0.23044320680638877, + 0.12816669516888, + 0.030675730036993715, + -0.030374677012523485, + 0.08354165339272113, + 0.0695816172916637, + -0.2270595495331493, + 0.11152924011126006, + -0.04461656568824396, + -0.17928918635214292, + 0.1539539533366538, + -0.06462326067448063, + -0.05328933349048401, + -0.04351311673901307, + -0.025672126717933313, + 0.2447331461571833, + -0.06325688595308082, + 0.11779237486777157, + -0.09625704685752111, + -0.027861690701714396, + -0.2461159330612999, + -0.18536744003806704, + -0.16877572572325772, + -0.17700359171957006, + -0.1415277919691853, + 0.21792262835356738, + -0.08334403894259484, + -0.05623470659743836, + 0.16948628919694186, + 0.009002369185819514, + 0.24524158558784895, + -0.1652635884775549, + 0.14081296148166503, + -0.11946578389790634, + -0.2518229753324598, + 0.019908381690474153, + -0.181087193931011, + -0.07603470715834096, + 0.13852480918409926, + 0.012823277616428971, + -0.1912107273903323, + -0.16128319608412772, + 0.14038928675377493, + 0.03453219856112949, + -0.048050524143181554, + 0.010936551602807806, + 0.0998206837536095, + 0.13071575232419097, + -0.07235774384617297, + -0.10665939086475172, + 0.0279836152045261, + 0.22051938029539866, + -0.09968385977053071, + -0.13144321158142735, + -0.06558313562423414, + 0.1431563681743743, + 0.18504671535829317, + 0.10939707187901501, + 0.14574788643736936, + 0.2505688155360533, + 0.09395443772757764, + 0.15472701717916612, + 0.168431384466765, + 0.10081654076517751, + -0.23068516573518055, + 0.04945589655845959, + 0.07772706397379325, + -0.011341590073822592, + 0.20772086327189215, + -0.1909033165595981, + 0.16812599763490202, + 0.025078931574348345, + 0.037640845758298055, + -0.23459608729801482, + 0.0560610936135096, + 0.10762389353247012, + 0.0210644533482479, + -0.1359858680217786, + -0.0356167209134262, + 0.024522947620054338, + 0.137388857260191, + -0.18447217937636984, + -0.09766875703200233, + -0.0837841838434764, + 0.013759764893819193, + -0.09630422100052588, + -0.022675431946046452, + 0.11173344998983095, + -0.05240482764194865, + -0.24684096578138956, + 0.037926728551984944, + -0.008952354854107072, + -0.06815879812949574, + 0.001116863791188305, + -0.003963608850620401, + 0.15880245602062623, + 0.04375831004462006, + -0.0899866822944631, + 0.01823142085424452, + 0.07902802690537622, + -0.020487319037828553, + -0.021311919604441745, + 0.08253340293738202, + 0.23468326806256395, + -0.061481669267882524, + -0.028406877915086365, + -0.08151204615600052, + 0.009151259934998004, + 0.21309677927546722, + -0.06903468771240431, + -0.040246557023328786, + -0.06539223008903998, + 0.14228013087210706 + ], + "y": [ + -0.0982499409067093, + 0.024079774104015024, + -0.034047632958542365, + 0.13463553014049018, + 0.05143112139886061, + -0.1419325394856221, + 0.13467753140153177, + -0.06012177255386561, + -0.12352873464216199, + 0.05392553721733984, + -0.016096685084267093, + 0.05398458085112011, + -0.09503606356048927, + 0.2294826826217347, + 0.0014915295491537695, + -0.2459410504546361, + 0.08576848774646781, + -0.07760236701488904, + 0.15448487416934206, + -0.23414537731816748, + -0.1287367482301743, + -0.007811025714359612, + -0.05975813030427617, + -0.04059424415091987, + -0.03286576414376887, + 0.08389421896114206, + 0.09456644424391476, + 0.13126748528703505, + 0.013044151672840922, + 0.2529037058084706, + 0.0981991289432279, + -0.030233170822614494, + -0.09642933747924273, + 0.1701822117587314, + 0.03629351965623594, + 0.18207197510835518, + -0.1314097195874911, + -0.05315562937166229, + -0.0674399082841291, + 0.15493728254918204, + -0.024626031612258424, + -0.0073859910309683335, + -0.1280423924630288, + -0.020056563965160007, + 0.012366314272107104, + -0.20141023916530476, + 0.11289246409539357, + -0.040705767335091264, + 0.003570956840123471, + -0.16707296003107924, + -0.17679686603005879, + -0.2471709465028105, + 0.10794531851342579, + 0.09538048640254586, + -0.13542928263338508, + 0.027959243693105054, + 0.06956300776801948, + 0.009567643336599076, + 0.08116037516342914, + 0.11795506685596696, + -0.06291187914646688, + -0.12707165639990334, + -0.035922077736004226, + 0.07931263092248429, + -0.06173877177255382, + 0.10117580299540439, + 0.14938294702041455, + 0.22072689621259933, + -0.11789704559725202, + -0.20676559766145472, + 0.23981457821232674, + 0.157252227910279, + -0.24512247697704587, + 0.11489567511138815, + -0.15881332086426797, + -0.11449532073408784, + -0.14603373484305568, + 0.21858355629439147, + 0.06892755682548626, + 0.1689001530349564, + 0.037617111232872175, + 0.07610555267303845, + 0.057455780092064916, + 0.19225121022182412, + -0.23623608775333035, + -0.03854355023273838, + 0.014105213234580652, + -0.01998381541035622, + -0.009151254555232975, + 0.24139748643683853, + 0.09740841223373775, + 0.216035718423505, + 0.13350666568247727, + -0.09441041880771552, + 0.15037073438568488, + -0.24987094303530955, + -0.06979618276448056, + -0.1425139993602383, + -0.24498228172720204, + 0.07275619643530584, + 0.21833704278791774, + 0.022395905837873098, + -0.06765076732549982, + -0.06059850722644813, + 0.20566304540075642, + -0.10239152646797568, + 0.11753432889571133, + 0.0629298744231974, + 0.07054458746655304, + -0.0358523277676974, + 0.09033139826199632, + -0.16437367855259347, + 0.08831007096569848, + 0.12748114017702833, + -0.02248848314795057, + -0.17476932589550642, + 0.23147712445522436, + 0.02929284932019894, + 0.08640670970214964, + -0.07494710790721558, + -0.17162885861843166, + 0.018686033145834655, + 0.21879118725790925, + -0.11941223696416971, + 0.1860969438627483, + -0.15378518792703358, + -0.04439014161266406, + 0.150416504768064, + 0.021747965539256543, + 0.0073015589955438606, + -0.06808497605446845, + -0.08071401904672126, + 0.043432157762796735, + -0.02925091639630921, + -0.03793838929140977, + 0.2244390815310654, + -0.042248837376439946, + -0.020663748246959823, + -0.11968365532269211, + -0.06844117352616111, + -0.13893826561023018, + 0.11088846983073908, + -0.19194279106900466, + 0.17395104227600452, + -0.041206966379603256, + 0.04325096413303019, + 0.0298871396281951, + 0.11683695362450902, + -0.16196900694610958, + -0.037816337881599396, + 0.1650141465486068, + 0.10308991929683861, + -0.08874924968124558, + -0.1203228169183977, + -0.009577792101542148, + -0.02684949425552752, + -0.15669386673296964, + -0.1012631602118302, + 0.135600940032313, + 0.09916416685109906, + 0.06323774200499532, + 0.014361080623027475, + 0.14604349650666423, + -0.2223544642307573, + -0.12122820550312571, + 0.1216408590055733, + 0.05636443246070054, + -0.14353308063387332, + 0.02523673768766896, + 0.05192575534308008, + -0.04689354335022172, + -0.00010360659153167695, + 0.07917434232204261, + 0.024643272935399627, + -0.22773944929979972, + -0.22436058423325775, + -0.037190451197098504, + -0.07085290268315302, + 0.09615744114323516, + -0.1549398214845134, + 0.03868991536784084, + -0.01708778451226952, + 0.16197233878463146, + -0.027747007041473603, + 0.0458511754311179, + 0.22421894572931783, + 0.2357379976950944, + 0.03496909592193581, + 0.01838991672105568, + -0.14061976283115207, + 0.09314726848096552, + 0.03597705598714745, + 0.04006380989387247, + -0.23278381728826328, + -0.13308955746247236, + -0.13595131687369855, + -0.18038700388838105, + -0.03162870573146629, + -0.23607696512847312, + 0.14745060689060407 + ], + "z": [ + 0.5129302007607308, + 0.08250892984940092, + 0.1535273415064812, + 0.5413592807540979, + 0.5601585481859045, + 0.5284442190937841, + 0.40981876887037955, + 0.087132106774891, + 0.3260139401388647, + 0.20983911781647893, + 0.5860880001091248, + 0.5181316085867409, + 0.515733316784762, + 0.37863975328293137, + 0.11190325159593792, + 0.38916196615302606, + 0.5314521611513912, + 0.09108178493543125, + 0.435769371368567, + 0.3878912716411359, + 0.4909479556817168, + 0.5859786505567431, + 0.3391202771009813, + 0.08563639917369074, + 0.08829265305401746, + 0.5708162705021389, + 0.5410944579792442, + 0.270304996601606, + 0.33563579150685485, + 0.3089686803254761, + 0.4155972492660154, + 0.08678974011687692, + 0.5525616058409403, + 0.337475545235697, + 0.582192856890553, + 0.5096103991062677, + 0.18856882346257833, + 0.08768302698432119, + 0.09332505788816509, + 0.3916691884826053, + 0.5827621834068228, + 0.5686542063620819, + 0.13811065927308186, + 0.5861705789139655, + 0.5812113677026067, + 0.47436284712083526, + 0.3446230799120046, + 0.5836924315243054, + 0.1862787337319285, + 0.16617423518326266, + 0.5107709153156488, + 0.31563944659421783, + 0.5624852888234678, + 0.10074203280216891, + 0.30953386087260426, + 0.16999400878419751, + 0.5746079017830255, + 0.08422452767727105, + 0.09301804072975653, + 0.2864718397671988, + 0.5776323788693286, + 0.4132289255755501, + 0.5844449681070603, + 0.355675100491847, + 0.45245974086178603, + 0.38545552115486903, + 0.43139424542058324, + 0.3816918138251079, + 0.11724298748925482, + 0.18814270392606952, + 0.4116816308401989, + 0.18272035871563058, + 0.26905079842504875, + 0.5587962172593507, + 0.40456921129896123, + 0.175434314808576, + 0.18210887097563136, + 0.4621253290697692, + 0.5350283383942042, + 0.42410571151423143, + 0.4371195993384063, + 0.10061076591279837, + 0.5606605242671776, + 0.16693255153340664, + 0.42147804679655604, + 0.0819417855073536, + 0.09085577630897385, + 0.43808942568390763, + 0.11382708967191299, + 0.4060305755197086, + 0.5656718548822229, + 0.2286133516884237, + 0.12835010240533665, + 0.3968580113880779, + 0.16125285250586668, + 0.3440038958915121, + 0.4989249600293913, + 0.47630091440823175, + 0.3141657850841171, + 0.570516303963203, + 0.21059593041780264, + 0.08123308965726384, + 0.34173342340076657, + 0.5714818633219789, + 0.4245143705191451, + 0.12134516762031503, + 0.5565097750756105, + 0.3292596483605198, + 0.49177526628793883, + 0.14651521018490815, + 0.1747054048979891, + 0.20071852083025687, + 0.42921991087949957, + 0.12965413231699607, + 0.5797369808758414, + 0.26035502541680816, + 0.22867114476269088, + 0.27345417303411323, + 0.16046293536170633, + 0.13525811590110287, + 0.4772867741648544, + 0.36098630186525565, + 0.4605981369322704, + 0.20072998189590738, + 0.4883507728474425, + 0.48033704573818586, + 0.08317548016531404, + 0.2597842595060135, + 0.13790523203417346, + 0.12137592945708367, + 0.09067960232308878, + 0.5690584802350781, + 0.5830824890772421, + 0.1012082253038151, + 0.547525715344772, + 0.4275445261181587, + 0.5596848515289142, + 0.5846687080678936, + 0.3729279024595375, + 0.5564415934864443, + 0.16576946358284156, + 0.5519744537895791, + 0.4179201450759767, + 0.3397793700544539, + 0.5585692319844356, + 0.5365537264034037, + 0.30351713382795065, + 0.5381070494422086, + 0.21311337532609154, + 0.5194081681364219, + 0.16821660162860824, + 0.35953365186413033, + 0.5658623988721004, + 0.5428288232981616, + 0.07937262358905318, + 0.1892002800700777, + 0.3925832836201917, + 0.4943277196740823, + 0.5463791088308765, + 0.5638610980117753, + 0.25875125831021906, + 0.5803825478450079, + 0.5108640112588556, + 0.4540867163388579, + 0.510084097876582, + 0.11281334635434852, + 0.0864865366247031, + 0.17466564334762724, + 0.16014071057956714, + 0.5617176024356912, + 0.5682199352984645, + 0.07931147056339397, + 0.5543687700817402, + 0.08115531429639805, + 0.3189432856668233, + 0.22593331930906138, + 0.2857388131341466, + 0.09198414725403999, + 0.0980089072166702, + 0.14353960364304258, + 0.5840957086371251, + 0.07954485750460018, + 0.4474288130784918, + 0.08427821620478887, + 0.09987497227193178, + 0.45107070551899175, + 0.38524346119964326, + 0.5838079821714491, + 0.080502822040052, + 0.5278371923761704, + 0.3611879743534135, + 0.5768705960518319, + 0.5822692364813605, + 0.2720517221546613, + 0.5492189537665277, + 0.30742239388394077, + 0.1679486366232935, + 0.5818514646398306, + 0.4003701314520298, + 0.18279382596382307 + ] + } + ], + "layout": { + "legend": { + "tracegroupgap": 0 + }, + "margin": { + "t": 60 + }, + "scene": { + "domain": { + "x": [ + 0, + 1 + ], + "y": [ + 0, + 1 + ] + }, + "xaxis": { + "title": { + "text": "x" + } + }, + "yaxis": { + "title": { + "text": "y" + } + }, + "zaxis": { + "title": { + "text": "z" + } + } + }, + "template": { + "data": { + "bar": [ + { + "error_x": { + "color": "#2a3f5f" + }, + "error_y": { + "color": "#2a3f5f" + }, + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "bar" + } + ], + "barpolar": [ + { + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "barpolar" + } + ], + "carpet": [ + { + "aaxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "baxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "type": "carpet" + } + ], + "choropleth": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "choropleth" + } + ], + "contour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "contour" + } + ], + "contourcarpet": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "contourcarpet" + } + ], + "heatmap": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "heatmap" + } + ], + "heatmapgl": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "heatmapgl" + } + ], + "histogram": [ + { + "marker": { + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "histogram" + } + ], + "histogram2d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2d" + } + ], + "histogram2dcontour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2dcontour" + } + ], + "mesh3d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "mesh3d" + } + ], + "parcoords": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "parcoords" + } + ], + "pie": [ + { + "automargin": true, + "type": "pie" + } + ], + "scatter": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatter" + } + ], + "scatter3d": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatter3d" + } + ], + "scattercarpet": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattercarpet" + } + ], + "scattergeo": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergeo" + } + ], + "scattergl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergl" + } + ], + "scattermapbox": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermapbox" + } + ], + "scatterpolar": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolar" + } + ], + "scatterpolargl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolargl" + } + ], + "scatterternary": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterternary" + } + ], + "surface": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "surface" + } + ], + "table": [ + { + "cells": { + "fill": { + "color": "#EBF0F8" + }, + "line": { + "color": "white" + } + }, + "header": { + "fill": { + "color": "#C8D4E3" + }, + "line": { + "color": "white" + } + }, + "type": "table" + } + ] + }, + "layout": { + "annotationdefaults": { + "arrowcolor": "#2a3f5f", + "arrowhead": 0, + "arrowwidth": 1 + }, + "autotypenumbers": "strict", + "coloraxis": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "colorscale": { + "diverging": [ + [ + 0, + "#8e0152" + ], + [ + 0.1, + "#c51b7d" + ], + [ + 0.2, + "#de77ae" + ], + [ + 0.3, + "#f1b6da" + ], + [ + 0.4, + "#fde0ef" + ], + [ + 0.5, + "#f7f7f7" + ], + [ + 0.6, + "#e6f5d0" + ], + [ + 0.7, + "#b8e186" + ], + [ + 0.8, + "#7fbc41" + ], + [ + 0.9, + "#4d9221" + ], + [ + 1, + "#276419" + ] + ], + "sequential": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "sequentialminus": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ] + }, + "colorway": [ + "#636efa", + "#EF553B", + "#00cc96", + "#ab63fa", + "#FFA15A", + "#19d3f3", + "#FF6692", + "#B6E880", + "#FF97FF", + "#FECB52" + ], + "font": { + "color": "#2a3f5f" + }, + "geo": { + "bgcolor": "white", + "lakecolor": "white", + "landcolor": "#E5ECF6", + "showlakes": true, + "showland": true, + "subunitcolor": "white" + }, + "hoverlabel": { + "align": "left" + }, + "hovermode": "closest", + "mapbox": { + "style": "light" + }, + "paper_bgcolor": "white", + "plot_bgcolor": "#E5ECF6", + "polar": { + "angularaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "radialaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "scene": { + "xaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "yaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "zaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + } + }, + "shapedefaults": { + "line": { + "color": "#2a3f5f" + } + }, + "ternary": { + "aaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "baxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "caxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "title": { + "x": 0.05 + }, + "xaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + }, + "yaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + } + } + } + } + } + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "# Let's plot end-effector position which is child link of last shoulder joint:\n", + "poses = [shoulder.poe(q) for q in theta]\n", + "ts = np.array([pose.translation() for pose in poses])\n", + "px.scatter_3d(x=ts[:,0], y=ts[:,1], z=ts[:,2])" + ] + }, + { + "cell_type": "code", + "execution_count": 21, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[[ 0. -0.249 0.039]\n", + " [ 0. -0. -0.028]\n", + " [ 0. 0.028 -0. ]]\n" + ] + } + ], + "source": [ + "# Solve equation for finding the shoulder \"center\":\n", + "A = shoulder.axes\n", + "JR = A[:3,:]\n", + "Jt = A[3:,:]\n", + "Skew = -Jt @ np.linalg.pinv(JR)\n", + "print(np.round(Skew,3))" + ] + }, + { + "cell_type": "code", + "execution_count": 22, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "array([[ 0., 0., 0.],\n", + " [ 0., 1., 0.],\n", + " [ 1., -0., 1.],\n", + " [-0., 0., 0.],\n", + " [ 0., -0., 0.],\n", + " [ 0., -0., 0.]])" + ] + }, + "execution_count": 22, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "sTc = Pose3(Rot3(), Point3(-Skew[1,2], Skew[0,2], -Skew[0,1]))\n", + "np.round(sTc.AdjointMap() @ A, 5)" + ] + }, + { + "cell_type": "code", + "execution_count": 28, + "metadata": {}, + "outputs": [], + "source": [ + "# Let's now go to the shoulder position:\n", + "cTs = sTc.inverse()\n", + "poses = [shoulder.poe(q, cTs) for q in theta]\n", + "ts = np.array([pose.translation() for pose in poses])" + ] + }, + { + "cell_type": "code", + "execution_count": 29, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "array([[ 0. , 0. , 0.333],\n", + " [ 0. , 0. , 0.333],\n", + " [-0. , -0. , 0.333],\n", + " [ 0. , -0. , 0.333],\n", + " [-0. , -0. , 0.333],\n", + " [ 0. , 0. , 0.333],\n", + " [-0. , -0. , 0.333],\n", + " [-0. , -0. , 0.333],\n", + " [ 0. , 0. , 0.333],\n", + " [-0. , 0. , 0.333]])" + ] + }, + "execution_count": 29, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "np.round(ts[:10,:],3)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "interpreter": { + "hash": "c6e4e9f98eb68ad3b7c296f83d20e6de614cb42e90992a65aa266555a3137d0d" + }, + "kernelspec": { + "display_name": "Python 3.9.6 64-bit ('base': conda)", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.9.6" + }, + "orig_nbformat": 4 + }, + "nbformat": 4, + "nbformat_minor": 2 +} From d4cad014859945972481f867f6cdbfb0db1bc6f8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 15 Nov 2021 22:11:24 -0500 Subject: [PATCH 3/4] Fix math in meeting --- python/tests/prototype/chain.md | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/python/tests/prototype/chain.md b/python/tests/prototype/chain.md index 6671c2eb..eee9f9a7 100644 --- a/python/tests/prototype/chain.md +++ b/python/tests/prototype/chain.md @@ -12,11 +12,11 @@ Below I use tensor notation with superscripts and subscripts, and make sure matr ## Kinematics We know that the forward kinematics of a chain can be given by a **product of exponential maps** or POE, e.g., in the 3DOF case we have -$$T^b_e(\theta) = M^b_e \exp[A^e_1\theta^1]\exp[A^e_2\theta^2]\exp [A^e_3\theta^3] \doteq M^b_e \exp[A^e_j\theta^j]$$ -where the $6\times3$ matrix $A^e_j$ collects the joint screw axes expressed in the end-effector frame. Note I defined $\exp[A^e_j\theta^j]$ as shorthand for the POE of all axes in the matrix $A^e_j$. +$$T^b_e(\theta^j) = M^b_e \exp[A^e_1\theta^1]\exp[A^e_2\theta^2]\exp [A^e_3\theta^3] \doteq M^b_e \exp[A^e_j,\theta^j]$$ +where the $6\times3$ matrix $A^e_j$ collects the joint screw axes expressed in the end-effector frame. Note I defined $\exp[A^e_j,\theta^j]$ as shorthand for the POE of all axes in the matrix $A^e_j$. In fact, $A^e_j$ is just the **manipulator Jacobian** $J^e_j(\theta^j)$ at rest, and more generally we have -$$T^b_e(\theta^j+\delta \theta^j) = T^b_e(\theta^j) \exp[J^e_j(\theta^j)\delta \theta^j]$$ +$$T^b_e(\theta^j+\delta \theta^j) = T^b_e(\theta^j) \exp[J^e_j(\theta^j),\delta \theta^j]$$ with $A^e_j=J^e_j(0^j)$. ## Monoid Math @@ -49,6 +49,7 @@ Note, above we treat $\tau_j$ and $\mathcal{F_e}$ as *row* vectors, as indicated Of course, we are typically more interested in applying a wrench $\mathcal{F_b}$ on the body $B$, by delivering torques $\tau_j$ ate the joints, so we need to adjoint from the body: $$\tau_j = \mathcal{F_b} Ad^b_e J^e_j(\theta)$$ +$$\tau_j = \mathcal{F_b} (Ad^b_e J^e_j(\theta)) = \mathcal{F_b} J^b_j(\theta)$$ with $$Ad^b_e\doteq Ad_{T^b_e}.$$ From 6586ef124dad427f43a4d0c132e1a46a280fcfb0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Nov 2021 19:09:07 +0000 Subject: [PATCH 4/4] Remove deps --- python/tests/panda.ipynb | 1562 +------------------------------------- 1 file changed, 14 insertions(+), 1548 deletions(-) diff --git a/python/tests/panda.ipynb b/python/tests/panda.ipynb index d2866ef7..482d71b0 100644 --- a/python/tests/panda.ipynb +++ b/python/tests/panda.ipynb @@ -10,7 +10,7 @@ }, { "cell_type": "code", - "execution_count": 16, + "execution_count": null, "metadata": {}, "outputs": [], "source": [ @@ -23,15 +23,12 @@ "\n", "import plotly.express as px\n", "\n", - "import roboplot # type: ignore\n", - "%matplotlib widget\n", - "\n", "from prototype.chain import Chain" ] }, { "cell_type": "code", - "execution_count": 17, + "execution_count": null, "metadata": {}, "outputs": [], "source": [ @@ -43,7 +40,7 @@ }, { "cell_type": "code", - "execution_count": 18, + "execution_count": null, "metadata": {}, "outputs": [], "source": [ @@ -53,7 +50,7 @@ }, { "cell_type": "code", - "execution_count": 19, + "execution_count": null, "metadata": {}, "outputs": [], "source": [ @@ -64,1494 +61,9 @@ }, { "cell_type": "code", - "execution_count": 20, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "data": { - "application/vnd.plotly.v1+json": { - "config": { - "plotlyServerURL": "https://plot.ly" - }, - "data": [ - { - "hovertemplate": "x=%{x}
y=%{y}
z=%{z}", - "legendgroup": "", - "marker": { - "color": "#636efa", - "symbol": "circle" - }, - "mode": "markers", - "name": "", - "scene": "scene", - "showlegend": false, - "type": "scatter3d", - "x": [ - 0.1500642655603171, - 0.03495139605267255, - 0.17657161542870764, - 0.05483517100858217, - -0.10149406667219099, - -0.07876498771569276, - -0.20126612014121192, - 0.021944552356212343, - 0.2218987573590024, - 0.21557051135920083, - -0.01531576841323546, - 0.16540605903135894, - 0.14874102793752408, - 0.09900466215720122, - 0.12514477622672626, - 0.030100582902938836, - -0.13344556221873427, - 0.0008134405736729272, - 0.17355140965562083, - 0.08191521476688898, - -0.15174486740328824, - 0.02209076892582329, - 0.24685769160133464, - 0.041359015463585294, - -0.059877940260079844, - -0.030860870718399186, - 0.11091030616417734, - -0.20829161213879324, - 0.2537126613755947, - 0.0030697060332432503, - 0.2192757107398812, - 0.05489695020901817, - 0.08391243022200068, - -0.188587370011046, - -0.03365876316146201, - -0.014344847141831568, - 0.16253717320608066, - -0.03925905817262866, - 0.05054693587313683, - -0.19261250382817335, - 0.03949205159267791, - -0.094655977055026, - 0.10084890894410468, - 0.0070430140032235605, - -0.052776917743337856, - -0.06322715275387808, - 0.22730465766451544, - -0.006599781358974252, - 0.2073820872781236, - -0.09382431482137125, - -0.041080185755639605, - 0.056145758722311465, - 0.015215855440855033, - 0.038807361658544814, - 0.2136713868349819, - -0.19285885862910065, - -0.03652152212318867, - -0.05066000792968933, - 0.01920584703061529, - 0.22015663592168308, - 0.02728198129061525, - 0.2048495190931698, - -0.005678395606938336, - 0.24029681276654954, - -0.2155571993817883, - 0.2270662612994212, - -0.18041762508030212, - -0.11599981201212396, - -0.0640032032320543, - -0.028487948850901355, - 0.029075434258580765, - 0.13128205537913978, - -0.01930499237149294, - -0.01903323262395118, - 0.18494158251258497, - -0.16312905385628293, - 0.14301478001026902, - -0.00975141744393504, - -0.13777065807948252, - 0.1664923405635369, - -0.2286728532380336, - -0.0689231496998687, - -0.09703984159265808, - -0.002878638559664373, - -0.03018870500684162, - 0.005601741098185101, - 0.07559384713588055, - 0.23044320680638877, - 0.12816669516888, - 0.030675730036993715, - -0.030374677012523485, - 0.08354165339272113, - 0.0695816172916637, - -0.2270595495331493, - 0.11152924011126006, - -0.04461656568824396, - -0.17928918635214292, - 0.1539539533366538, - -0.06462326067448063, - -0.05328933349048401, - -0.04351311673901307, - -0.025672126717933313, - 0.2447331461571833, - -0.06325688595308082, - 0.11779237486777157, - -0.09625704685752111, - -0.027861690701714396, - -0.2461159330612999, - -0.18536744003806704, - -0.16877572572325772, - -0.17700359171957006, - -0.1415277919691853, - 0.21792262835356738, - -0.08334403894259484, - -0.05623470659743836, - 0.16948628919694186, - 0.009002369185819514, - 0.24524158558784895, - -0.1652635884775549, - 0.14081296148166503, - -0.11946578389790634, - -0.2518229753324598, - 0.019908381690474153, - -0.181087193931011, - -0.07603470715834096, - 0.13852480918409926, - 0.012823277616428971, - -0.1912107273903323, - -0.16128319608412772, - 0.14038928675377493, - 0.03453219856112949, - -0.048050524143181554, - 0.010936551602807806, - 0.0998206837536095, - 0.13071575232419097, - -0.07235774384617297, - -0.10665939086475172, - 0.0279836152045261, - 0.22051938029539866, - -0.09968385977053071, - -0.13144321158142735, - -0.06558313562423414, - 0.1431563681743743, - 0.18504671535829317, - 0.10939707187901501, - 0.14574788643736936, - 0.2505688155360533, - 0.09395443772757764, - 0.15472701717916612, - 0.168431384466765, - 0.10081654076517751, - -0.23068516573518055, - 0.04945589655845959, - 0.07772706397379325, - -0.011341590073822592, - 0.20772086327189215, - -0.1909033165595981, - 0.16812599763490202, - 0.025078931574348345, - 0.037640845758298055, - -0.23459608729801482, - 0.0560610936135096, - 0.10762389353247012, - 0.0210644533482479, - -0.1359858680217786, - -0.0356167209134262, - 0.024522947620054338, - 0.137388857260191, - -0.18447217937636984, - -0.09766875703200233, - -0.0837841838434764, - 0.013759764893819193, - -0.09630422100052588, - -0.022675431946046452, - 0.11173344998983095, - -0.05240482764194865, - -0.24684096578138956, - 0.037926728551984944, - -0.008952354854107072, - -0.06815879812949574, - 0.001116863791188305, - -0.003963608850620401, - 0.15880245602062623, - 0.04375831004462006, - -0.0899866822944631, - 0.01823142085424452, - 0.07902802690537622, - -0.020487319037828553, - -0.021311919604441745, - 0.08253340293738202, - 0.23468326806256395, - -0.061481669267882524, - -0.028406877915086365, - -0.08151204615600052, - 0.009151259934998004, - 0.21309677927546722, - -0.06903468771240431, - -0.040246557023328786, - -0.06539223008903998, - 0.14228013087210706 - ], - "y": [ - -0.0982499409067093, - 0.024079774104015024, - -0.034047632958542365, - 0.13463553014049018, - 0.05143112139886061, - -0.1419325394856221, - 0.13467753140153177, - -0.06012177255386561, - -0.12352873464216199, - 0.05392553721733984, - -0.016096685084267093, - 0.05398458085112011, - -0.09503606356048927, - 0.2294826826217347, - 0.0014915295491537695, - -0.2459410504546361, - 0.08576848774646781, - -0.07760236701488904, - 0.15448487416934206, - -0.23414537731816748, - -0.1287367482301743, - -0.007811025714359612, - -0.05975813030427617, - -0.04059424415091987, - -0.03286576414376887, - 0.08389421896114206, - 0.09456644424391476, - 0.13126748528703505, - 0.013044151672840922, - 0.2529037058084706, - 0.0981991289432279, - -0.030233170822614494, - -0.09642933747924273, - 0.1701822117587314, - 0.03629351965623594, - 0.18207197510835518, - -0.1314097195874911, - -0.05315562937166229, - -0.0674399082841291, - 0.15493728254918204, - -0.024626031612258424, - -0.0073859910309683335, - -0.1280423924630288, - -0.020056563965160007, - 0.012366314272107104, - -0.20141023916530476, - 0.11289246409539357, - -0.040705767335091264, - 0.003570956840123471, - -0.16707296003107924, - -0.17679686603005879, - -0.2471709465028105, - 0.10794531851342579, - 0.09538048640254586, - -0.13542928263338508, - 0.027959243693105054, - 0.06956300776801948, - 0.009567643336599076, - 0.08116037516342914, - 0.11795506685596696, - -0.06291187914646688, - -0.12707165639990334, - -0.035922077736004226, - 0.07931263092248429, - -0.06173877177255382, - 0.10117580299540439, - 0.14938294702041455, - 0.22072689621259933, - -0.11789704559725202, - -0.20676559766145472, - 0.23981457821232674, - 0.157252227910279, - -0.24512247697704587, - 0.11489567511138815, - -0.15881332086426797, - -0.11449532073408784, - -0.14603373484305568, - 0.21858355629439147, - 0.06892755682548626, - 0.1689001530349564, - 0.037617111232872175, - 0.07610555267303845, - 0.057455780092064916, - 0.19225121022182412, - -0.23623608775333035, - -0.03854355023273838, - 0.014105213234580652, - -0.01998381541035622, - -0.009151254555232975, - 0.24139748643683853, - 0.09740841223373775, - 0.216035718423505, - 0.13350666568247727, - -0.09441041880771552, - 0.15037073438568488, - -0.24987094303530955, - -0.06979618276448056, - -0.1425139993602383, - -0.24498228172720204, - 0.07275619643530584, - 0.21833704278791774, - 0.022395905837873098, - -0.06765076732549982, - -0.06059850722644813, - 0.20566304540075642, - -0.10239152646797568, - 0.11753432889571133, - 0.0629298744231974, - 0.07054458746655304, - -0.0358523277676974, - 0.09033139826199632, - -0.16437367855259347, - 0.08831007096569848, - 0.12748114017702833, - -0.02248848314795057, - -0.17476932589550642, - 0.23147712445522436, - 0.02929284932019894, - 0.08640670970214964, - -0.07494710790721558, - -0.17162885861843166, - 0.018686033145834655, - 0.21879118725790925, - -0.11941223696416971, - 0.1860969438627483, - -0.15378518792703358, - -0.04439014161266406, - 0.150416504768064, - 0.021747965539256543, - 0.0073015589955438606, - -0.06808497605446845, - -0.08071401904672126, - 0.043432157762796735, - -0.02925091639630921, - -0.03793838929140977, - 0.2244390815310654, - -0.042248837376439946, - -0.020663748246959823, - -0.11968365532269211, - -0.06844117352616111, - -0.13893826561023018, - 0.11088846983073908, - -0.19194279106900466, - 0.17395104227600452, - -0.041206966379603256, - 0.04325096413303019, - 0.0298871396281951, - 0.11683695362450902, - -0.16196900694610958, - -0.037816337881599396, - 0.1650141465486068, - 0.10308991929683861, - -0.08874924968124558, - -0.1203228169183977, - -0.009577792101542148, - -0.02684949425552752, - -0.15669386673296964, - -0.1012631602118302, - 0.135600940032313, - 0.09916416685109906, - 0.06323774200499532, - 0.014361080623027475, - 0.14604349650666423, - -0.2223544642307573, - -0.12122820550312571, - 0.1216408590055733, - 0.05636443246070054, - -0.14353308063387332, - 0.02523673768766896, - 0.05192575534308008, - -0.04689354335022172, - -0.00010360659153167695, - 0.07917434232204261, - 0.024643272935399627, - -0.22773944929979972, - -0.22436058423325775, - -0.037190451197098504, - -0.07085290268315302, - 0.09615744114323516, - -0.1549398214845134, - 0.03868991536784084, - -0.01708778451226952, - 0.16197233878463146, - -0.027747007041473603, - 0.0458511754311179, - 0.22421894572931783, - 0.2357379976950944, - 0.03496909592193581, - 0.01838991672105568, - -0.14061976283115207, - 0.09314726848096552, - 0.03597705598714745, - 0.04006380989387247, - -0.23278381728826328, - -0.13308955746247236, - -0.13595131687369855, - -0.18038700388838105, - -0.03162870573146629, - -0.23607696512847312, - 0.14745060689060407 - ], - "z": [ - 0.5129302007607308, - 0.08250892984940092, - 0.1535273415064812, - 0.5413592807540979, - 0.5601585481859045, - 0.5284442190937841, - 0.40981876887037955, - 0.087132106774891, - 0.3260139401388647, - 0.20983911781647893, - 0.5860880001091248, - 0.5181316085867409, - 0.515733316784762, - 0.37863975328293137, - 0.11190325159593792, - 0.38916196615302606, - 0.5314521611513912, - 0.09108178493543125, - 0.435769371368567, - 0.3878912716411359, - 0.4909479556817168, - 0.5859786505567431, - 0.3391202771009813, - 0.08563639917369074, - 0.08829265305401746, - 0.5708162705021389, - 0.5410944579792442, - 0.270304996601606, - 0.33563579150685485, - 0.3089686803254761, - 0.4155972492660154, - 0.08678974011687692, - 0.5525616058409403, - 0.337475545235697, - 0.582192856890553, - 0.5096103991062677, - 0.18856882346257833, - 0.08768302698432119, - 0.09332505788816509, - 0.3916691884826053, - 0.5827621834068228, - 0.5686542063620819, - 0.13811065927308186, - 0.5861705789139655, - 0.5812113677026067, - 0.47436284712083526, - 0.3446230799120046, - 0.5836924315243054, - 0.1862787337319285, - 0.16617423518326266, - 0.5107709153156488, - 0.31563944659421783, - 0.5624852888234678, - 0.10074203280216891, - 0.30953386087260426, - 0.16999400878419751, - 0.5746079017830255, - 0.08422452767727105, - 0.09301804072975653, - 0.2864718397671988, - 0.5776323788693286, - 0.4132289255755501, - 0.5844449681070603, - 0.355675100491847, - 0.45245974086178603, - 0.38545552115486903, - 0.43139424542058324, - 0.3816918138251079, - 0.11724298748925482, - 0.18814270392606952, - 0.4116816308401989, - 0.18272035871563058, - 0.26905079842504875, - 0.5587962172593507, - 0.40456921129896123, - 0.175434314808576, - 0.18210887097563136, - 0.4621253290697692, - 0.5350283383942042, - 0.42410571151423143, - 0.4371195993384063, - 0.10061076591279837, - 0.5606605242671776, - 0.16693255153340664, - 0.42147804679655604, - 0.0819417855073536, - 0.09085577630897385, - 0.43808942568390763, - 0.11382708967191299, - 0.4060305755197086, - 0.5656718548822229, - 0.2286133516884237, - 0.12835010240533665, - 0.3968580113880779, - 0.16125285250586668, - 0.3440038958915121, - 0.4989249600293913, - 0.47630091440823175, - 0.3141657850841171, - 0.570516303963203, - 0.21059593041780264, - 0.08123308965726384, - 0.34173342340076657, - 0.5714818633219789, - 0.4245143705191451, - 0.12134516762031503, - 0.5565097750756105, - 0.3292596483605198, - 0.49177526628793883, - 0.14651521018490815, - 0.1747054048979891, - 0.20071852083025687, - 0.42921991087949957, - 0.12965413231699607, - 0.5797369808758414, - 0.26035502541680816, - 0.22867114476269088, - 0.27345417303411323, - 0.16046293536170633, - 0.13525811590110287, - 0.4772867741648544, - 0.36098630186525565, - 0.4605981369322704, - 0.20072998189590738, - 0.4883507728474425, - 0.48033704573818586, - 0.08317548016531404, - 0.2597842595060135, - 0.13790523203417346, - 0.12137592945708367, - 0.09067960232308878, - 0.5690584802350781, - 0.5830824890772421, - 0.1012082253038151, - 0.547525715344772, - 0.4275445261181587, - 0.5596848515289142, - 0.5846687080678936, - 0.3729279024595375, - 0.5564415934864443, - 0.16576946358284156, - 0.5519744537895791, - 0.4179201450759767, - 0.3397793700544539, - 0.5585692319844356, - 0.5365537264034037, - 0.30351713382795065, - 0.5381070494422086, - 0.21311337532609154, - 0.5194081681364219, - 0.16821660162860824, - 0.35953365186413033, - 0.5658623988721004, - 0.5428288232981616, - 0.07937262358905318, - 0.1892002800700777, - 0.3925832836201917, - 0.4943277196740823, - 0.5463791088308765, - 0.5638610980117753, - 0.25875125831021906, - 0.5803825478450079, - 0.5108640112588556, - 0.4540867163388579, - 0.510084097876582, - 0.11281334635434852, - 0.0864865366247031, - 0.17466564334762724, - 0.16014071057956714, - 0.5617176024356912, - 0.5682199352984645, - 0.07931147056339397, - 0.5543687700817402, - 0.08115531429639805, - 0.3189432856668233, - 0.22593331930906138, - 0.2857388131341466, - 0.09198414725403999, - 0.0980089072166702, - 0.14353960364304258, - 0.5840957086371251, - 0.07954485750460018, - 0.4474288130784918, - 0.08427821620478887, - 0.09987497227193178, - 0.45107070551899175, - 0.38524346119964326, - 0.5838079821714491, - 0.080502822040052, - 0.5278371923761704, - 0.3611879743534135, - 0.5768705960518319, - 0.5822692364813605, - 0.2720517221546613, - 0.5492189537665277, - 0.30742239388394077, - 0.1679486366232935, - 0.5818514646398306, - 0.4003701314520298, - 0.18279382596382307 - ] - } - ], - "layout": { - "legend": { - "tracegroupgap": 0 - }, - "margin": { - "t": 60 - }, - "scene": { - "domain": { - "x": [ - 0, - 1 - ], - "y": [ - 0, - 1 - ] - }, - "xaxis": { - "title": { - "text": "x" - } - }, - "yaxis": { - "title": { - "text": "y" - } - }, - "zaxis": { - "title": { - "text": "z" - } - } - }, - "template": { - "data": { - "bar": [ - { - "error_x": { - "color": "#2a3f5f" - }, - "error_y": { - "color": "#2a3f5f" - }, - "marker": { - "line": { - "color": "#E5ECF6", - "width": 0.5 - }, - "pattern": { - "fillmode": "overlay", - "size": 10, - "solidity": 0.2 - } - }, - "type": "bar" - } - ], - "barpolar": [ - { - "marker": { - "line": { - "color": "#E5ECF6", - "width": 0.5 - }, - "pattern": { - "fillmode": "overlay", - "size": 10, - "solidity": 0.2 - } - }, - "type": "barpolar" - } - ], - "carpet": [ - { - "aaxis": { - "endlinecolor": "#2a3f5f", - "gridcolor": "white", - "linecolor": "white", - "minorgridcolor": "white", - "startlinecolor": "#2a3f5f" - }, - "baxis": { - "endlinecolor": "#2a3f5f", - "gridcolor": "white", - "linecolor": "white", - "minorgridcolor": "white", - "startlinecolor": "#2a3f5f" - }, - "type": "carpet" - } - ], - "choropleth": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "type": "choropleth" - } - ], - "contour": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1, - "#f0f921" - ] - ], - "type": "contour" - } - ], - "contourcarpet": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "type": "contourcarpet" - } - ], - "heatmap": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1, - "#f0f921" - ] - ], - "type": "heatmap" - } - ], - "heatmapgl": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1, - "#f0f921" - ] - ], - "type": "heatmapgl" - } - ], - "histogram": [ - { - "marker": { - "pattern": { - "fillmode": "overlay", - "size": 10, - "solidity": 0.2 - } - }, - "type": "histogram" - } - ], - "histogram2d": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1, - "#f0f921" - ] - ], - "type": "histogram2d" - } - ], - "histogram2dcontour": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1, - "#f0f921" - ] - ], - "type": "histogram2dcontour" - } - ], - "mesh3d": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "type": "mesh3d" - } - ], - "parcoords": [ - { - "line": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "parcoords" - } - ], - "pie": [ - { - "automargin": true, - "type": "pie" - } - ], - "scatter": [ - { - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scatter" - } - ], - "scatter3d": [ - { - "line": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scatter3d" - } - ], - "scattercarpet": [ - { - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scattercarpet" - } - ], - "scattergeo": [ - { - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scattergeo" - } - ], - "scattergl": [ - { - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scattergl" - } - ], - "scattermapbox": [ - { - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scattermapbox" - } - ], - "scatterpolar": [ - { - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scatterpolar" - } - ], - "scatterpolargl": [ - { - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scatterpolargl" - } - ], - "scatterternary": [ - { - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scatterternary" - } - ], - "surface": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1, - "#f0f921" - ] - ], - "type": "surface" - } - ], - "table": [ - { - "cells": { - "fill": { - "color": "#EBF0F8" - }, - "line": { - "color": "white" - } - }, - "header": { - "fill": { - "color": "#C8D4E3" - }, - "line": { - "color": "white" - } - }, - "type": "table" - } - ] - }, - "layout": { - "annotationdefaults": { - "arrowcolor": "#2a3f5f", - "arrowhead": 0, - "arrowwidth": 1 - }, - "autotypenumbers": "strict", - "coloraxis": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "colorscale": { - "diverging": [ - [ - 0, - "#8e0152" - ], - [ - 0.1, - "#c51b7d" - ], - [ - 0.2, - "#de77ae" - ], - [ - 0.3, - "#f1b6da" - ], - [ - 0.4, - "#fde0ef" - ], - [ - 0.5, - "#f7f7f7" - ], - [ - 0.6, - "#e6f5d0" - ], - [ - 0.7, - "#b8e186" - ], - [ - 0.8, - "#7fbc41" - ], - [ - 0.9, - "#4d9221" - ], - [ - 1, - "#276419" - ] - ], - "sequential": [ - [ - 0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1, - "#f0f921" - ] - ], - "sequentialminus": [ - [ - 0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1, - "#f0f921" - ] - ] - }, - "colorway": [ - "#636efa", - "#EF553B", - "#00cc96", - "#ab63fa", - "#FFA15A", - "#19d3f3", - "#FF6692", - "#B6E880", - "#FF97FF", - "#FECB52" - ], - "font": { - "color": "#2a3f5f" - }, - "geo": { - "bgcolor": "white", - "lakecolor": "white", - "landcolor": "#E5ECF6", - "showlakes": true, - "showland": true, - "subunitcolor": "white" - }, - "hoverlabel": { - "align": "left" - }, - "hovermode": "closest", - "mapbox": { - "style": "light" - }, - "paper_bgcolor": "white", - "plot_bgcolor": "#E5ECF6", - "polar": { - "angularaxis": { - "gridcolor": "white", - "linecolor": "white", - "ticks": "" - }, - "bgcolor": "#E5ECF6", - "radialaxis": { - "gridcolor": "white", - "linecolor": "white", - "ticks": "" - } - }, - "scene": { - "xaxis": { - "backgroundcolor": "#E5ECF6", - "gridcolor": "white", - "gridwidth": 2, - "linecolor": "white", - "showbackground": true, - "ticks": "", - "zerolinecolor": "white" - }, - "yaxis": { - "backgroundcolor": "#E5ECF6", - "gridcolor": "white", - "gridwidth": 2, - "linecolor": "white", - "showbackground": true, - "ticks": "", - "zerolinecolor": "white" - }, - "zaxis": { - "backgroundcolor": "#E5ECF6", - "gridcolor": "white", - "gridwidth": 2, - "linecolor": "white", - "showbackground": true, - "ticks": "", - "zerolinecolor": "white" - } - }, - "shapedefaults": { - "line": { - "color": "#2a3f5f" - } - }, - "ternary": { - "aaxis": { - "gridcolor": "white", - "linecolor": "white", - "ticks": "" - }, - "baxis": { - "gridcolor": "white", - "linecolor": "white", - "ticks": "" - }, - "bgcolor": "#E5ECF6", - "caxis": { - "gridcolor": "white", - "linecolor": "white", - "ticks": "" - } - }, - "title": { - "x": 0.05 - }, - "xaxis": { - "automargin": true, - "gridcolor": "white", - "linecolor": "white", - "ticks": "", - "title": { - "standoff": 15 - }, - "zerolinecolor": "white", - "zerolinewidth": 2 - }, - "yaxis": { - "automargin": true, - "gridcolor": "white", - "linecolor": "white", - "ticks": "", - "title": { - "standoff": 15 - }, - "zerolinecolor": "white", - "zerolinewidth": 2 - } - } - } - } - } - }, - "metadata": {}, - "output_type": "display_data" - } - ], + "outputs": [], "source": [ "# Let's plot end-effector position which is child link of last shoulder joint:\n", "poses = [shoulder.poe(q) for q in theta]\n", @@ -1561,19 +73,9 @@ }, { "cell_type": "code", - "execution_count": 21, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "[[ 0. -0.249 0.039]\n", - " [ 0. -0. -0.028]\n", - " [ 0. 0.028 -0. ]]\n" - ] - } - ], + "outputs": [], "source": [ "# Solve equation for finding the shoulder \"center\":\n", "A = shoulder.axes\n", @@ -1585,25 +87,9 @@ }, { "cell_type": "code", - "execution_count": 22, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "array([[ 0., 0., 0.],\n", - " [ 0., 1., 0.],\n", - " [ 1., -0., 1.],\n", - " [-0., 0., 0.],\n", - " [ 0., -0., 0.],\n", - " [ 0., -0., 0.]])" - ] - }, - "execution_count": 22, - "metadata": {}, - "output_type": "execute_result" - } - ], + "outputs": [], "source": [ "sTc = Pose3(Rot3(), Point3(-Skew[1,2], Skew[0,2], -Skew[0,1]))\n", "np.round(sTc.AdjointMap() @ A, 5)" @@ -1611,7 +97,7 @@ }, { "cell_type": "code", - "execution_count": 28, + "execution_count": null, "metadata": {}, "outputs": [], "source": [ @@ -1623,29 +109,9 @@ }, { "cell_type": "code", - "execution_count": 29, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "array([[ 0. , 0. , 0.333],\n", - " [ 0. , 0. , 0.333],\n", - " [-0. , -0. , 0.333],\n", - " [ 0. , -0. , 0.333],\n", - " [-0. , -0. , 0.333],\n", - " [ 0. , 0. , 0.333],\n", - " [-0. , -0. , 0.333],\n", - " [-0. , -0. , 0.333],\n", - " [ 0. , 0. , 0.333],\n", - " [-0. , 0. , 0.333]])" - ] - }, - "execution_count": 29, - "metadata": {}, - "output_type": "execute_result" - } - ], + "outputs": [], "source": [ "np.round(ts[:10,:],3)" ] @@ -1676,7 +142,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.9.6" + "version": "3.6.9" }, "orig_nbformat": 4 },