diff --git a/stretch/config/chomp_planning.yaml b/stretch/config/chomp_planning.yaml new file mode 100644 index 0000000..75258e5 --- /dev/null +++ b/stretch/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.01 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearence: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: true +max_recovery_attempts: 5 \ No newline at end of file diff --git a/stretch/config/fake_controllers.yaml b/stretch/config/fake_controllers.yaml new file mode 100644 index 0000000..e16ec64 --- /dev/null +++ b/stretch/config/fake_controllers.yaml @@ -0,0 +1,16 @@ +controller_list: + - name: fake_stretch_arm_controller + joints: + - joint_lift + - joint_arm_l3 + - joint_arm_l2 + - joint_arm_l1 + - joint_arm_l0 + - joint_wrist_yaw + - name: fake_gripper_controller + joints: + - joint_gripper_finger_left + - joint_gripper_finger_right +initial: # Define initial robot poses. + - group: stretch_arm + pose: home \ No newline at end of file diff --git a/stretch/config/fetch_pick.yml b/stretch/config/fetch_pick.yml new file mode 100644 index 0000000..a3af26f --- /dev/null +++ b/stretch/config/fetch_pick.yml @@ -0,0 +1,243 @@ +joint_trajectory: + joint_names: [torso_lift_joint, shoulder_pan_joint, shoulder_lift_joint, upperarm_roll_joint, elbow_flex_joint, forearm_roll_joint, wrist_flex_joint, wrist_roll_joint] + points: + - positions: [0.05, 1.32, 1.4, -0.2, 1.72, 0, 1.66, 0] + velocities: [0, 0, 0, 0, 0, 0, 0, 0] + accelerations: [0, 0, 0, 0, 0, 0, 0, 0] + time_from_start: 0 + - positions: [0.05133619423835708, 1.301766747958771, 1.396541930675425, -0.222066895595983, 1.714244126283814, -0.04797641198078807, 1.62524674029715, -0.03591095497237014] + velocities: [0.008371336239582521, -0.114232406634125, -0.02166500964065906, -0.1382504111265078, -0.03606089058580116, -0.3005750696498553, -0.2177312355064521, -0.2249842651079317] + accelerations: [0.02676540351101794, -0.365231591480017, -0.0692688369582509, -0.4420235830295204, -0.1152963230567571, -0.9610189812339895, -0.6961450607762718, -0.7193349385215934] + time_from_start: 0.220538773 + - positions: [0.05267238847671415, 1.283533495917543, 1.393083861350851, -0.2441337911919659, 1.708488252567627, -0.09595282396157614, 1.590493480594301, -0.07182190994474028] + velocities: [0.01210320271514311, -0.1651561870844673, -0.03132307626906553, -0.1998812021674863, -0.05213651158638835, -0.4345687349038888, -0.314793863886725, -0.3252802289129639] + accelerations: [0.02535841439676932, -0.3460322966447705, -0.06562755056717073, -0.4187875287212771, -0.1092354889136416, -0.9105006602743704, -0.6595504874103891, -0.6815213323272187] + time_from_start: 0.345604922 + - positions: [0.05400858271507122, 1.265300243876314, 1.389625792026276, -0.2662006867879489, 1.702732378851441, -0.1439292359423642, 1.555740220891451, -0.1077328649171104] + velocities: [0.01469803674929351, -0.2005644096255223, -0.03803850410004492, -0.2427342021855863, -0.06331418066045691, -0.5277369458349875, -0.3822832591302645, -0.395017820564102] + accelerations: [0.02569608815689577, -0.3506400779084368, -0.06650144991356205, -0.4243641218514913, -0.1106900734828398, -0.9226249270657925, -0.6683330906754733, -0.6905965003284013] + time_from_start: 0.4444175460000001 + - positions: [0.05534477695342829, 1.247066991835085, 1.386167722701702, -0.2882675823839319, 1.696976505135254, -0.1919056479231523, 1.520986961188602, -0.1436438198894806] + velocities: [0.01686644151301166, -0.2301537234014049, -0.0436503334145413, -0.2785448352187257, -0.07265493638837034, -0.6055940996071505, -0.4386815968353056, -0.4532948910650945] + accelerations: [0.02497871941841413, -0.3408511081315934, -0.06464490035084569, -0.4125169662497546, -0.1075998911215324, -0.8968676103885727, -0.649674948501564, -0.6713168209773345] + time_from_start: 0.5285948540000001 + - positions: [0.05668097119178537, 1.228833739793856, 1.382709653377127, -0.3103344779799149, 1.691220631419068, -0.2398820599039403, 1.486233701485752, -0.1795547748618507] + velocities: [0.01867542412487317, -0.2548384847582759, -0.04833197856701432, -0.3084197061656154, -0.08044742281718045, -0.6705461047583319, -0.4857316743638993, -0.5019122935770468] + accelerations: [0.02276926462044665, -0.3107016395524159, -0.05892683358962685, -0.3760284027227141, -0.09808230570344685, -0.8175365441429232, -0.5922089348104345, -0.6119365082311665] + time_from_start: 0.6034126310000001 + - positions: [0.05801716543014244, 1.210600487752628, 1.379251584052553, -0.3324013735758978, 1.685464757702881, -0.2878584718847284, 1.451480441782903, -0.2154657298342208] + velocities: [0.02039886227828804, -0.2783559355349845, -0.05279223474837288, -0.3368818329326804, -0.08787141259647406, -0.7324266132189194, -0.5305568143084294, -0.5482306417245777] + accelerations: [0.02764846642899876, -0.3772815676658368, -0.07155420288840955, -0.4566071343249938, -0.1191002600095181, -0.9927255918014486, -0.7191127656515618, -0.7430677400672796] + time_from_start: 0.671965177 + - positions: [0.05935335966849951, 1.192367235711399, 1.375793514727978, -0.3544682691718808, 1.679708883986695, -0.3358348838655165, 1.416727182080053, -0.2513766848065909] + velocities: [0.02062089962137707, -0.2813857815879691, -0.05336686716558061, -0.3405487211198195, -0.08882787451676297, -0.7403989234874153, -0.53633181410007, -0.5541980174256278] + accelerations: [-0.02112824918883656, 0.2883089011996067, 0.05467988732820403, 0.3489274654778232, 0.09101336518603773, 0.7586154455828104, 0.5495275387724464, 0.5678333160590975] + time_from_start: 0.734679072 + - positions: [0.06068955390685659, 1.17413398367017, 1.372335445403404, -0.3765351647678638, 1.673953010270508, -0.3838112958463046, 1.381973922377204, -0.2872876397789611] + velocities: [0.01894909221301441, -0.2585728663949275, -0.04904023129970068, -0.3129392624962806, -0.08162629255802589, -0.6803722307554043, -0.4928495453040172, -0.5092672739447954] + accelerations: [-0.02790449408073105, 0.3807752339080049, 0.07221680218962254, 0.4608353634987484, 0.1202031407052355, 1.001918333204849, 0.7257718240550562, 0.7499486240054631] + time_from_start: 0.80170458 + - positions: [0.06202574814521367, 1.155900731628942, 1.368877376078829, -0.3986020603638467, 1.668197136554322, -0.4317877078270926, 1.347220662674354, -0.3231985947513312] + velocities: [0.01698938413293589, -0.2318313565712978, -0.0439685087893772, -0.2805752001763684, -0.07318453169293558, -0.6100083873013817, -0.4418792283443526, -0.4565990415856903] + accelerations: [-0.02466660103938977, 0.3365920469052495, 0.06383713830461274, 0.4073624135015144, 0.1062553905073298, 0.8856609163997735, 0.6415570186580336, 0.6629284678970305] + time_from_start: 0.8760922510000001 + - positions: [0.06336194238357074, 1.137667479587713, 1.365419306754255, -0.4206689559598297, 1.662441262838135, -0.4797641198078807, 1.312467402971504, -0.3591095497237014] + velocities: [0.01478049092337804, -0.2016895512365083, -0.03825189541838764, -0.2440959111338407, -0.06366936540813757, -0.5306974850379411, -0.3844278210833743, -0.3972338218368476] + accelerations: [-0.02714666320172578, 0.3704341315256778, 0.07025553664850022, 0.4483199863130331, 0.1169386611014743, 0.9747082125358383, 0.7060612965039016, 0.7295815023764369] + time_from_start: 0.9595199630000001 + - positions: [0.0646981366219278, 1.119434227546484, 1.36196123742968, -0.4427358515558126, 1.656685389121949, -0.5277405317886688, 1.277714143268655, -0.3950205046960715] + velocities: [0.01195760803863887, -0.1631694516560709, -0.03094627739492897, -0.1974767444670096, -0.05150933886879161, -0.429341119051596, -0.3110070719229102, -0.3213672919146799] + accelerations: [-0.02790690538525214, 0.3808081377492571, 0.07222304264328379, 0.460875185557259, 0.1202135277911266, 1.002004911739479, 0.7258345400059496, 0.750013429140531] + time_from_start: 1.058170015 + - positions: [0.06603433086028489, 1.101200975505255, 1.358503168105105, -0.4648027471517956, 1.650929515405762, -0.5757169437694568, 1.242960883565805, -0.4309314596684417] + velocities: [0.007524858819872428, -0.1026816637123767, -0.01947432694272758, -0.1242710596902468, -0.03241455160933202, -0.2701820711959918, -0.1957150878871096, -0.2022347189478507] + accelerations: [-0.02746677104406057, 0.3748022142505598, 0.07108397541764649, 0.4536064829416456, 0.1183175776265914, 0.986201770344457, 0.7143870253973138, 0.7381845766768376] + time_from_start: 1.187016625 + - positions: [0.06737052509864196, 1.082967723464027, 1.355045098780531, -0.4868696427477786, 1.645173641689576, -0.6236933557502449, 1.208207623862956, -0.4668424146408118] + velocities: [0.01290867440776896, -0.03584942783927918, -0.04846196230094286, -0.0782009591209786, -0.03449866551841595, -0.008332146595463358, -0.05503487883364414, -0.1024195749142255] + accelerations: [0.05202943761588653, 0.1770435508015178, -0.2298315798928703, -0.005839953298151566, -0.09067493553133119, 1.009554707295178, 0.4215108838053243, 0.1475581978641948] + time_from_start: 1.472571441 + - positions: [0.07470789300538395, 1.08024395779517, 1.325604788928041, -0.5143351129021789, 1.628220291680549, -0.5711583454066077, 1.212246178191844, -0.4942925992586554] + velocities: [0.03754138823780361, -0.01393605251133448, -0.1506303235793887, -0.1405261248046079, -0.08674122692354749, 0.2687935570976757, 0.02066312305819253, -0.1404479168873285] + accelerations: [0.1358080227801803, -0.05041443126547065, -0.5449134242578654, -0.5083609332128197, -0.3137911269395097, 0.9723753765910943, 0.07474997645853852, -0.5080780117996798] + time_from_start: 1.819687915 + - positions: [0.08204526091212597, 1.077520192126314, 1.29616447907555, -0.5418005830565793, 1.611266941671522, -0.5186233350629705, 1.216284732520732, -0.5217427838764991] + velocities: [0.06229831428561082, -0.02312627795627427, -0.2499645239501936, -0.2331970419676902, -0.1439433241565919, 0.4460513125392614, 0.03428956133021371, -0.2330672593027703] + accelerations: [0.1393027417324134, -0.05171173509775093, -0.5589355654546401, -0.5214424769353876, -0.3218658472388449, 0.9973973052496046, 0.0766735016970405, -0.5211522751657249] + time_from_start: 1.955704341 + - positions: [0.08938262881886797, 1.074796426457458, 1.26672416922306, -0.5692660532109797, 1.594313591662494, -0.4660883247193334, 1.22032328684962, -0.5491929684943427] + velocities: [0.07671295043136613, -0.02847725552232871, -0.3078015248932712, -0.2871543688837911, -0.1772490510790774, 0.5492590388849987, 0.04222350875464278, -0.286994556997145] + accelerations: [0.125946482205541, -0.04675364636270042, -0.5053451739220282, -0.4714468992197473, -0.29100555163307, 0.9017674770448807, 0.06932209443280336, -0.471184521813788] + time_from_start: 2.059556706 + - positions: [0.09671999672560996, 1.072072660788602, 1.23728385937057, -0.59673152336538, 1.577360241653467, -0.4135533143756962, 1.224361841178508, -0.5766431531121864] + velocities: [0.08848925751474372, -0.03284883690509516, -0.3550525464938306, -0.3312358180953485, -0.2044587887310269, 0.6335765247053045, 0.04870529575978766, -0.3310514732733619] + accelerations: [0.1372780979363299, -0.05096015015156807, -0.5508119247356279, -0.5138637655416726, -0.3171878080080439, 0.9829010057425489, 0.07555911925485324, -0.5135777816015026] + time_from_start: 2.148200104 + - positions: [0.104057364632352, 1.069348895119745, 1.20784354951808, -0.6241969935197804, 1.56040689164444, -0.361018304032059, 1.228400395507395, -0.60409333773003] + velocities: [0.09710226176271033, -0.03604614220237606, -0.3896111943691733, -0.3634762909895702, -0.2243595593480834, 0.6952449966979577, 0.05344597198489465, -0.3632740031677274] + accelerations: [0.07662866559972535, -0.02844596744549528, -0.3074633421019491, -0.2868388712070586, -0.1770543068234903, 0.548655565737904, 0.04217711761329481, -0.2866792349063851] + time_from_start: 2.226087743 + - positions: [0.111394732539094, 1.066625129450889, 1.17840323966559, -0.6516624636741808, 1.543453541635413, -0.3084832936884219, 1.232438949836283, -0.6315435223478737] + velocities: [0.1, -0.03712183583371351, -0.4012380219538759, -0.374323197412024, -0.2310549262965193, 0.7159925876875399, 0.05504091358397099, -0.3741148728908743] + accelerations: [0, -9.456925142776832e-16, -5.144567277670597e-14, -9.835202148487905e-15, 3.555803853684089e-14, 1.815729627413152e-14, 1.418538771416525e-15, -9.078648137065759e-15] + time_from_start: 2.299461422 + - positions: [0.118732100445836, 1.063901363782032, 1.1489629298131, -0.6791279338285812, 1.526500191626386, -0.2559482833447847, 1.236477504165171, -0.6589937069657174] + velocities: [0.1, -0.03712183583371354, -0.4012380219538778, -0.3743231974120244, -0.231054926296518, 0.7159925876875406, 0.05504091358397105, -0.3741148728908746] + accelerations: [0, 0, 0, 0, 0, 0, 0, 0] + time_from_start: 2.372835101 + - positions: [0.126069468352578, 1.061177598113176, 1.119522619960609, -0.7065934039829815, 1.509546841617359, -0.2034132730011475, 1.240516058494059, -0.686443891583561] + velocities: [0.1, -0.03712183583371203, -0.4012380219538778, -0.3743231974120244, -0.231054926296518, 0.7159925876875406, 0.05504091358397105, -0.3741148728908746] + accelerations: [0, 4.123219362250703e-14, 0, 0, 0, 0, 0, 0] + time_from_start: 2.44620878 + - positions: [0.13340683625932, 1.05845383244432, 1.090082310108119, -0.7340588741373819, 1.492593491608332, -0.1508782626575104, 1.244554612822947, -0.7138940762014047] + velocities: [0.1, -0.03712183583371203, -0.4012380219538778, -0.3743231974120244, -0.2310549262965195, 0.7159925876875406, 0.05504091358397105, -0.3741148728908746] + accelerations: [0, -4.123219362250703e-14, 0, 0, -4.123219362250703e-14, 0, 0, 0] + time_from_start: 2.51958246 + - positions: [0.140744204166062, 1.055730066775464, 1.060642000255629, -0.7615243442917823, 1.475640141599304, -0.0983432523138732, 1.248593167151835, -0.7413442608192483] + velocities: [0.1, -0.03712183583371347, -0.4012380219538755, -0.3743231974120237, -0.2310549262965191, 0.7159925876875393, 0.05504091358397094, -0.3741148728908739] + accelerations: [0, 1.891385028555365e-15, 6.203742893661596e-14, 1.967040429697579e-14, 5.295878079955021e-14, -3.6314592548263e-14, -2.837077542833047e-15, 1.891385028555365e-14] + time_from_start: 2.592956139 + - positions: [0.148081572072804, 1.053006301106607, 1.031201690403139, -0.7889898144461827, 1.458686791590277, -0.04580824197023603, 1.252631721480723, -0.768794445437092] + velocities: [0.1, -0.03712183583371347, -0.4012380219538755, -0.3743231974120237, -0.2310549262965176, 0.7159925876875393, 0.05504091358397094, -0.3741148728908739] + accelerations: [0, -1.891385028555365e-15, -6.203742893661596e-14, -1.967040429697579e-14, -1.172658717704326e-14, 3.6314592548263e-14, 2.837077542833047e-15, -1.891385028555365e-14] + time_from_start: 2.666329818 + - positions: [0.155418939979546, 1.050282535437751, 1.001761380550649, -0.816455284600583, 1.44173344158125, 0.006726768373401137, 1.256670275809611, -0.7962446300549356] + velocities: [0.1, -0.03712183583371347, -0.401238021953877, -0.3743231974120237, -0.2310549262965191, 0.7159925876875393, 0.05504091358397094, -0.3741148728908739] + accelerations: [0, 1.891385028555365e-15, 2.118351231982008e-14, 1.967040429697579e-14, -2.950560644546369e-14, -3.6314592548263e-14, -2.837077542833047e-15, 1.891385028555365e-14] + time_from_start: 2.739703497 + - positions: [0.162756307886288, 1.047558769768895, 0.9723210706981584, -0.8439207547549834, 1.424780091572223, 0.0592617787170383, 1.260708830138499, -0.8236948146727793] + velocities: [0.1, -0.03712183583371354, -0.401238021953877, -0.3743231974120244, -0.2310549262965195, 0.7159925876875406, 0.05504091358397105, -0.3741148728908746] + accelerations: [0, -3.782770057110737e-15, -2.118351231982012e-14, -3.858425458252951e-14, 1.740074226270939e-14, 7.414229311937043e-14, 5.674155085666105e-15, -3.858425458252951e-14] + time_from_start: 2.813077176 + - positions: [0.17009367579303, 1.044835004100038, 0.9428807608456683, -0.8713862249093838, 1.407826741563196, 0.1117967890606755, 1.264747384467386, -0.8511449992906229] + velocities: [0.1, -0.03712183583371203, -0.401238021953877, -0.3743231974120244, -0.231054926296518, 0.7159925876875406, 0.05504091358397105, -0.3741148728908746] + accelerations: [0, 4.501496367961776e-14, 2.118351231982012e-14, 3.858425458252951e-14, 2.383145135979764e-14, -7.414229311937043e-14, -5.674155085666105e-15, 3.858425458252951e-14] + time_from_start: 2.886450855 + - positions: [0.177431043699772, 1.042111238431182, 0.9134404509931781, -0.8988516950637842, 1.390873391554169, 0.1643317994043126, 1.268785938796274, -0.8785951839084666] + velocities: [0.1, -0.03712183583371196, -0.4012380219538763, -0.3743231974120237, -0.2310549262965176, 0.7159925876875393, 0.05504091358397094, -0.3741148728908739] + accelerations: [0, -4.312357865106231e-14, 0, -1.967040429697579e-14, -1.172658717704326e-14, 3.6314592548263e-14, 2.837077542833047e-15, -1.891385028555365e-14] + time_from_start: 2.959824534 + - positions: [0.184768411606514, 1.039387472762326, 0.8840001411406879, -0.9263171652181845, 1.373920041545142, 0.2168668097479498, 1.272824493125162, -0.9060453685263102] + velocities: [0.1, -0.03712183583371354, -0.401238021953877, -0.3743231974120244, -0.231054926296518, 0.7159925876875406, 0.05504091358397105, -0.3741148728908746] + accelerations: [0, 0, -2.118351231982012e-14, 0, 0, 0, 0, 0] + time_from_start: 3.033198213 + - positions: [0.192105779513256, 1.036663707093469, 0.8545598312881977, -0.9537826353725849, 1.356966691536114, 0.269401820091587, 1.27686304745405, -0.9334955531441539] + velocities: [0.1, -0.03712183583371354, -0.401238021953877, -0.3743231974120244, -0.2310549262965195, 0.7159925876875406, 0.05504091358397105, -0.3741148728908746] + accelerations: [0, 0, 2.118351231982012e-14, 0, -4.123219362250703e-14, 0, 0, 0] + time_from_start: 3.106571892 + - positions: [0.199443147419998, 1.033939941424613, 0.8251195214357075, -0.9812481055269853, 1.340013341527087, 0.3219368304352241, 1.280901601782938, -0.9609457377619975] + velocities: [0.1, -0.03712183583371354, -0.401238021953877, -0.3743231974120244, -0.2310549262965195, 0.7159925876875406, 0.05504091358397105, -0.3741148728908746] + accelerations: [0, 0, -2.118351231982012e-14, 0, 4.123219362250703e-14, 0, 0, 0] + time_from_start: 3.179945571 + - positions: [0.20678051532674, 1.031216175755757, 0.7956792115832173, -1.008713575681386, 1.32305999151806, 0.3744718407788613, 1.284940156111826, -0.9883959223798412] + velocities: [0.1, -0.03712183583371354, -0.401238021953877, -0.3743231974120244, -0.231054926296518, 0.7159925876875406, 0.05504091358397256, -0.3741148728908739] + accelerations: [0, 0, 2.118351231982012e-14, 0, 0, 0, 4.123219362250703e-14, 2.042695830839798e-14] + time_from_start: 3.25331925 + - positions: [0.214117883233482, 1.0284924100869, 0.7662389017307272, -1.036179045835786, 1.306106641509033, 0.4270068511224985, 1.288978710440714, -1.015846106997685] + velocities: [0.1, -0.03712183583371188, -0.4012380219538763, -0.374323197412023, -0.2310549262965186, 0.7159925876875386, 0.05504091358397235, -0.3741148728908724] + accelerations: [0, 4.510953293104536e-14, 0, 3.858425458252936e-14, -1.740074226270932e-14, -5.295878079955011e-14, -4.690634870817296e-14, 1.815729627413147e-14] + time_from_start: 3.326692929 + - positions: [0.2214552511402241, 1.025768644418044, 0.7367985918782368, -1.063644515990186, 1.289153291500006, 0.4795418614661358, 1.293017264769602, -1.043296291615528] + velocities: [0.1, -0.03712183583371188, -0.4012380219538763, -0.374323197412023, -0.2310549262965186, 0.7159925876875379, 0.05504091358397084, -0.3741148728908732] + accelerations: [0, -4.510953293104536e-14, 0, -3.858425458252936e-14, 1.740074226270932e-14, 3.328837650257436e-14, 5.674155085666083e-15, -3.858425458252936e-14] + time_from_start: 3.400066608 + - positions: [0.2287926190469661, 1.023044878749188, 0.7073582820257467, -1.091109986144587, 1.272199941490979, 0.5320768718097728, 1.29705581909849, -1.070746476233372] + velocities: [0.1, -0.03712183583371354, -0.4012380219538763, -0.3743231974120244, -0.231054926296518, 0.7159925876875392, 0.05504091358397105, -0.3741148728908746] + accelerations: [0, 0, 0, 0, 0, 0, 0, 0] + time_from_start: 3.473440287 + - positions: [0.2361299869537081, 1.020321113080332, 0.6779179721732566, -1.118575456298987, 1.255246591481952, 0.5846118821534099, 1.301094373427378, -1.098196660851216] + velocities: [0.1, -0.03712183583371354, -0.4012380219538763, -0.3743231974120244, -0.231054926296518, 0.7159925876875406, 0.05504091358397105, -0.3741148728908746] + accelerations: [0, 0, 0, 0, 0, 4.085391661679595e-14, 0, 0] + time_from_start: 3.546813966 + - positions: [0.2434673548604501, 1.017597347411475, 0.6484776623207664, -1.146040926453388, 1.238293241472924, 0.6371468924970471, 1.305132927756266, -1.125646845469059] + velocities: [0.1, -0.03712183583371354, -0.4012380219538778, -0.3743231974120244, -0.2310549262965195, 0.7159925876875421, 0.05504091358397105, -0.3741148728908746] + accelerations: [0, 0, -4.16104706282181e-14, 0, -4.123219362250703e-14, 0, 0, 0] + time_from_start: 3.620187646 + - positions: [0.2508047227671921, 1.014873581742619, 0.6190373524682761, -1.173506396607788, 1.221339891463897, 0.6896819028406844, 1.309171482085153, -1.153097030086903] + velocities: [0.1, -0.03712183583371354, -0.4012380219538778, -0.3743231974120244, -0.2310549262965195, 0.7159925876875406, 0.05504091358397105, -0.3741148728908746] + accelerations: [0, 0, 4.16104706282181e-14, 0, 4.123219362250703e-14, -4.085391661679595e-14, 0, 0] + time_from_start: 3.693561325 + - positions: [0.2581420906739341, 1.012149816073763, 0.5895970426157859, -1.200971866762188, 1.20438654145487, 0.7422169131843215, 1.313210036414041, -1.180547214704747] + velocities: [0.1, -0.03712183583371203, -0.4012380219538763, -0.3743231974120244, -0.231054926296518, 0.7159925876875392, 0.05504091358397105, -0.3741148728908746] + accelerations: [0, 4.123219362250703e-14, 0, 0, 0, 0, 0, 0] + time_from_start: 3.766935004 + - positions: [0.2654794585806761, 1.009426050404906, 0.5601567327632958, -1.228437336916589, 1.187433191445843, 0.7947519235279585, 1.317248590742929, -1.20799739932259] + velocities: [0.1, -0.03712183583371203, -0.4012380219538763, -0.3743231974120244, -0.231054926296518, 0.7159925876875406, 0.05504091358397105, -0.3741148728908746] + accelerations: [0, -4.123219362250703e-14, 0, 0, 0, 4.085391661679595e-14, 0, 0] + time_from_start: 3.840308683 + - positions: [0.2728168264874181, 1.00670228473605, 0.5307164229108057, -1.255902807070989, 1.170479841436816, 0.8472869338715958, 1.321287145071817, -1.235447583940434] + velocities: [0.1, -0.0371218358337134, -0.4012380219538763, -0.374323197412023, -0.2310549262965186, 0.7159925876875395, 0.05504091358397084, -0.3741148728908732] + accelerations: [0, 3.87733930853849e-15, 0, 3.858425458252936e-14, -1.740074226270932e-14, -7.414229311937015e-14, -5.674155085666083e-15, 3.858425458252936e-14] + time_from_start: 3.913682362 + - positions: [0.2801541943941601, 1.003978519067194, 0.5012761130583153, -1.283368277225389, 1.153526491427789, 0.8998219442152331, 1.325325699400705, -1.262897768558278] + velocities: [0.1, -0.03712183583371353, -0.4012380219538778, -0.3743231974120244, -0.2310549262965195, 0.7159925876875406, 0.05504091358397105, -0.3741148728908746] + accelerations: [0, -7.660109365649241e-15, -4.16104706282181e-14, -7.716850916505902e-14, -6.430709097088252e-15, 1.074306696219449e-13, 1.134831017133221e-14, -7.716850916505902e-14] + time_from_start: 3.987056041 + - positions: [0.2874915623009021, 1.001254753398337, 0.4718358032058252, -1.31083374737979, 1.136573141418761, 0.9523569545588702, 1.329364253729593, -1.290347953176121] + velocities: [0.1, -0.03712183583371292, -0.4012380219538778, -0.3743231974120258, -0.2310549262965189, 0.7159925876875419, 0.05504091358397126, -0.374114872890876] + accelerations: [0, 2.439886686836434e-14, 4.161047062821826e-14, 3.858425458252965e-14, 2.383145135979773e-14, -7.414229311937072e-14, -5.674155085666126e-15, 3.858425458252965e-14] + time_from_start: 4.06042972 + - positions: [0.2948289302076441, 0.9985309877294811, 0.4423954933533351, -1.33829921753419, 1.119619791409734, 1.004891964902507, 1.333402808058481, -1.317798137793965] + velocities: [0.1, -0.03712183583371279, -0.4012380219538763, -0.3743231974120244, -0.231054926296518, 0.7159925876875406, 0.05504091358397105, -0.3741148728908746] + accelerations: [0, -2.061609681125351e-14, 0, 0, 0, 4.085391661679595e-14, 0, 0] + time_from_start: 4.133803399 + - positions: [0.3021662981143861, 0.9958072220606248, 0.4129551835008449, -1.36576468768859, 1.102666441400707, 1.057426975246144, 1.337441362387369, -1.345248322411809] + velocities: [0.1, -0.0371218358337134, -0.4012380219538763, -0.374323197412023, -0.2310549262965186, 0.7159925876875395, 0.05504091358397084, -0.3741148728908732] + accelerations: [0, 3.87733930853849e-15, 0, 3.858425458252936e-14, -1.740074226270932e-14, -7.414229311937015e-14, -5.674155085666083e-15, 3.858425458252936e-14] + time_from_start: 4.207177078 + - positions: [0.3095036660211281, 0.9930834563917684, 0.3835148736483546, -1.393230157842991, 1.08571309139168, 1.109961985589782, 1.341479916716257, -1.372698507029652] + velocities: [0.09617416112411559, -0.0357016142069445, -0.3858873017251352, -0.3600021950039802, -0.2222151371016204, 0.6885998649193372, 0.05293513691443334, -0.3598018406433485] + accelerations: [-0.1001352481978134, 0.03717204244768842, 0.4017806891475107, 0.3748294627904843, 0.2313674239203097, -0.7169609547588321, -0.05511535542764789, 0.374620856514174] + time_from_start: 4.280550757 + - positions: [0.3168410339278701, 0.9903596907229122, 0.3540745637958644, -1.420695627997391, 1.068759741382653, 1.162496995933419, 1.345518471045144, -1.400148691647496] + velocities: [0.08715138464098035, -0.03235219393323226, -0.3496844918388845, -0.3262278495769696, -0.201367567548612, 0.6239974540964757, 0.04796891830747621, -0.3260462918722408] + accelerations: [-0.1230168055719653, 0.04566609661228698, 0.4935901973477849, 0.4604804399711143, 0.2842363894466353, -0.8807912095052637, -0.06770957364862712, 0.4602241657999722] + time_from_start: 4.360003938 + - positions: [0.3241784018346121, 0.9876359250540558, 0.3246342539433742, -1.448161098151792, 1.051806391373626, 1.215032006277056, 1.349557025374032, -1.42759887626534] + velocities: [0.07571954408296151, -0.02810848484851333, -0.3038156009109683, -0.2834358184771488, -0.1749537367729472, 0.542146323064804, 0.04167672882487971, -0.2832780760995212] + accelerations: [-0.1278123295665186, 0.04744628315692797, 0.5128316629658916, 0.4784311987201865, 0.2953166838777606, -0.9151268058470203, -0.07034907386636781, 0.478164934296647] + time_from_start: 4.44953377 + - positions: [0.3315157697413541, 0.9849121593851995, 0.2951939440908842, -1.475626568306192, 1.034853041364598, 1.267567016620693, 1.35359557970292, -1.455049060883183] + velocities: [0.06108030738564317, -0.02267413143442567, -0.245077417157502, -0.2286377595950324, -0.1411290592115855, 0.4373304734179715, 0.03361915920495574, -0.2285105143371544] + accelerations: [-0.1372755419240551, 0.05095920131289485, 0.550801669042528, 0.513854197794807, 0.3171819022157884, -0.9828827048841386, -0.07555771240234709, 0.513568219179438] + time_from_start: 4.555130745 + - positions: [0.3388531376480961, 0.9821883937163433, 0.2657536342383939, -1.503092038460592, 1.017899691355571, 1.32010202696433, 1.357634134031808, -1.482499245501027] + velocities: [0.03804504519244093, -0.01412301921919965, -0.1526511867816087, -0.1424114296211946, -0.08790495112887137, 0.2723997035602523, 0.02094034044735415, -0.1423321724629762] + accelerations: [-0.1292866948414937, 0.04799359461388995, 0.5187473770315565, 0.4839500899590059, 0.2987232774772173, -0.9256831519313097, -0.07116057798327889, 0.4836807540710669] + time_from_start: 4.694423242 + - positions: [0.3461905055548381, 0.9794646280474869, 0.2363133243859038, -1.530557508614993, 1.000946341346544, 1.372637037307967, 1.361672688360696, -1.509949430118871] + velocities: [0.01767972892448451, 0.0990347868900794, -0.08948628620571952, -0.0008459073278277787, -0.05532430777174868, 0.09363852377344141, 0.01542111953883199, 0.05224855975627998] + accelerations: [-0.04092872044080442, 0.7688892779875072, 0.03183325842348115, 0.6195175902536564, -0.008742177287700631, -0.5282032789266314, 0.01808461575306076, 0.9981268286784342] + time_from_start: 5.007796921 + - positions: [0.3491416391975212, 1.030545655645158, 0.215307329153791, -1.509322637881912, 0.9869756992463063, 1.377487605682102, 1.366108501047152, -1.462492303785859] + velocities: [0.01731956995678542, 0.2997835876175422, -0.1232796775695019, 0.1246228987618844, -0.08199070001326519, 0.02846694472623533, 0.0260328326129022, 0.2785156888812803] + accelerations: [0.05700670331743764, 0.9867262340457251, -0.4057703523707084, 0.4101915136463588, -0.2698692590005137, 0.09369786180670238, 0.08568607471081091, 0.9167237572828102] + time_from_start: 5.254850029 + - positions: [0.3520927728402042, 1.08162668324283, 0.1943013339216782, -1.488087767148832, 0.9730050571460684, 1.382338174056236, 1.370544313733608, -1.415035177452847] + velocities: [0.02584117636965673, 0.4472836554072953, -0.1839359694736433, 0.1859400848080565, -0.1223319138408704, 0.04247330281360125, 0.03884155441678389, 0.4155514863276338] + accelerations: [0.05430145094908342, 0.9399011533757285, -0.3865145255489707, 0.3907258806735059, -0.2570626168063173, 0.08925143098694564, 0.08161984314406769, 0.8732206432419045] + time_from_start: 5.384891441 + - positions: [0.3550439064828873, 1.132707710840501, 0.1732953386895654, -1.466852896415751, 0.9590344150458305, 1.387188742430371, 1.374980126420065, -1.367578051119836] + velocities: [0.03169655099726124, 0.5486340478874681, -0.2256141807653115, 0.2280724103363227, -0.1500512085898211, 0.05209736543705658, 0.04764269601248172, 0.5097116590186608] + accelerations: [0.05774570786934601, 0.9995176276189789, -0.4110305431900593, 0.4155090180466665, -0.2733677003980275, 0.09491251100321844, 0.08679686336477879, 0.9286076760160349] + time_from_start: 5.486694851 + - positions: [0.3579950401255704, 1.183788738438172, 0.1522893434574526, -1.445618025682671, 0.9450637729455926, 1.392039310804505, 1.379415939106521, -1.320120924786824] + velocities: [0.03331437987560243, 0.5766369686592034, -0.2371297913135049, 0.2397134917848006, -0.1577099970336359, 0.05475647564424864, 0.05007443470410302, 0.5357279357315071] + accelerations: [-0.02458674572675346, -0.4255707768840276, 0.1750070061347061, -0.1769138339597719, 0.1163934496188164, -0.04041148442760755, -0.03695603514392103, -0.3953790100170652] + time_from_start: 5.572472279 + - positions: [0.3609461737682534, 1.234869766035844, 0.1312833482253398, -1.42438315494959, 0.9310931308453547, 1.396889879178639, 1.383851751792977, -1.272663798453813] + velocities: [0.02984114868977805, 0.5122857944360721, -0.2064617542814594, 0.2100693024923088, -0.1408579798846311, 0.06126617793479697, 0.03066582534418305, 0.4683114005020363] + accelerations: [-0.04714984867569861, -0.8998696846413793, 0.4532495082124532, -0.4313194555991778, 0.2313144460976612, 0.1642491535605639, -0.3515863349811202, -0.9870056433844518] + time_from_start: 5.664053531 + - positions: [0.3639827099213742, 1.28649274196755, 0.1109845278956959, -1.40356280756178, 0.9168088252191724, 1.404583236802116, 1.38527787697041, -1.226390900738453] + velocities: [0.02509080408788792, 0.4265590495950396, -0.1677285230353294, 0.1720378849474291, -0.1180307745157939, 0.06356997551644882, 0.01178402812527333, 0.3823515191682214] + accelerations: [-0.038773302554092, -0.6591699105856987, 0.2591941154145713, -0.2658534553323877, 0.1823952279474426, -0.09823590688531163, -0.01821008550412029, -0.5908551628237249] + time_from_start: 5.774641581 + - velocities: [0.01598775477442555, 0.2718016313008303, -0.1068759090211824, 0.1096218162963567, -0.07520871280904698, 0.04050652087566379, 0.007508733130348002, 0.2436327789527268] + positions: [0.367019246074495, 1.338115717899256, 0.09068570756605197, -1.38274246017397, 0.90252451959299, 1.412276594425592, 1.386704002147843, -1.180118003023093] + accelerations: [-0.05833938330779903, -0.9918052769678912, 0.3899906341276828, -0.400010461133685, 0.2744369041531519, -0.1478084622370402, -0.02739939825371802, -0.8890170182246813] + time_from_start: 5.908271222 + - positions: [0.3700557822276158, 1.389738693830961, 0.07038688723640804, -1.361922112786161, 0.8882402139668076, 1.419969952049068, 1.388130127325277, -1.133845105307734] + velocities: [0, 0, 0, 0, 0, 0, 0, 0] + accelerations: [-0.05637954588516872, -0.9584868394467679, 0.3768893945207172, -0.3865726181754049, 0.2652175452156025, -0.1428430248386813, -0.02647895029883613, -0.859151621594102] + time_from_start: 6.236474896 \ No newline at end of file diff --git a/stretch/config/joint_limits.yaml b/stretch/config/joint_limits.yaml new file mode 100644 index 0000000..c666ced --- /dev/null +++ b/stretch/config/joint_limits.yaml @@ -0,0 +1,50 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 + +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + joint_arm_l0: + has_velocity_limits: true + max_velocity: 1 + has_acceleration_limits: false + max_acceleration: 0 + joint_arm_l1: + has_velocity_limits: true + max_velocity: 1 + has_acceleration_limits: false + max_acceleration: 0 + joint_arm_l2: + has_velocity_limits: true + max_velocity: 1 + has_acceleration_limits: false + max_acceleration: 0 + joint_arm_l3: + has_velocity_limits: true + max_velocity: 1 + has_acceleration_limits: false + max_acceleration: 0 + joint_gripper_finger_left: + has_velocity_limits: true + max_velocity: 1 + has_acceleration_limits: false + max_acceleration: 0 + joint_gripper_finger_right: + has_velocity_limits: true + max_velocity: 1 + has_acceleration_limits: false + max_acceleration: 0 + joint_lift: + has_velocity_limits: true + max_velocity: 1 + has_acceleration_limits: false + max_acceleration: 0 + joint_wrist_yaw: + has_velocity_limits: true + max_velocity: 1 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/stretch/config/kinematics.yaml b/stretch/config/kinematics.yaml new file mode 100644 index 0000000..5928c35 --- /dev/null +++ b/stretch/config/kinematics.yaml @@ -0,0 +1,8 @@ +stretch_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 +stretch_head: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.1 diff --git a/stretch/config/ompl_planning.yaml b/stretch/config/ompl_planning.yaml new file mode 100644 index 0000000..e59094e --- /dev/null +++ b/stretch/config/ompl_planning.yaml @@ -0,0 +1,210 @@ +planner_configs: + AnytimePathShortening: + type: geometric::AnytimePathShortening + shortcut: true # Attempt to shortcut all new solution paths + hybridize: true # Compute hybrid solution trajectories + max_hybrid_paths: 24 # Number of hybrid paths generated per iteration + num_planners: 4 # The number of default planners to use for planning + planners: "" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]" + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 +stretch_arm: + default_planner_config: RRTConnect + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo +stretch_gripper: + default_planner_config: RRTConnect + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo +stretch_head: + default_planner_config: RRTConnect + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo \ No newline at end of file diff --git a/stretch/config/ros_controllers.yaml b/stretch/config/ros_controllers.yaml new file mode 100644 index 0000000..03882df --- /dev/null +++ b/stretch/config/ros_controllers.yaml @@ -0,0 +1,31 @@ +# Publish all joint states +# Creates the /joint_states topic necessary in ROS +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 +controller_list: + - name: stretch_arm_controller + action_ns: follow_joint_trajectory + default: True + type: FollowJointTrajectory + joints: + - joint_lift + - joint_arm_l3 + - joint_arm_l2 + - joint_arm_l1 + - joint_arm_l0 + - joint_wrist_yaw + - name: stretch_head_controller + action_ns: follow_joint_trajectory + default: True + type: FollowJointTrajectory + joints: + - joint_head_pan + - joint_head_tilt + - name: stretch_gripper_controller + action_ns: follow_joint_trajectory + default: True + type: FollowJointTrajectory + joints: + - joint_gripper_finger_left + - joint_gripper_finger_right diff --git a/stretch/config/sensors_3d.yaml b/stretch/config/sensors_3d.yaml new file mode 100644 index 0000000..a4bb13e --- /dev/null +++ b/stretch/config/sensors_3d.yaml @@ -0,0 +1,10 @@ +# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it +sensors: + - filtered_cloud_topic: filtered_cloud + max_range: 5.0 + max_update_rate: 1.0 + padding_offset: 0.1 + padding_scale: 1.0 + point_cloud_topic: /head_mount_kinect/depth_registered/points + point_subsample: 1 + sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater \ No newline at end of file diff --git a/stretch/config/stretch_description.srdf b/stretch/config/stretch_description.srdf new file mode 100644 index 0000000..319bcec --- /dev/null +++ b/stretch/config/stretch_description.srdf @@ -0,0 +1,408 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch/config/stretch_path.yml b/stretch/config/stretch_path.yml new file mode 100644 index 0000000..47b1291 --- /dev/null +++ b/stretch/config/stretch_path.yml @@ -0,0 +1,89 @@ +joint_trajectory: + joint_names: [joint_lift, joint_arm_l3, joint_arm_l2, joint_arm_l1, joint_arm_l0, joint_wrist_yaw] + points: + - accelerations: [0.9743443773410299, 0, 0, 0, 0, 0] + time_from_start: 0 + positions: [0, 0, 0, 0, 0.1, 0] + velocities: [0, 0, 0, 0, 0, 0] + - positions: [0.049835, 1.748663077130641e-18, 0.006499999999999994, 0.006499999999999991, 0.1015, 0.006499999999999991] + velocities: [0.2668737218617201, 9.364346818019912e-18, 0.03480845173274163, 0.03480845173274161, 0.008032719630632675, 0.03480845173274161] + accelerations: [0.9834821331023582, 3.450945907843437e-17, 0.1282759880639174, 0.1282759880639173, 0.02960215109167348, 0.1282759880639173] + time_from_start: 0.319835 + - positions: [0.09967000000000001, 3.497326154261283e-18, 0.01299999999999999, 0.01299999999999998, 0.103, 0.01299999999999998] + velocities: [0.433214405985736, 1.520108430276593e-17, 0.05650433709054442, 0.05650433709054441, 0.01303946240551021, 0.05650433709054441] + accelerations: [0.9454737118570534, 3.317577947872146e-17, 0.1233185337026355, 0.1233185337026356, 0.02845812316214557, 0.1233185337026356] + time_from_start: 0.451697092 + - positions: [0.149505, 5.245989231391924e-18, 0.01949999999999998, 0.01949999999999997, 0.1045, 0.01949999999999997] + velocities: [0.5306480814296293, 1.861993994072853e-17, 0.06921265233856902, 0.06921265233856899, 0.01597215053966965, 0.06921265233856899] + accelerations: [0.8920131096561211, 3.129989744501141e-17, 0.1163456448834109, 0.1163456448834106, 0.02684899497309454, 0.1163456448834106] + time_from_start: 0.5537142860000001 + - positions: [0.19934, 6.994652308522565e-18, 0.02599999999999997, 0.02599999999999996, 0.106, 0.02599999999999996] + velocities: [0.6123499985252059, 2.148678303806622e-17, 0.07986906773179159, 0.07986906773179156, 0.018431323322721, 0.07986906773179156] + accelerations: [0.9678887153853143, 3.396230077988781e-17, 0.1262421320358088, 0.1262421320358088, 0.02913279970057122, 0.1262421320358088] + time_from_start: 0.640716719 + - positions: [0.249175, 8.743315385653206e-18, 0.03249999999999997, 0.03249999999999995, 0.1075, 0.03249999999999995] + velocities: [0.6824366302586928, 2.394605674355035e-17, 0.08901049657231863, 0.0890104965723186, 0.02054088382438118, 0.0890104965723186] + accelerations: [0.8346601496145684, 2.928743625129594e-17, 0.1088650741947364, 0.108865074194737, 0.02512270942955706, 0.108865074194737] + time_from_start: 0.7171625020000001 + - positions: [0.29901, 1.049197846278385e-17, 0.03899999999999996, 0.03899999999999994, 0.109, 0.03899999999999994] + velocities: [0.742510652237474, 2.605399743039752e-17, 0.0968459765133656, 0.09684597651336554, 0.02234907150308431, 0.09684597651336554] + accelerations: [0.8787824499975725, 3.083564609493623e-17, 0.1146199643821446, 0.1146199643821431, 0.026450761011261, 0.1146199643821431] + time_from_start: 0.7870599260000001 + - positions: [0.348845, 1.224064153991449e-17, 0.04549999999999995, 0.04549999999999993, 0.1104999999999999, 0.04549999999999993] + velocities: [0.7957962168487385, 2.792373756042215e-17, 0.1037960351061863, 0.1037960351061862, 0.0239529311783505, 0.1037960351061862] + accelerations: [0.7577782655792028, 2.658972356116122e-17, 0.09883733773983946, 0.09883733773984102, 0.02280861640150083, 0.09883733773984102] + time_from_start: 0.851609025 + - positions: [0.39868, 1.398930461704513e-17, 0.05199999999999995, 0.05199999999999993, 0.1119999999999999, 0.05199999999999993] + velocities: [0.8491035348048623, 2.979424099477027e-17, 0.1107489309969219, 0.1107489309969218, 0.02555744561467421, 0.1107489309969218] + accelerations: [1.006051549039789, 3.530139856518503e-17, 0.1312197264725321, 0.1312197264725299, 0.03028147533981946, 0.1312197264725299] + time_from_start: 0.9124171950000001 + - positions: [0.448515, 1.573796769417577e-17, 0.05849999999999994, 0.05849999999999991, 0.1134999999999999, 0.05849999999999991] + velocities: [0.9010729108773707, 3.161779731220803e-17, 0.1175273185653236, 0.1175273185653236, 0.02712168889969, 0.1175273185653236] + accelerations: [0.8099060539199602, 2.841883841545754e-17, 0.1056363870869792, 0.1056363870869815, 0.02437762778929836, 0.1056363870869815] + time_from_start: 0.9691340740000001 + - positions: [0.49835, 1.748663077130641e-17, 0.06499999999999993, 0.06499999999999991, 0.1149999999999999, 0.06499999999999991] + velocities: [0.9234832224823122, 3.240415397821691e-17, 0.1204503049289661, 0.120450304928966, 0.02779622421437657, 0.120450304928966] + accelerations: [0, 1.142049718514557e-30, 4.886167952403701e-15, 0, 0, 0] + time_from_start: 1.023098238 + - positions: [0.548185, 1.923529384843706e-17, 0.07149999999999994, 0.0714999999999999, 0.1164999999999999, 0.0714999999999999] + velocities: [0.9010729108773707, 3.161779731220804e-17, 0.1175273185653236, 0.1175273185653236, 0.02712168889969, 0.1175273185653236] + accelerations: [-0.8099060539199602, -2.841883841545966e-17, -0.1056363870869885, -0.1056363870869792, -0.02437762778929836, -0.1056363870869792] + time_from_start: 1.077062402 + - positions: [0.59802, 2.098395692556769e-17, 0.07799999999999992, 0.07799999999999989, 0.1179999999999999, 0.07799999999999989] + velocities: [0.8491035348048623, 2.979424099477023e-17, 0.1107489309969218, 0.1107489309969218, 0.0255574456146742, 0.1107489309969218] + accelerations: [-1.006051549039787, -3.53013985651846e-17, -0.1312197264725277, -0.1312197264725358, -0.03028147533981985, -0.1312197264725358] + time_from_start: 1.13377928 + - velocities: [0.7957962168487385, 2.792373756042214e-17, 0.1037960351061863, 0.1037960351061862, 0.0239529311783505, 0.1037960351061862] + accelerations: [-0.7577782655792028, -2.658972356116053e-17, -0.0988373377398408, -0.09883733773983724, -0.02280861640149994, -0.09883733773983724] + time_from_start: 1.194587451 + positions: [0.6478550000000001, 2.273262000269834e-17, 0.08449999999999992, 0.08449999999999988, 0.1194999999999999, 0.08449999999999988] + - positions: [0.69769, 2.448128307982898e-17, 0.0909999999999999, 0.09099999999999986, 0.1209999999999999, 0.09099999999999986] + velocities: [0.7425106522374741, 2.605399743039755e-17, 0.0968459765133656, 0.0968459765133657, 0.02234907150308434, 0.0968459765133657] + accelerations: [-0.8787824499975736, -3.083564609493586e-17, -0.1146199643821424, -0.1146199643821393, -0.02645076101126124, -0.1146199643821393] + time_from_start: 1.259136549 + - positions: [0.747525, 2.622994615695962e-17, 0.09749999999999989, 0.09749999999999986, 0.1224999999999999, 0.09749999999999986] + velocities: [0.6824366302586929, 2.394605674355036e-17, 0.08901049657231869, 0.0890104965723187, 0.02054088382438117, 0.0890104965723187] + accelerations: [-0.8346601496145696, -2.928743625129669e-17, -0.1088650741947368, -0.1088650741947421, -0.02512270942955776, -0.1088650741947421] + time_from_start: 1.329033973 + - positions: [0.7973600000000001, 2.797860923409026e-17, 0.1039999999999999, 0.1039999999999999, 0.1239999999999999, 0.1039999999999999] + velocities: [0.612349998525206, 2.14867830380662e-17, 0.07986906773179156, 0.07986906773179148, 0.018431323322721, 0.07986906773179148] + accelerations: [-0.9678887153853116, -3.396230077988799e-17, -0.1262421320358105, -0.1262421320358083, -0.02913279970057049, -0.1262421320358083] + time_from_start: 1.405479757 + - positions: [0.847195, 2.97272723112209e-17, 0.1104999999999999, 0.1104999999999998, 0.1254999999999999, 0.1104999999999998] + velocities: [0.5306480814296295, 1.861993994072857e-17, 0.06921265233856906, 0.06921265233856899, 0.01597215053966974, 0.06921265233856899] + accelerations: [-0.8920131096561228, -3.129989744501014e-17, -0.116345644883408, -0.1163456448834095, -0.02684899497309323, -0.1163456448834095] + time_from_start: 1.492482189 + - positions: [0.89703, 3.147593538835155e-17, 0.1169999999999999, 0.1169999999999998, 0.1269999999999999, 0.1169999999999998] + velocities: [0.4351040696195361, 1.526739081474795e-17, 0.05675080671269156, 0.05675080671269155, 0.01309634001062105, 0.05675080671269155] + accelerations: [-0.9182811431153932, -3.222161792698415e-17, -0.119771795530253, -0.1197717955302509, -0.02763964512236749, -0.1197717955302509] + time_from_start: 1.594499383 + - positions: [0.946865, 3.322459846548218e-17, 0.1234999999999999, 0.1234999999999998, 0.1284999999999998, 0.1234999999999998] + velocities: [0.2687633854955201, 9.430653330001891e-18, 0.03505492135488868, 0.03505492135488872, 0.00808959723574346, 0.03505492135488872] + accelerations: [-1.003115394662581, -3.519837168150461e-17, -0.1308367626227903, -0.1308367626227909, -0.03019309906679702, -0.1308367626227909] + time_from_start: 1.72505591 + - positions: [0.9967, 3.497326154261283e-17, 0.1299999999999999, 0.1299999999999998, 0.1299999999999998, 0.1299999999999998] + velocities: [0, 0, 0, 0, 0, 0] + accelerations: [-0.9743443773410309, -3.418882386006029e-17, -0.1270841467385712, -0.127084146738571, -0.02932711078582413, -0.127084146738571] + time_from_start: 2.04489091 + header: + frame_id: base_link \ No newline at end of file diff --git a/stretch/config/stretch_pick.yml b/stretch/config/stretch_pick.yml new file mode 100644 index 0000000..163f0fa --- /dev/null +++ b/stretch/config/stretch_pick.yml @@ -0,0 +1,153 @@ +joint_trajectory: + points: + - positions: [0, 0, 0, 0, 0.1, 0] + velocities: [0, 0, 0, 0, 0, 0] + accelerations: [0, 0, 0, 0, 0, 0] + time_from_start: 0 + - positions: [0.03108750978255144, 0.002482273220601527, 0.001233382171790044, 0.002390735418240729, 0.09804999297292596, -0.03143866353619015] + velocities: [0.2406119589541576, 0.01921236620257077, 0.009546165085892558, 0.01850387941483995, -0.01509271775193793, -0.2433298283862257] + accelerations: [0.9834392190463783, 0.07852558325203207, 0.03901748349402132, 0.07562982654792154, -0.06168758453972421, -0.9945478081761994] + time_from_start: 0.181438664 + - positions: [0.06217501956510287, 0.004964546441203054, 0.002466764343580088, 0.004781470836481457, 0.09609998594585192, -0.06287732707238029] + velocities: [0.3528787974388098, 0.02817664055845573, 0.01400029853172811, 0.02713758179037775, -0.02213481039569586, -0.356864794231956] + accelerations: [0.96156930573919, 0.07677931600454145, 0.03814980508039613, 0.07394795570322947, -0.06031576399414764, -0.9724308598956956] + time_from_start: 0.281758178 + - positions: [0.0932625293476543, 0.00744681966180458, 0.003700146515370132, 0.007172206254722185, 0.09414997891877788, -0.09431599060857043] + velocities: [0.4277342266921027, 0.03415369143039056, 0.0169701521014895, 0.03289421933436996, -0.02683022067717466, -0.4325657645127031] + accelerations: [0.8719050790427344, 0.06961981335118116, 0.03459241951209912, 0.06705247118189656, -0.05469145142108372, -0.8817538171199922] + time_from_start: 0.36028725 + - positions: [0.1243500391302057, 0.009929092882406109, 0.004933528687160176, 0.009562941672962915, 0.09219997189170384, -0.1257546541447606] + velocities: [0.488739955540474, 0.03902487243146853, 0.0193905253917577, 0.03758576773093057, -0.03065688935466607, -0.4942605929650121] + accelerations: [0.9131159169221179, 0.07291041333754403, 0.03622743991355552, 0.07022172502122767, -0.05727645819771247, -0.9234301583643256] + time_from_start: 0.4279282250000001 + - positions: [0.1554375489127572, 0.01241136610300764, 0.00616691085895022, 0.01195367709120364, 0.0902499648646298, -0.1571933176809507] + velocities: [0.5421431386816502, 0.04328900591573438, 0.0215092712953017, 0.04169265446053491, -0.03400667784277022, -0.5482669999844421] + accelerations: [0.8444257695658784, 0.06742564744621943, 0.03350219097211848, 0.06493921866039659, -0.05296777374622946, -0.8539641108716576] + time_from_start: 0.4879561550000001 + - positions: [0.1865250586953086, 0.01489363932360916, 0.007400293030740263, 0.01434441250944437, 0.08829995783755576, -0.1886319812171409] + velocities: [0.5868310053104301, 0.04685723944084919, 0.02328224115943038, 0.04512930365702372, -0.03680978605441536, -0.5934596452918159] + accelerations: [0.7703242462240363, 0.06150879439868406, 0.03056224826099216, 0.05924055904957411, -0.0483196532552598, -0.7790255623626566] + time_from_start: 0.542842083 + - positions: [0.21761256847786, 0.01737591254421069, 0.008633675202530309, 0.0167351479276851, 0.08634995081048172, -0.220070644753331] + velocities: [0.6259391738482052, 0.04997994563851886, 0.02483383915435647, 0.04813685506013624, -0.03926289998982167, -0.6330095661692292] + accelerations: [0.7515384360612754, 0.06000878639480652, 0.02981693017345427, 0.05779586624430502, -0.04714128734293947, -0.7600275541885342] + time_from_start: 0.5940351780000001 + - positions: [0.2487000782604115, 0.01985818576481222, 0.009867057374320353, 0.01912588334592583, 0.08439994378340768, -0.2515093082895212] + velocities: [0.6678684238937127, 0.05332790934728411, 0.02649736221697505, 0.05136135724269211, -0.04189297016271711, -0.675412434514939] + accelerations: [0.9977673444941729, 0.07966965437092376, 0.03958594505964983, 0.07673170821115657, -0.06258633601590961, -1.009037779171261] + time_from_start: 0.64226139 + - positions: [0.2797875880429629, 0.02234045898541374, 0.0111004395461104, 0.02151661876416656, 0.08244993675633364, -0.2829479718257114] + velocities: [0.7087453062355851, 0.05659184367617207, 0.02811913309124181, 0.05450492876339413, -0.04445702911658898, -0.7167510479755719] + accelerations: [0.8032369847060132, 0.06413680834775592, 0.03186804551372781, 0.06177166077340216, -0.05038415027573712, -0.8123100717500553] + time_from_start: 0.687242848 + - positions: [0.3108750978255144, 0.02482273220601527, 0.01233382171790044, 0.02390735418240729, 0.0804999297292596, -0.3143866353619014] + velocities: [0.7411191101938852, 0.05917682482058645, 0.02940354837295746, 0.05699458458618801, -0.04648772072403436, -0.7494905352214384] + accelerations: [0.7028429451030334, 0.05612055237364794, 0.02788495971925814, 0.0540510170828015, -0.04408679535501367, -0.7107820158140605] + time_from_start: 0.730041163 + - positions: [0.3419626076080658, 0.0273050054266168, 0.01356720388969048, 0.02629808960064801, 0.07854992270218555, -0.3458252988980916] + velocities: [0.7558659241832895, 0.06035432735170377, 0.02998862120743206, 0.05812866482475583, -0.0474127350177951, -0.7644039241189917] + accelerations: [3.779175822697827e-14, 1.012279238222632e-15, 4.217830159260967e-16, 8.435660318521935e-16, 5.904962222965354e-15, -5.398822603854038e-15] + time_from_start: 0.771169501 + - positions: [0.3730501173906172, 0.02978727864721832, 0.01480058606148053, 0.02868882501888874, 0.07659991567511153, -0.3772639624342817] + velocities: [0.7411191101938853, 0.05917682482058641, 0.02940354837295743, 0.05699458458618801, -0.04648772072403431, -0.7494905352214385] + accelerations: [-0.7028429451031015, -0.05612055237365186, -0.02788495971926002, -0.05405101708280311, 0.04408679535500405, 0.7107820158140653] + time_from_start: 0.81229784 + - positions: [0.4041376271731687, 0.03226955186781985, 0.01603396823327057, 0.03107956043712947, 0.07464990864803747, -0.4087026259704719] + velocities: [0.7087453062355841, 0.05659184367617209, 0.02811913309124182, 0.05450492876339416, -0.04445702911658894, -0.716751047975572] + accelerations: [-0.8032369847059975, -0.06413680834774893, -0.0318680455137243, -0.06177166077339895, 0.05038415027574593, 0.8123100717500522] + time_from_start: 0.8550961540000001 + - positions: [0.4352251369557201, 0.03475182508842138, 0.01726735040506062, 0.0334702958553702, 0.07269990162096344, -0.440141289506662] + velocities: [0.6678684238937123, 0.05332790934728418, 0.02649736221697508, 0.05136135724269215, -0.041892970162717, -0.675412434514939] + accelerations: [-0.9977673444941624, -0.07966965437092847, -0.03958594505965219, -0.07673170821115981, 0.06258633601590419, 1.009037779171267] + time_from_start: 0.9000776130000001 + - positions: [0.4663126467382715, 0.0372340983090229, 0.01850073257685066, 0.03586103127361093, 0.0707498945938894, -0.4715799530428522] + velocities: [0.6259391738482054, 0.04997994563851882, 0.02483383915435644, 0.04813685506013618, -0.03926289998982166, -0.6330095661692292] + accelerations: [-0.7515384360612662, -0.06000878639480692, -0.02981693017345454, -0.05779586624430542, 0.04714128734294099, 0.760027554188534] + time_from_start: 0.9483038250000001 + - positions: [0.497400156520823, 0.03971637152962443, 0.01973411474864071, 0.03825176669185166, 0.06879988756681536, -0.5030186165790423] + velocities: [0.5868310053104309, 0.04685723944084919, 0.02328224115943038, 0.04512930365702372, -0.03680978605441538, -0.5934596452918158] + accelerations: [-0.770324246224018, -0.06150879439868161, -0.03056224826099087, -0.05924055904957167, 0.04831965325525734, 0.7790255623626571] + time_from_start: 0.99949692 + - positions: [0.5284876663033744, 0.04219864475022596, 0.02096749692043075, 0.04064250211009238, 0.06684988053974132, -0.5344572801152324] + velocities: [0.5421431386816498, 0.04328900591573434, 0.02150927129530168, 0.04169265446053487, -0.03400667784277029, -0.5482669999844421] + accelerations: [-0.844425769565937, -0.06742564744622355, -0.03350219097212048, -0.06493921866040035, 0.05296777374622987, 0.8539641108716544] + time_from_start: 1.054382848 + - positions: [0.5595751760859259, 0.04468091797082749, 0.02220087909222079, 0.04303323752833312, 0.06489987351266727, -0.5658959436514227] + velocities: [0.4887399555404729, 0.03902487243146845, 0.01939052539175766, 0.03758576773093049, -0.03065688935466603, -0.494260592965012] + accelerations: [-0.9131159169220852, -0.07291041333754131, -0.03622743991355427, -0.07022172502122528, 0.05727645819771553, 0.9234301583643285] + time_from_start: 1.114410778 + - positions: [0.5906626858684773, 0.04716319119142901, 0.02343426126401084, 0.04542397294657384, 0.06294986648559324, -0.5973346071876128] + velocities: [0.4238340993678461, 0.0338422743473995, 0.01681541640399889, 0.03259428625529838, -0.02658557979915055, -0.4286215826997085] + accelerations: [-0.9681785859074595, -0.07730705332682684, -0.03841202512905671, -0.07445623186616097, 0.06073034022949519, 0.9791147962058293] + time_from_start: 1.182051753 + - positions: [0.6217501956510287, 0.04964546441203054, 0.02466764343580088, 0.04781470836481457, 0.0609998594585192, -0.6287732707238028] + velocities: [0.3429328677315833, 0.02738247868640735, 0.01360569849883467, 0.02637270590984824, -0.02151093820536807, -0.3468065187442358] + accelerations: [-0.9786321106565957, -0.0781417456108716, -0.03882676375392979, -0.07526014353480263, 0.06138605202053714, 0.9896864003533685] + time_from_start: 1.262159259 + - positions: [0.6528377054335802, 0.05212773763263207, 0.02590102560759093, 0.0502054437830553, 0.05904985243144516, -0.6602119342599931] + velocities: [0.2061610186630611, 0.01646153002729342, 0.008179340407632518, 0.01585448473119601, -0.01293173489654523, -0.208489742194277] + accelerations: [-0.9752491032728539, -0.07787161948328859, -0.0386925445442431, -0.07499997874099786, 0.06117384820565818, 0.9862651796888373] + time_from_start: 1.366552148 + - positions: [0.6839252152161316, 0.0546100108532336, 0.02713440777938097, 0.05259617920129603, 0.05709984540437112, -0.6916505977961832] + velocities: [0.06801508899771461, 0.006490945118556335, 0.007623636897620131, 0.006074403334841541, 0.00478029448368332, 0.02686727117849011] + accelerations: [-0.3249955194316414, -0.0185433144886968, 0.02151866536719045, -0.01909746069011686, 0.08359562599262314, 0.9969880559420263] + time_from_start: 1.637990812 + - positions: [0.69039811664906, 0.0557651210248394, 0.03035660417286947, 0.05360201618192553, 0.06214069591297353, -0.6406064957921845] + velocities: [0.0349615322268157, 0.006238998370140282, 0.01740377545052668, 0.005432741773930017, 0.02722671731266173, 0.2757001687254669] + accelerations: [0.1238494556996118, 0.02210133546892978, 0.06165199232923763, 0.0192452123463203, 0.09644926594704739, 0.9766538723594954] + time_from_start: 1.939034914 + - positions: [0.6968710180819884, 0.0569202311964452, 0.03357880056635798, 0.05460785316255504, 0.06718154642157595, -0.5895623937881859] + velocities: [0.05580642734960452, 0.009958837244852749, 0.02778031935178442, 0.008671871334158615, 0.04345993224266811, 0.4400791514638015] + accelerations: [0.1251081417205459, 0.02232595205558542, 0.06227856351978898, 0.01944080206137537, 0.09742948295397734, 0.9865796372284258] + time_from_start: 2.072713002 + - positions: [0.7033439195149168, 0.05807534136805099, 0.03680099695984648, 0.05561369014318454, 0.07222239693017836, -0.5385182917841872] + velocities: [0.06864402990570895, 0.01224974889324476, 0.0341708502575833, 0.01066673183487105, 0.05345737095615993, 0.5413141078663646] + accelerations: [0.1149209091011128, 0.0205080074844959, 0.05720738106072776, 0.01785778779720716, 0.08949605197825147, 0.9062450073330172] + time_from_start: 2.175146436 + - positions: [0.7098168209478452, 0.05923045153965679, 0.04002319335333498, 0.05661952712381405, 0.07726324743878077, -0.4874741897801885] + velocities: [0.07879542617011087, 0.01406129835101349, 0.03922419345629634, 0.01224417741681933, 0.06136289393571885, 0.6213661389025791] + accelerations: [0.1139880126604629, 0.02034152910090655, 0.05674298722161352, 0.01771282317062431, 0.08876954755883749, 0.8988883587619665] + time_from_start: 2.262503847 + - positions: [0.7162897223807736, 0.06038556171126259, 0.04324538974682349, 0.05762536410444355, 0.08230409794738319, -0.4364300877761898] + velocities: [0.08186513305173945, 0.01460909746081909, 0.04075228693115336, 0.01272118525221257, 0.06375346540600267, 0.6455732789018263] + accelerations: [-0.04118766304602151, -0.007350071528524332, -0.02050312996393328, -0.006400232579879647, -0.03207539221243461, -0.3247982833681933] + time_from_start: 2.340029113 + - positions: [0.7227626238137019, 0.06154067188286839, 0.046467586140312, 0.05863120108507305, 0.0873449484559856, -0.3853859857721912] + velocities: [0.07536835314205033, 0.01344972609789518, 0.03751820388335726, 0.01171163774776065, 0.05869401924397165, 0.5943408756520722] + accelerations: [-0.1128858492049099, -0.02014484447169789, -0.05619433262703505, -0.01754155580717851, -0.08791122439836403, -0.8901969018578508] + time_from_start: 2.420702216 + - positions: [0.7292355252466304, 0.06269578205447418, 0.04968978253380051, 0.05963703806570256, 0.09238579896458801, -0.3343418837681925] + velocities: [0.06453664238886903, 0.01151677232182767, 0.03212620159723226, 0.01002847674926184, 0.05025869310391234, 0.5089240105426238] + accelerations: [-0.1179076575354431, -0.02104100239137579, -0.05869417799916539, -0.01832190455507724, -0.09182201855137835, -0.9297979523795099] + time_from_start: 2.512515733 + - positions: [0.7357084266795587, 0.06385089222607998, 0.05291197892728901, 0.06064287504633206, 0.09742664947319044, -0.2832977817641938] + velocities: [0.05071356842581253, 0.009050000117270585, 0.02524510514733085, 0.007880481893772631, 0.03949380657827314, 0.3999178091211607] + accelerations: [-0.1201919506224419, -0.02144864187223107, -0.05983129417850449, -0.01867686538451752, -0.093600939501869, -0.9478114646427156] + time_from_start: 2.623026179 + - positions: [0.7421813281124872, 0.06500600239768578, 0.05613417532077751, 0.06164871202696157, 0.1024674999817928, -0.2322536797601951] + velocities: [-0.01067218199458236, 0.005481832302697443, 0.02117577169958657, 0.008337210032498616, 0.04415542412467895, 0.4006773215831587] + accelerations: [-0.8520752548930834, -0.03447473611731797, -0.002499984547507279, 0.02671136810350283, 0.1716367008642085, 0.9986716928269778] + time_from_start: 2.774070281 + - positions: [0.735746551885849, 0.06533838833764165, 0.05824092667611494, 0.06265255604106233, 0.1079739999854343, -0.1858048192743558] + velocities: [-0.06985498918408897, 0.0036083331296652, 0.02287058445504782, 0.0108975837352712, 0.0597777583320131, 0.504242033067055] + accelerations: [-0.1220015649269252, 0.006301944839425939, 0.03994341888674777, 0.01903260289857896, 0.1044017062991727, 0.8806574570356843] + time_from_start: 2.8743024 + - time_from_start: 2.959518545 + positions: [0.7293117756592109, 0.06567077427759752, 0.06034767803145238, 0.0636564000551631, 0.1134804999890757, -0.1393559587885165] + velocities: [-0.07059729614521942, 0.00364667671587884, 0.02311361640229162, 0.01101338580410956, 0.06041298062105654, 0.5096003099147377] + accelerations: [0.107301313297223, -0.005542608883762243, -0.03513054366717594, -0.01673932041531671, -0.09182210247124496, -0.7745449967098517] + - positions: [0.7228769994325728, 0.0660031602175534, 0.06245442938678981, 0.06466024406926388, 0.1189869999927171, -0.09290709830267727] + velocities: [-0.05797052601310691, 0.002994445665233337, 0.01897960083555911, 0.0090435725319595, 0.04960773933067173, 0.4184550915580654] + accelerations: [0.1365089447941442, -0.007051318076914533, -0.0446931477228689, -0.02129579681971558, -0.116816261907332, -0.9853776896799766] + time_from_start: 3.057485159 + - positions: [0.7164422232059346, 0.06633554615750926, 0.06456118074212724, 0.06566408808336464, 0.1244934999963585, -0.04645823781683794] + velocities: [-0.03562778648253932, 0.001840339878412955, 0.01166456840395478, 0.005558039461901023, 0.03048814744849887, 0.2571760113267114] + accelerations: [0.1346874914595742, -0.006957231591638078, -0.0440968023106696, -0.02101164474317392, -0.1152575701299996, -0.9722296906138025] + time_from_start: 3.185520804 + - positions: [0.7100074469792965, 0.06666793209746513, 0.06666793209746467, 0.06666793209746541, 0.1299999999999999, -9.377330998666325e-06] + velocities: [0, 0, 0, 0, 0, 0] + accelerations: [0.1370400229617853, -0.007078750719436368, -0.04486702317868338, -0.0213786465756078, -0.1172707271177851, -0.9892112302487599] + time_from_start: 3.491969665 + joint_names: [joint_lift, joint_arm_l3, joint_arm_l2, joint_arm_l1, joint_arm_l0, joint_wrist_yaw] + header: + frame_id: base_link \ No newline at end of file diff --git a/stretch/meshes/base_link.STL b/stretch/meshes/base_link.STL new file mode 100644 index 0000000..4d5884c Binary files /dev/null and b/stretch/meshes/base_link.STL differ diff --git a/stretch/meshes/guthrie/base_link.STL b/stretch/meshes/guthrie/base_link.STL new file mode 100644 index 0000000..4d5884c Binary files /dev/null and b/stretch/meshes/guthrie/base_link.STL differ diff --git a/stretch/meshes/guthrie/laser.STL b/stretch/meshes/guthrie/laser.STL new file mode 100644 index 0000000..5646371 Binary files /dev/null and b/stretch/meshes/guthrie/laser.STL differ diff --git a/stretch/meshes/guthrie/link_arm_l0.STL b/stretch/meshes/guthrie/link_arm_l0.STL new file mode 100644 index 0000000..c333f85 Binary files /dev/null and b/stretch/meshes/guthrie/link_arm_l0.STL differ diff --git a/stretch/meshes/guthrie/link_arm_l1.STL b/stretch/meshes/guthrie/link_arm_l1.STL new file mode 100644 index 0000000..3b6fc65 Binary files /dev/null and b/stretch/meshes/guthrie/link_arm_l1.STL differ diff --git a/stretch/meshes/guthrie/link_arm_l2.STL b/stretch/meshes/guthrie/link_arm_l2.STL new file mode 100644 index 0000000..d303727 Binary files /dev/null and b/stretch/meshes/guthrie/link_arm_l2.STL differ diff --git a/stretch/meshes/guthrie/link_arm_l3.STL b/stretch/meshes/guthrie/link_arm_l3.STL new file mode 100644 index 0000000..3557a2f Binary files /dev/null and b/stretch/meshes/guthrie/link_arm_l3.STL differ diff --git a/stretch/meshes/guthrie/link_arm_l4.STL b/stretch/meshes/guthrie/link_arm_l4.STL new file mode 100644 index 0000000..9a454e8 Binary files /dev/null and b/stretch/meshes/guthrie/link_arm_l4.STL differ diff --git a/stretch/meshes/guthrie/link_aruco_inner_wrist.STL b/stretch/meshes/guthrie/link_aruco_inner_wrist.STL new file mode 100644 index 0000000..10db01e Binary files /dev/null and b/stretch/meshes/guthrie/link_aruco_inner_wrist.STL differ diff --git a/stretch/meshes/guthrie/link_aruco_left_base.STL b/stretch/meshes/guthrie/link_aruco_left_base.STL new file mode 100644 index 0000000..8ea0795 Binary files /dev/null and b/stretch/meshes/guthrie/link_aruco_left_base.STL differ diff --git a/stretch/meshes/guthrie/link_aruco_right_base.STL b/stretch/meshes/guthrie/link_aruco_right_base.STL new file mode 100644 index 0000000..8ea0795 Binary files /dev/null and b/stretch/meshes/guthrie/link_aruco_right_base.STL differ diff --git a/stretch/meshes/guthrie/link_aruco_shoulder.STL b/stretch/meshes/guthrie/link_aruco_shoulder.STL new file mode 100644 index 0000000..98d6567 Binary files /dev/null and b/stretch/meshes/guthrie/link_aruco_shoulder.STL differ diff --git a/stretch/meshes/guthrie/link_aruco_top_wrist.STL b/stretch/meshes/guthrie/link_aruco_top_wrist.STL new file mode 100644 index 0000000..2a53165 Binary files /dev/null and b/stretch/meshes/guthrie/link_aruco_top_wrist.STL differ diff --git a/stretch/meshes/guthrie/link_dry_erase_holder.STL b/stretch/meshes/guthrie/link_dry_erase_holder.STL new file mode 100644 index 0000000..a123087 Binary files /dev/null and b/stretch/meshes/guthrie/link_dry_erase_holder.STL differ diff --git a/stretch/meshes/guthrie/link_dry_erase_marker.STL b/stretch/meshes/guthrie/link_dry_erase_marker.STL new file mode 100644 index 0000000..5929498 Binary files /dev/null and b/stretch/meshes/guthrie/link_dry_erase_marker.STL differ diff --git a/stretch/meshes/guthrie/link_gripper.STL b/stretch/meshes/guthrie/link_gripper.STL new file mode 100644 index 0000000..7ae7d2e Binary files /dev/null and b/stretch/meshes/guthrie/link_gripper.STL differ diff --git a/stretch/meshes/guthrie/link_gripper_finger_left.STL b/stretch/meshes/guthrie/link_gripper_finger_left.STL new file mode 100644 index 0000000..60848f1 Binary files /dev/null and b/stretch/meshes/guthrie/link_gripper_finger_left.STL differ diff --git a/stretch/meshes/guthrie/link_gripper_finger_right.STL b/stretch/meshes/guthrie/link_gripper_finger_right.STL new file mode 100644 index 0000000..6efbb40 Binary files /dev/null and b/stretch/meshes/guthrie/link_gripper_finger_right.STL differ diff --git a/stretch/meshes/guthrie/link_gripper_fingertip_left.STL b/stretch/meshes/guthrie/link_gripper_fingertip_left.STL new file mode 100644 index 0000000..853c3be Binary files /dev/null and b/stretch/meshes/guthrie/link_gripper_fingertip_left.STL differ diff --git a/stretch/meshes/guthrie/link_gripper_fingertip_right.STL b/stretch/meshes/guthrie/link_gripper_fingertip_right.STL new file mode 100644 index 0000000..c40a44f Binary files /dev/null and b/stretch/meshes/guthrie/link_gripper_fingertip_right.STL differ diff --git a/stretch/meshes/guthrie/link_head.STL b/stretch/meshes/guthrie/link_head.STL new file mode 100644 index 0000000..0614101 Binary files /dev/null and b/stretch/meshes/guthrie/link_head.STL differ diff --git a/stretch/meshes/guthrie/link_head_pan.STL b/stretch/meshes/guthrie/link_head_pan.STL new file mode 100644 index 0000000..eaa3ebd Binary files /dev/null and b/stretch/meshes/guthrie/link_head_pan.STL differ diff --git a/stretch/meshes/guthrie/link_head_tilt.STL b/stretch/meshes/guthrie/link_head_tilt.STL new file mode 100644 index 0000000..7fa1faa Binary files /dev/null and b/stretch/meshes/guthrie/link_head_tilt.STL differ diff --git a/stretch/meshes/guthrie/link_left_wheel.STL b/stretch/meshes/guthrie/link_left_wheel.STL new file mode 100644 index 0000000..97837f2 Binary files /dev/null and b/stretch/meshes/guthrie/link_left_wheel.STL differ diff --git a/stretch/meshes/guthrie/link_lift.STL b/stretch/meshes/guthrie/link_lift.STL new file mode 100644 index 0000000..e9f16bf Binary files /dev/null and b/stretch/meshes/guthrie/link_lift.STL differ diff --git a/stretch/meshes/guthrie/link_mast.STL b/stretch/meshes/guthrie/link_mast.STL new file mode 100644 index 0000000..9ba50d8 Binary files /dev/null and b/stretch/meshes/guthrie/link_mast.STL differ diff --git a/stretch/meshes/guthrie/link_puller.STL b/stretch/meshes/guthrie/link_puller.STL new file mode 100644 index 0000000..33dd044 Binary files /dev/null and b/stretch/meshes/guthrie/link_puller.STL differ diff --git a/stretch/meshes/guthrie/link_respeaker.STL b/stretch/meshes/guthrie/link_respeaker.STL new file mode 100644 index 0000000..34ba67f Binary files /dev/null and b/stretch/meshes/guthrie/link_respeaker.STL differ diff --git a/stretch/meshes/guthrie/link_right_wheel.STL b/stretch/meshes/guthrie/link_right_wheel.STL new file mode 100644 index 0000000..f9e5483 Binary files /dev/null and b/stretch/meshes/guthrie/link_right_wheel.STL differ diff --git a/stretch/meshes/guthrie/link_wrist_yaw.STL b/stretch/meshes/guthrie/link_wrist_yaw.STL new file mode 100644 index 0000000..39498ae Binary files /dev/null and b/stretch/meshes/guthrie/link_wrist_yaw.STL differ diff --git a/stretch/meshes/guthrie/omni_wheel_m.STL b/stretch/meshes/guthrie/omni_wheel_m.STL new file mode 100644 index 0000000..19d2bb8 Binary files /dev/null and b/stretch/meshes/guthrie/omni_wheel_m.STL differ diff --git a/stretch/meshes/hank/base_link.STL b/stretch/meshes/hank/base_link.STL new file mode 100644 index 0000000..4d5884c Binary files /dev/null and b/stretch/meshes/hank/base_link.STL differ diff --git a/stretch/meshes/hank/laser.STL b/stretch/meshes/hank/laser.STL new file mode 100644 index 0000000..5646371 Binary files /dev/null and b/stretch/meshes/hank/laser.STL differ diff --git a/stretch/meshes/hank/link_arm_l0.STL b/stretch/meshes/hank/link_arm_l0.STL new file mode 100644 index 0000000..c333f85 Binary files /dev/null and b/stretch/meshes/hank/link_arm_l0.STL differ diff --git a/stretch/meshes/hank/link_arm_l1.STL b/stretch/meshes/hank/link_arm_l1.STL new file mode 100644 index 0000000..3b6fc65 Binary files /dev/null and b/stretch/meshes/hank/link_arm_l1.STL differ diff --git a/stretch/meshes/hank/link_arm_l2.STL b/stretch/meshes/hank/link_arm_l2.STL new file mode 100644 index 0000000..d303727 Binary files /dev/null and b/stretch/meshes/hank/link_arm_l2.STL differ diff --git a/stretch/meshes/hank/link_arm_l3.STL b/stretch/meshes/hank/link_arm_l3.STL new file mode 100644 index 0000000..3557a2f Binary files /dev/null and b/stretch/meshes/hank/link_arm_l3.STL differ diff --git a/stretch/meshes/hank/link_arm_l4.STL b/stretch/meshes/hank/link_arm_l4.STL new file mode 100644 index 0000000..9a454e8 Binary files /dev/null and b/stretch/meshes/hank/link_arm_l4.STL differ diff --git a/stretch/meshes/hank/link_aruco_inner_wrist.STL b/stretch/meshes/hank/link_aruco_inner_wrist.STL new file mode 100644 index 0000000..10db01e Binary files /dev/null and b/stretch/meshes/hank/link_aruco_inner_wrist.STL differ diff --git a/stretch/meshes/hank/link_aruco_left_base.STL b/stretch/meshes/hank/link_aruco_left_base.STL new file mode 100644 index 0000000..8ea0795 Binary files /dev/null and b/stretch/meshes/hank/link_aruco_left_base.STL differ diff --git a/stretch/meshes/hank/link_aruco_right_base.STL b/stretch/meshes/hank/link_aruco_right_base.STL new file mode 100644 index 0000000..8ea0795 Binary files /dev/null and b/stretch/meshes/hank/link_aruco_right_base.STL differ diff --git a/stretch/meshes/hank/link_aruco_shoulder.STL b/stretch/meshes/hank/link_aruco_shoulder.STL new file mode 100644 index 0000000..98d6567 Binary files /dev/null and b/stretch/meshes/hank/link_aruco_shoulder.STL differ diff --git a/stretch/meshes/hank/link_aruco_top_wrist.STL b/stretch/meshes/hank/link_aruco_top_wrist.STL new file mode 100644 index 0000000..2a53165 Binary files /dev/null and b/stretch/meshes/hank/link_aruco_top_wrist.STL differ diff --git a/stretch/meshes/hank/link_dry_erase_holder.STL b/stretch/meshes/hank/link_dry_erase_holder.STL new file mode 100644 index 0000000..a123087 Binary files /dev/null and b/stretch/meshes/hank/link_dry_erase_holder.STL differ diff --git a/stretch/meshes/hank/link_dry_erase_marker.STL b/stretch/meshes/hank/link_dry_erase_marker.STL new file mode 100644 index 0000000..5929498 Binary files /dev/null and b/stretch/meshes/hank/link_dry_erase_marker.STL differ diff --git a/stretch/meshes/hank/link_gripper.STL b/stretch/meshes/hank/link_gripper.STL new file mode 100644 index 0000000..7ae7d2e Binary files /dev/null and b/stretch/meshes/hank/link_gripper.STL differ diff --git a/stretch/meshes/hank/link_gripper_finger_left.STL b/stretch/meshes/hank/link_gripper_finger_left.STL new file mode 100644 index 0000000..60848f1 Binary files /dev/null and b/stretch/meshes/hank/link_gripper_finger_left.STL differ diff --git a/stretch/meshes/hank/link_gripper_finger_right.STL b/stretch/meshes/hank/link_gripper_finger_right.STL new file mode 100644 index 0000000..6efbb40 Binary files /dev/null and b/stretch/meshes/hank/link_gripper_finger_right.STL differ diff --git a/stretch/meshes/hank/link_gripper_fingertip_left.STL b/stretch/meshes/hank/link_gripper_fingertip_left.STL new file mode 100644 index 0000000..853c3be Binary files /dev/null and b/stretch/meshes/hank/link_gripper_fingertip_left.STL differ diff --git a/stretch/meshes/hank/link_gripper_fingertip_right.STL b/stretch/meshes/hank/link_gripper_fingertip_right.STL new file mode 100644 index 0000000..c40a44f Binary files /dev/null and b/stretch/meshes/hank/link_gripper_fingertip_right.STL differ diff --git a/stretch/meshes/hank/link_head.STL b/stretch/meshes/hank/link_head.STL new file mode 100644 index 0000000..0614101 Binary files /dev/null and b/stretch/meshes/hank/link_head.STL differ diff --git a/stretch/meshes/hank/link_head_pan.STL b/stretch/meshes/hank/link_head_pan.STL new file mode 100644 index 0000000..eaa3ebd Binary files /dev/null and b/stretch/meshes/hank/link_head_pan.STL differ diff --git a/stretch/meshes/hank/link_head_tilt.STL b/stretch/meshes/hank/link_head_tilt.STL new file mode 100644 index 0000000..f1c5026 Binary files /dev/null and b/stretch/meshes/hank/link_head_tilt.STL differ diff --git a/stretch/meshes/hank/link_left_wheel.STL b/stretch/meshes/hank/link_left_wheel.STL new file mode 100644 index 0000000..97837f2 Binary files /dev/null and b/stretch/meshes/hank/link_left_wheel.STL differ diff --git a/stretch/meshes/hank/link_lift.STL b/stretch/meshes/hank/link_lift.STL new file mode 100644 index 0000000..e9f16bf Binary files /dev/null and b/stretch/meshes/hank/link_lift.STL differ diff --git a/stretch/meshes/hank/link_mast.STL b/stretch/meshes/hank/link_mast.STL new file mode 100644 index 0000000..9ba50d8 Binary files /dev/null and b/stretch/meshes/hank/link_mast.STL differ diff --git a/stretch/meshes/hank/link_puller.STL b/stretch/meshes/hank/link_puller.STL new file mode 100644 index 0000000..33dd044 Binary files /dev/null and b/stretch/meshes/hank/link_puller.STL differ diff --git a/stretch/meshes/hank/link_respeaker.STL b/stretch/meshes/hank/link_respeaker.STL new file mode 100644 index 0000000..34ba67f Binary files /dev/null and b/stretch/meshes/hank/link_respeaker.STL differ diff --git a/stretch/meshes/hank/link_right_wheel.STL b/stretch/meshes/hank/link_right_wheel.STL new file mode 100644 index 0000000..f9e5483 Binary files /dev/null and b/stretch/meshes/hank/link_right_wheel.STL differ diff --git a/stretch/meshes/hank/link_wrist_yaw.STL b/stretch/meshes/hank/link_wrist_yaw.STL new file mode 100644 index 0000000..39498ae Binary files /dev/null and b/stretch/meshes/hank/link_wrist_yaw.STL differ diff --git a/stretch/meshes/hank/omni_wheel_m.STL b/stretch/meshes/hank/omni_wheel_m.STL new file mode 100644 index 0000000..19d2bb8 Binary files /dev/null and b/stretch/meshes/hank/omni_wheel_m.STL differ diff --git a/stretch/meshes/irma/base_link.STL b/stretch/meshes/irma/base_link.STL new file mode 100644 index 0000000..4d5884c Binary files /dev/null and b/stretch/meshes/irma/base_link.STL differ diff --git a/stretch/meshes/irma/laser.STL b/stretch/meshes/irma/laser.STL new file mode 100644 index 0000000..5646371 Binary files /dev/null and b/stretch/meshes/irma/laser.STL differ diff --git a/stretch/meshes/irma/link_arm_l0.STL b/stretch/meshes/irma/link_arm_l0.STL new file mode 100644 index 0000000..c333f85 Binary files /dev/null and b/stretch/meshes/irma/link_arm_l0.STL differ diff --git a/stretch/meshes/irma/link_arm_l1.STL b/stretch/meshes/irma/link_arm_l1.STL new file mode 100644 index 0000000..3b6fc65 Binary files /dev/null and b/stretch/meshes/irma/link_arm_l1.STL differ diff --git a/stretch/meshes/irma/link_arm_l2.STL b/stretch/meshes/irma/link_arm_l2.STL new file mode 100644 index 0000000..d303727 Binary files /dev/null and b/stretch/meshes/irma/link_arm_l2.STL differ diff --git a/stretch/meshes/irma/link_arm_l3.STL b/stretch/meshes/irma/link_arm_l3.STL new file mode 100644 index 0000000..3557a2f Binary files /dev/null and b/stretch/meshes/irma/link_arm_l3.STL differ diff --git a/stretch/meshes/irma/link_arm_l4.STL b/stretch/meshes/irma/link_arm_l4.STL new file mode 100644 index 0000000..9a454e8 Binary files /dev/null and b/stretch/meshes/irma/link_arm_l4.STL differ diff --git a/stretch/meshes/irma/link_aruco_inner_wrist.STL b/stretch/meshes/irma/link_aruco_inner_wrist.STL new file mode 100644 index 0000000..10db01e Binary files /dev/null and b/stretch/meshes/irma/link_aruco_inner_wrist.STL differ diff --git a/stretch/meshes/irma/link_aruco_left_base.STL b/stretch/meshes/irma/link_aruco_left_base.STL new file mode 100644 index 0000000..8ea0795 Binary files /dev/null and b/stretch/meshes/irma/link_aruco_left_base.STL differ diff --git a/stretch/meshes/irma/link_aruco_right_base.STL b/stretch/meshes/irma/link_aruco_right_base.STL new file mode 100644 index 0000000..8ea0795 Binary files /dev/null and b/stretch/meshes/irma/link_aruco_right_base.STL differ diff --git a/stretch/meshes/irma/link_aruco_shoulder.STL b/stretch/meshes/irma/link_aruco_shoulder.STL new file mode 100644 index 0000000..98d6567 Binary files /dev/null and b/stretch/meshes/irma/link_aruco_shoulder.STL differ diff --git a/stretch/meshes/irma/link_aruco_top_wrist.STL b/stretch/meshes/irma/link_aruco_top_wrist.STL new file mode 100644 index 0000000..2a53165 Binary files /dev/null and b/stretch/meshes/irma/link_aruco_top_wrist.STL differ diff --git a/stretch/meshes/irma/link_dry_erase_holder.STL b/stretch/meshes/irma/link_dry_erase_holder.STL new file mode 100644 index 0000000..a123087 Binary files /dev/null and b/stretch/meshes/irma/link_dry_erase_holder.STL differ diff --git a/stretch/meshes/irma/link_dry_erase_marker.STL b/stretch/meshes/irma/link_dry_erase_marker.STL new file mode 100644 index 0000000..5929498 Binary files /dev/null and b/stretch/meshes/irma/link_dry_erase_marker.STL differ diff --git a/stretch/meshes/irma/link_gripper.STL b/stretch/meshes/irma/link_gripper.STL new file mode 100644 index 0000000..7ae7d2e Binary files /dev/null and b/stretch/meshes/irma/link_gripper.STL differ diff --git a/stretch/meshes/irma/link_gripper_finger_left.STL b/stretch/meshes/irma/link_gripper_finger_left.STL new file mode 100644 index 0000000..60848f1 Binary files /dev/null and b/stretch/meshes/irma/link_gripper_finger_left.STL differ diff --git a/stretch/meshes/irma/link_gripper_finger_right.STL b/stretch/meshes/irma/link_gripper_finger_right.STL new file mode 100644 index 0000000..6efbb40 Binary files /dev/null and b/stretch/meshes/irma/link_gripper_finger_right.STL differ diff --git a/stretch/meshes/irma/link_gripper_fingertip_left.STL b/stretch/meshes/irma/link_gripper_fingertip_left.STL new file mode 100644 index 0000000..853c3be Binary files /dev/null and b/stretch/meshes/irma/link_gripper_fingertip_left.STL differ diff --git a/stretch/meshes/irma/link_gripper_fingertip_right.STL b/stretch/meshes/irma/link_gripper_fingertip_right.STL new file mode 100644 index 0000000..c40a44f Binary files /dev/null and b/stretch/meshes/irma/link_gripper_fingertip_right.STL differ diff --git a/stretch/meshes/irma/link_head.STL b/stretch/meshes/irma/link_head.STL new file mode 100644 index 0000000..0614101 Binary files /dev/null and b/stretch/meshes/irma/link_head.STL differ diff --git a/stretch/meshes/irma/link_head_pan.STL b/stretch/meshes/irma/link_head_pan.STL new file mode 100644 index 0000000..eaa3ebd Binary files /dev/null and b/stretch/meshes/irma/link_head_pan.STL differ diff --git a/stretch/meshes/irma/link_head_tilt.STL b/stretch/meshes/irma/link_head_tilt.STL new file mode 100644 index 0000000..f1c5026 Binary files /dev/null and b/stretch/meshes/irma/link_head_tilt.STL differ diff --git a/stretch/meshes/irma/link_left_wheel.STL b/stretch/meshes/irma/link_left_wheel.STL new file mode 100644 index 0000000..97837f2 Binary files /dev/null and b/stretch/meshes/irma/link_left_wheel.STL differ diff --git a/stretch/meshes/irma/link_lift.STL b/stretch/meshes/irma/link_lift.STL new file mode 100644 index 0000000..e9f16bf Binary files /dev/null and b/stretch/meshes/irma/link_lift.STL differ diff --git a/stretch/meshes/irma/link_mast.STL b/stretch/meshes/irma/link_mast.STL new file mode 100644 index 0000000..9ba50d8 Binary files /dev/null and b/stretch/meshes/irma/link_mast.STL differ diff --git a/stretch/meshes/irma/link_puller.STL b/stretch/meshes/irma/link_puller.STL new file mode 100644 index 0000000..33dd044 Binary files /dev/null and b/stretch/meshes/irma/link_puller.STL differ diff --git a/stretch/meshes/irma/link_respeaker.STL b/stretch/meshes/irma/link_respeaker.STL new file mode 100644 index 0000000..34ba67f Binary files /dev/null and b/stretch/meshes/irma/link_respeaker.STL differ diff --git a/stretch/meshes/irma/link_right_wheel.STL b/stretch/meshes/irma/link_right_wheel.STL new file mode 100644 index 0000000..f9e5483 Binary files /dev/null and b/stretch/meshes/irma/link_right_wheel.STL differ diff --git a/stretch/meshes/irma/link_wrist_yaw.STL b/stretch/meshes/irma/link_wrist_yaw.STL new file mode 100644 index 0000000..39498ae Binary files /dev/null and b/stretch/meshes/irma/link_wrist_yaw.STL differ diff --git a/stretch/meshes/irma/omni_wheel_m.STL b/stretch/meshes/irma/omni_wheel_m.STL new file mode 100644 index 0000000..19d2bb8 Binary files /dev/null and b/stretch/meshes/irma/omni_wheel_m.STL differ diff --git a/stretch/meshes/joplin/base_link.STL b/stretch/meshes/joplin/base_link.STL new file mode 100644 index 0000000..4d5884c Binary files /dev/null and b/stretch/meshes/joplin/base_link.STL differ diff --git a/stretch/meshes/joplin/laser.STL b/stretch/meshes/joplin/laser.STL new file mode 100644 index 0000000..5646371 Binary files /dev/null and b/stretch/meshes/joplin/laser.STL differ diff --git a/stretch/meshes/joplin/link_arm_l0.STL b/stretch/meshes/joplin/link_arm_l0.STL new file mode 100644 index 0000000..c333f85 Binary files /dev/null and b/stretch/meshes/joplin/link_arm_l0.STL differ diff --git a/stretch/meshes/joplin/link_arm_l1.STL b/stretch/meshes/joplin/link_arm_l1.STL new file mode 100644 index 0000000..3b6fc65 Binary files /dev/null and b/stretch/meshes/joplin/link_arm_l1.STL differ diff --git a/stretch/meshes/joplin/link_arm_l2.STL b/stretch/meshes/joplin/link_arm_l2.STL new file mode 100644 index 0000000..d303727 Binary files /dev/null and b/stretch/meshes/joplin/link_arm_l2.STL differ diff --git a/stretch/meshes/joplin/link_arm_l3.STL b/stretch/meshes/joplin/link_arm_l3.STL new file mode 100644 index 0000000..3557a2f Binary files /dev/null and b/stretch/meshes/joplin/link_arm_l3.STL differ diff --git a/stretch/meshes/joplin/link_arm_l4.STL b/stretch/meshes/joplin/link_arm_l4.STL new file mode 100644 index 0000000..9a454e8 Binary files /dev/null and b/stretch/meshes/joplin/link_arm_l4.STL differ diff --git a/stretch/meshes/joplin/link_aruco_inner_wrist.STL b/stretch/meshes/joplin/link_aruco_inner_wrist.STL new file mode 100644 index 0000000..10db01e Binary files /dev/null and b/stretch/meshes/joplin/link_aruco_inner_wrist.STL differ diff --git a/stretch/meshes/joplin/link_aruco_left_base.STL b/stretch/meshes/joplin/link_aruco_left_base.STL new file mode 100644 index 0000000..8ea0795 Binary files /dev/null and b/stretch/meshes/joplin/link_aruco_left_base.STL differ diff --git a/stretch/meshes/joplin/link_aruco_right_base.STL b/stretch/meshes/joplin/link_aruco_right_base.STL new file mode 100644 index 0000000..8ea0795 Binary files /dev/null and b/stretch/meshes/joplin/link_aruco_right_base.STL differ diff --git a/stretch/meshes/joplin/link_aruco_shoulder.STL b/stretch/meshes/joplin/link_aruco_shoulder.STL new file mode 100644 index 0000000..98d6567 Binary files /dev/null and b/stretch/meshes/joplin/link_aruco_shoulder.STL differ diff --git a/stretch/meshes/joplin/link_aruco_top_wrist.STL b/stretch/meshes/joplin/link_aruco_top_wrist.STL new file mode 100644 index 0000000..2a53165 Binary files /dev/null and b/stretch/meshes/joplin/link_aruco_top_wrist.STL differ diff --git a/stretch/meshes/joplin/link_dry_erase_holder.STL b/stretch/meshes/joplin/link_dry_erase_holder.STL new file mode 100644 index 0000000..a123087 Binary files /dev/null and b/stretch/meshes/joplin/link_dry_erase_holder.STL differ diff --git a/stretch/meshes/joplin/link_dry_erase_marker.STL b/stretch/meshes/joplin/link_dry_erase_marker.STL new file mode 100644 index 0000000..5929498 Binary files /dev/null and b/stretch/meshes/joplin/link_dry_erase_marker.STL differ diff --git a/stretch/meshes/joplin/link_gripper.STL b/stretch/meshes/joplin/link_gripper.STL new file mode 100644 index 0000000..7ae7d2e Binary files /dev/null and b/stretch/meshes/joplin/link_gripper.STL differ diff --git a/stretch/meshes/joplin/link_gripper_finger_left.STL b/stretch/meshes/joplin/link_gripper_finger_left.STL new file mode 100644 index 0000000..60848f1 Binary files /dev/null and b/stretch/meshes/joplin/link_gripper_finger_left.STL differ diff --git a/stretch/meshes/joplin/link_gripper_finger_right.STL b/stretch/meshes/joplin/link_gripper_finger_right.STL new file mode 100644 index 0000000..6efbb40 Binary files /dev/null and b/stretch/meshes/joplin/link_gripper_finger_right.STL differ diff --git a/stretch/meshes/joplin/link_gripper_fingertip_left.STL b/stretch/meshes/joplin/link_gripper_fingertip_left.STL new file mode 100644 index 0000000..853c3be Binary files /dev/null and b/stretch/meshes/joplin/link_gripper_fingertip_left.STL differ diff --git a/stretch/meshes/joplin/link_gripper_fingertip_right.STL b/stretch/meshes/joplin/link_gripper_fingertip_right.STL new file mode 100644 index 0000000..c40a44f Binary files /dev/null and b/stretch/meshes/joplin/link_gripper_fingertip_right.STL differ diff --git a/stretch/meshes/joplin/link_head.STL b/stretch/meshes/joplin/link_head.STL new file mode 100644 index 0000000..0614101 Binary files /dev/null and b/stretch/meshes/joplin/link_head.STL differ diff --git a/stretch/meshes/joplin/link_head_pan.STL b/stretch/meshes/joplin/link_head_pan.STL new file mode 100644 index 0000000..eaa3ebd Binary files /dev/null and b/stretch/meshes/joplin/link_head_pan.STL differ diff --git a/stretch/meshes/joplin/link_head_tilt.STL b/stretch/meshes/joplin/link_head_tilt.STL new file mode 100644 index 0000000..f1c5026 Binary files /dev/null and b/stretch/meshes/joplin/link_head_tilt.STL differ diff --git a/stretch/meshes/joplin/link_left_wheel.STL b/stretch/meshes/joplin/link_left_wheel.STL new file mode 100644 index 0000000..97837f2 Binary files /dev/null and b/stretch/meshes/joplin/link_left_wheel.STL differ diff --git a/stretch/meshes/joplin/link_lift.STL b/stretch/meshes/joplin/link_lift.STL new file mode 100644 index 0000000..e9f16bf Binary files /dev/null and b/stretch/meshes/joplin/link_lift.STL differ diff --git a/stretch/meshes/joplin/link_mast.STL b/stretch/meshes/joplin/link_mast.STL new file mode 100644 index 0000000..9ba50d8 Binary files /dev/null and b/stretch/meshes/joplin/link_mast.STL differ diff --git a/stretch/meshes/joplin/link_puller.STL b/stretch/meshes/joplin/link_puller.STL new file mode 100644 index 0000000..33dd044 Binary files /dev/null and b/stretch/meshes/joplin/link_puller.STL differ diff --git a/stretch/meshes/joplin/link_respeaker.STL b/stretch/meshes/joplin/link_respeaker.STL new file mode 100644 index 0000000..34ba67f Binary files /dev/null and b/stretch/meshes/joplin/link_respeaker.STL differ diff --git a/stretch/meshes/joplin/link_right_wheel.STL b/stretch/meshes/joplin/link_right_wheel.STL new file mode 100644 index 0000000..f9e5483 Binary files /dev/null and b/stretch/meshes/joplin/link_right_wheel.STL differ diff --git a/stretch/meshes/joplin/link_wrist_yaw.STL b/stretch/meshes/joplin/link_wrist_yaw.STL new file mode 100644 index 0000000..39498ae Binary files /dev/null and b/stretch/meshes/joplin/link_wrist_yaw.STL differ diff --git a/stretch/meshes/joplin/omni_wheel_m.STL b/stretch/meshes/joplin/omni_wheel_m.STL new file mode 100644 index 0000000..19d2bb8 Binary files /dev/null and b/stretch/meshes/joplin/omni_wheel_m.STL differ diff --git a/stretch/meshes/kendrick/base_link.STL b/stretch/meshes/kendrick/base_link.STL new file mode 100644 index 0000000..4d5884c Binary files /dev/null and b/stretch/meshes/kendrick/base_link.STL differ diff --git a/stretch/meshes/kendrick/laser.STL b/stretch/meshes/kendrick/laser.STL new file mode 100644 index 0000000..5646371 Binary files /dev/null and b/stretch/meshes/kendrick/laser.STL differ diff --git a/stretch/meshes/kendrick/link_arm_l0.STL b/stretch/meshes/kendrick/link_arm_l0.STL new file mode 100644 index 0000000..c333f85 Binary files /dev/null and b/stretch/meshes/kendrick/link_arm_l0.STL differ diff --git a/stretch/meshes/kendrick/link_arm_l1.STL b/stretch/meshes/kendrick/link_arm_l1.STL new file mode 100644 index 0000000..3b6fc65 Binary files /dev/null and b/stretch/meshes/kendrick/link_arm_l1.STL differ diff --git a/stretch/meshes/kendrick/link_arm_l2.STL b/stretch/meshes/kendrick/link_arm_l2.STL new file mode 100644 index 0000000..d303727 Binary files /dev/null and b/stretch/meshes/kendrick/link_arm_l2.STL differ diff --git a/stretch/meshes/kendrick/link_arm_l3.STL b/stretch/meshes/kendrick/link_arm_l3.STL new file mode 100644 index 0000000..3557a2f Binary files /dev/null and b/stretch/meshes/kendrick/link_arm_l3.STL differ diff --git a/stretch/meshes/kendrick/link_arm_l4.STL b/stretch/meshes/kendrick/link_arm_l4.STL new file mode 100644 index 0000000..9a454e8 Binary files /dev/null and b/stretch/meshes/kendrick/link_arm_l4.STL differ diff --git a/stretch/meshes/kendrick/link_aruco_inner_wrist.STL b/stretch/meshes/kendrick/link_aruco_inner_wrist.STL new file mode 100644 index 0000000..10db01e Binary files /dev/null and b/stretch/meshes/kendrick/link_aruco_inner_wrist.STL differ diff --git a/stretch/meshes/kendrick/link_aruco_left_base.STL b/stretch/meshes/kendrick/link_aruco_left_base.STL new file mode 100644 index 0000000..8ea0795 Binary files /dev/null and b/stretch/meshes/kendrick/link_aruco_left_base.STL differ diff --git a/stretch/meshes/kendrick/link_aruco_right_base.STL b/stretch/meshes/kendrick/link_aruco_right_base.STL new file mode 100644 index 0000000..8ea0795 Binary files /dev/null and b/stretch/meshes/kendrick/link_aruco_right_base.STL differ diff --git a/stretch/meshes/kendrick/link_aruco_shoulder.STL b/stretch/meshes/kendrick/link_aruco_shoulder.STL new file mode 100644 index 0000000..98d6567 Binary files /dev/null and b/stretch/meshes/kendrick/link_aruco_shoulder.STL differ diff --git a/stretch/meshes/kendrick/link_aruco_top_wrist.STL b/stretch/meshes/kendrick/link_aruco_top_wrist.STL new file mode 100644 index 0000000..2a53165 Binary files /dev/null and b/stretch/meshes/kendrick/link_aruco_top_wrist.STL differ diff --git a/stretch/meshes/kendrick/link_dry_erase_holder.STL b/stretch/meshes/kendrick/link_dry_erase_holder.STL new file mode 100644 index 0000000..a123087 Binary files /dev/null and b/stretch/meshes/kendrick/link_dry_erase_holder.STL differ diff --git a/stretch/meshes/kendrick/link_dry_erase_marker.STL b/stretch/meshes/kendrick/link_dry_erase_marker.STL new file mode 100644 index 0000000..5929498 Binary files /dev/null and b/stretch/meshes/kendrick/link_dry_erase_marker.STL differ diff --git a/stretch/meshes/kendrick/link_gripper.STL b/stretch/meshes/kendrick/link_gripper.STL new file mode 100644 index 0000000..7ae7d2e Binary files /dev/null and b/stretch/meshes/kendrick/link_gripper.STL differ diff --git a/stretch/meshes/kendrick/link_gripper_finger_left.STL b/stretch/meshes/kendrick/link_gripper_finger_left.STL new file mode 100644 index 0000000..60848f1 Binary files /dev/null and b/stretch/meshes/kendrick/link_gripper_finger_left.STL differ diff --git a/stretch/meshes/kendrick/link_gripper_finger_right.STL b/stretch/meshes/kendrick/link_gripper_finger_right.STL new file mode 100644 index 0000000..6efbb40 Binary files /dev/null and b/stretch/meshes/kendrick/link_gripper_finger_right.STL differ diff --git a/stretch/meshes/kendrick/link_gripper_fingertip_left.STL b/stretch/meshes/kendrick/link_gripper_fingertip_left.STL new file mode 100644 index 0000000..853c3be Binary files /dev/null and b/stretch/meshes/kendrick/link_gripper_fingertip_left.STL differ diff --git a/stretch/meshes/kendrick/link_gripper_fingertip_right.STL b/stretch/meshes/kendrick/link_gripper_fingertip_right.STL new file mode 100644 index 0000000..c40a44f Binary files /dev/null and b/stretch/meshes/kendrick/link_gripper_fingertip_right.STL differ diff --git a/stretch/meshes/kendrick/link_head.STL b/stretch/meshes/kendrick/link_head.STL new file mode 100644 index 0000000..0614101 Binary files /dev/null and b/stretch/meshes/kendrick/link_head.STL differ diff --git a/stretch/meshes/kendrick/link_head_pan.STL b/stretch/meshes/kendrick/link_head_pan.STL new file mode 100644 index 0000000..eaa3ebd Binary files /dev/null and b/stretch/meshes/kendrick/link_head_pan.STL differ diff --git a/stretch/meshes/kendrick/link_head_tilt.STL b/stretch/meshes/kendrick/link_head_tilt.STL new file mode 100644 index 0000000..f1c5026 Binary files /dev/null and b/stretch/meshes/kendrick/link_head_tilt.STL differ diff --git a/stretch/meshes/kendrick/link_left_wheel.STL b/stretch/meshes/kendrick/link_left_wheel.STL new file mode 100644 index 0000000..97837f2 Binary files /dev/null and b/stretch/meshes/kendrick/link_left_wheel.STL differ diff --git a/stretch/meshes/kendrick/link_lift.STL b/stretch/meshes/kendrick/link_lift.STL new file mode 100644 index 0000000..e9f16bf Binary files /dev/null and b/stretch/meshes/kendrick/link_lift.STL differ diff --git a/stretch/meshes/kendrick/link_mast.STL b/stretch/meshes/kendrick/link_mast.STL new file mode 100644 index 0000000..9ba50d8 Binary files /dev/null and b/stretch/meshes/kendrick/link_mast.STL differ diff --git a/stretch/meshes/kendrick/link_puller.STL b/stretch/meshes/kendrick/link_puller.STL new file mode 100644 index 0000000..33dd044 Binary files /dev/null and b/stretch/meshes/kendrick/link_puller.STL differ diff --git a/stretch/meshes/kendrick/link_respeaker.STL b/stretch/meshes/kendrick/link_respeaker.STL new file mode 100644 index 0000000..34ba67f Binary files /dev/null and b/stretch/meshes/kendrick/link_respeaker.STL differ diff --git a/stretch/meshes/kendrick/link_right_wheel.STL b/stretch/meshes/kendrick/link_right_wheel.STL new file mode 100644 index 0000000..f9e5483 Binary files /dev/null and b/stretch/meshes/kendrick/link_right_wheel.STL differ diff --git a/stretch/meshes/kendrick/link_wrist_yaw.STL b/stretch/meshes/kendrick/link_wrist_yaw.STL new file mode 100644 index 0000000..39498ae Binary files /dev/null and b/stretch/meshes/kendrick/link_wrist_yaw.STL differ diff --git a/stretch/meshes/kendrick/omni_wheel_m.STL b/stretch/meshes/kendrick/omni_wheel_m.STL new file mode 100644 index 0000000..19d2bb8 Binary files /dev/null and b/stretch/meshes/kendrick/omni_wheel_m.STL differ diff --git a/stretch/meshes/laser.STL b/stretch/meshes/laser.STL new file mode 100644 index 0000000..5646371 Binary files /dev/null and b/stretch/meshes/laser.STL differ diff --git a/stretch/meshes/link_arm_l0.STL b/stretch/meshes/link_arm_l0.STL new file mode 100644 index 0000000..c333f85 Binary files /dev/null and b/stretch/meshes/link_arm_l0.STL differ diff --git a/stretch/meshes/link_arm_l1.STL b/stretch/meshes/link_arm_l1.STL new file mode 100644 index 0000000..3b6fc65 Binary files /dev/null and b/stretch/meshes/link_arm_l1.STL differ diff --git a/stretch/meshes/link_arm_l2.STL b/stretch/meshes/link_arm_l2.STL new file mode 100644 index 0000000..d303727 Binary files /dev/null and b/stretch/meshes/link_arm_l2.STL differ diff --git a/stretch/meshes/link_arm_l3.STL b/stretch/meshes/link_arm_l3.STL new file mode 100644 index 0000000..3557a2f Binary files /dev/null and b/stretch/meshes/link_arm_l3.STL differ diff --git a/stretch/meshes/link_arm_l4.STL b/stretch/meshes/link_arm_l4.STL new file mode 100644 index 0000000..9a454e8 Binary files /dev/null and b/stretch/meshes/link_arm_l4.STL differ diff --git a/stretch/meshes/link_aruco_inner_wrist.STL b/stretch/meshes/link_aruco_inner_wrist.STL new file mode 100644 index 0000000..10db01e Binary files /dev/null and b/stretch/meshes/link_aruco_inner_wrist.STL differ diff --git a/stretch/meshes/link_aruco_left_base.STL b/stretch/meshes/link_aruco_left_base.STL new file mode 100644 index 0000000..8ea0795 Binary files /dev/null and b/stretch/meshes/link_aruco_left_base.STL differ diff --git a/stretch/meshes/link_aruco_right_base.STL b/stretch/meshes/link_aruco_right_base.STL new file mode 100644 index 0000000..8ea0795 Binary files /dev/null and b/stretch/meshes/link_aruco_right_base.STL differ diff --git a/stretch/meshes/link_aruco_shoulder.STL b/stretch/meshes/link_aruco_shoulder.STL new file mode 100644 index 0000000..98d6567 Binary files /dev/null and b/stretch/meshes/link_aruco_shoulder.STL differ diff --git a/stretch/meshes/link_aruco_top_wrist.STL b/stretch/meshes/link_aruco_top_wrist.STL new file mode 100644 index 0000000..2a53165 Binary files /dev/null and b/stretch/meshes/link_aruco_top_wrist.STL differ diff --git a/stretch/meshes/link_dry_erase_holder.STL b/stretch/meshes/link_dry_erase_holder.STL new file mode 100644 index 0000000..a123087 Binary files /dev/null and b/stretch/meshes/link_dry_erase_holder.STL differ diff --git a/stretch/meshes/link_dry_erase_marker.STL b/stretch/meshes/link_dry_erase_marker.STL new file mode 100644 index 0000000..5929498 Binary files /dev/null and b/stretch/meshes/link_dry_erase_marker.STL differ diff --git a/stretch/meshes/link_gripper.STL b/stretch/meshes/link_gripper.STL new file mode 100644 index 0000000..7ae7d2e Binary files /dev/null and b/stretch/meshes/link_gripper.STL differ diff --git a/stretch/meshes/link_gripper_finger_left.STL b/stretch/meshes/link_gripper_finger_left.STL new file mode 100644 index 0000000..60848f1 Binary files /dev/null and b/stretch/meshes/link_gripper_finger_left.STL differ diff --git a/stretch/meshes/link_gripper_finger_right.STL b/stretch/meshes/link_gripper_finger_right.STL new file mode 100644 index 0000000..6efbb40 Binary files /dev/null and b/stretch/meshes/link_gripper_finger_right.STL differ diff --git a/stretch/meshes/link_gripper_fingertip_left.STL b/stretch/meshes/link_gripper_fingertip_left.STL new file mode 100644 index 0000000..853c3be Binary files /dev/null and b/stretch/meshes/link_gripper_fingertip_left.STL differ diff --git a/stretch/meshes/link_gripper_fingertip_right.STL b/stretch/meshes/link_gripper_fingertip_right.STL new file mode 100644 index 0000000..c40a44f Binary files /dev/null and b/stretch/meshes/link_gripper_fingertip_right.STL differ diff --git a/stretch/meshes/link_head.STL b/stretch/meshes/link_head.STL new file mode 100644 index 0000000..0614101 Binary files /dev/null and b/stretch/meshes/link_head.STL differ diff --git a/stretch/meshes/link_head_pan.STL b/stretch/meshes/link_head_pan.STL new file mode 100644 index 0000000..eaa3ebd Binary files /dev/null and b/stretch/meshes/link_head_pan.STL differ diff --git a/stretch/meshes/link_head_tilt.STL b/stretch/meshes/link_head_tilt.STL new file mode 100644 index 0000000..f1c5026 Binary files /dev/null and b/stretch/meshes/link_head_tilt.STL differ diff --git a/stretch/meshes/link_left_wheel.STL b/stretch/meshes/link_left_wheel.STL new file mode 100644 index 0000000..97837f2 Binary files /dev/null and b/stretch/meshes/link_left_wheel.STL differ diff --git a/stretch/meshes/link_lift.STL b/stretch/meshes/link_lift.STL new file mode 100644 index 0000000..e9f16bf Binary files /dev/null and b/stretch/meshes/link_lift.STL differ diff --git a/stretch/meshes/link_mast.STL b/stretch/meshes/link_mast.STL new file mode 100644 index 0000000..9ba50d8 Binary files /dev/null and b/stretch/meshes/link_mast.STL differ diff --git a/stretch/meshes/link_puller.STL b/stretch/meshes/link_puller.STL new file mode 100644 index 0000000..33dd044 Binary files /dev/null and b/stretch/meshes/link_puller.STL differ diff --git a/stretch/meshes/link_respeaker.STL b/stretch/meshes/link_respeaker.STL new file mode 100644 index 0000000..34ba67f Binary files /dev/null and b/stretch/meshes/link_respeaker.STL differ diff --git a/stretch/meshes/link_right_wheel.STL b/stretch/meshes/link_right_wheel.STL new file mode 100644 index 0000000..f9e5483 Binary files /dev/null and b/stretch/meshes/link_right_wheel.STL differ diff --git a/stretch/meshes/link_wrist_yaw.STL b/stretch/meshes/link_wrist_yaw.STL new file mode 100644 index 0000000..39498ae Binary files /dev/null and b/stretch/meshes/link_wrist_yaw.STL differ diff --git a/stretch/meshes/omni_wheel_m.STL b/stretch/meshes/omni_wheel_m.STL new file mode 100644 index 0000000..19d2bb8 Binary files /dev/null and b/stretch/meshes/omni_wheel_m.STL differ diff --git a/stretch/meshes/update_meshes.py b/stretch/meshes/update_meshes.py new file mode 100755 index 0000000..6e4dc30 --- /dev/null +++ b/stretch/meshes/update_meshes.py @@ -0,0 +1,8 @@ +#!/usr/bin/env python +import os +import stretch_body.hello_utils as hu +batch_name = hu.read_fleet_yaml('stretch_re1_factory_params.yaml')['robot']['batch_name'].lower() +cmd='cp ~/catkin_ws/src/stretch_ros/stretch_description/meshes/'+batch_name+'/*.STL ~/catkin_ws/src/stretch_ros/stretch_description/meshes' +print("Copying in mesh files from batch:"+batch_name) +print(cmd) +os.system(cmd) diff --git a/stretch/urdf/export_urdf.sh b/stretch/urdf/export_urdf.sh new file mode 100755 index 0000000..40fc6e7 --- /dev/null +++ b/stretch/urdf/export_urdf.sh @@ -0,0 +1,59 @@ +#!/bin/bash + +# Prior to running this script make sure you have used stretch_calibration to create a calibrated URDF. This script also requires rpl, which can be installed with "sudo apt install rpl". +echo "Prior to running this script make sure you have used stretch_calibration to create a calibrated URDF. This script also requires rpl, which can be installed with sudo apt install rpl." +echo "" + +# Move previous exported_urdf to exported_urdf_previous. +echo "Move previous exported_urdf to exported_urdf_previous." +echo "mv ./exported_urdf ./exported_urdf_previous" +mv ./exported_urdf ./exported_urdf_previous +echo "" + +# Create new exported URDF directories. +echo "Create new exported URDF directories." +echo "mkdir ./exported_urdf/" +mkdir ./exported_urdf/ +echo "mkdir ./exported_urdf/meshes/" +mkdir ./exported_urdf/meshes/ +echo "" + +# Copy the mesh files and the original calibrated URDF file to the exported URDF. +echo "Copy the mesh files and the current calibrated URDF file to the exported URDF." +echo "cp ../meshes/* ./exported_urdf/meshes/" +cp ../meshes/* ./exported_urdf/meshes/ +echo "cp ./stretch.urdf ./exported_urdf/" +cp ./stretch.urdf ./exported_urdf/ +echo "" + +# Replace the mesh file locations in the original URDF with local directories." +echo "Replace the mesh file locations in the exported URDF with local directories." +OLD_NAME="package://stretch_description/" +NEW_NAME="./" +echo "rpl -Ri $OLD_NAME $NEW_NAME ./exported_urdf/stretch.urdf" +rpl -Ri $OLD_NAME $NEW_NAME ./exported_urdf/stretch.urdf +echo "" + +# Copy D435i mesh from the realsense2_description ROS package to the exported URDF. +echo "Copy D435i mesh from the realsense2_description ROS package to the exported URDF." +echo "cp `rospack find realsense2_description`/meshes/d435.dae ./exported_urdf/meshes/" +cp `rospack find realsense2_description`/meshes/d435.dae ./exported_urdf/meshes/ +echo "rpl -Ri "package://realsense2_description/" "./" ./exported_urdf/stretch.urdf" +rpl -Ri "package://realsense2_description/" "./" ./exported_urdf/stretch.urdf +echo "" + +# copy controller calibration file used by stretch ROS +echo "Copy the current controller parameter yaml file to the exported URDF." +echo "cp `rospack find stretch_core`/config/controller_calibration_head.yaml ./exported_urdf/" +cp `rospack find stretch_core`/config/controller_calibration_head.yaml ./exported_urdf/ +echo "" + +# copy license file +echo "Copy license file to exported URDF" +echo "cp export_urdf_license_template.md ./exported_urdf/LICENSE.md" +cp export_urdf_license_template.md ./exported_urdf/LICENSE.md + +echo "Copy the exported URDF to the standard Stretch directory." +echo "cp -rf exported_urdf/* $HELLO_FLEET_PATH/$HELLO_FLEET_ID/exported_urdf" +cp -rf exported_urdf/* $HELLO_FLEET_PATH/$HELLO_FLEET_ID/exported_urdf +echo "" diff --git a/stretch/urdf/export_urdf_license_template.md b/stretch/urdf/export_urdf_license_template.md new file mode 100644 index 0000000..fb7ae66 --- /dev/null +++ b/stretch/urdf/export_urdf_license_template.md @@ -0,0 +1,23 @@ +The following license applies to the entire contents of this directory (the "Contents") except where otherwise noted. The Contents consist of software and data used with the Stretch RE1 mobile manipulator, which is a robot produced and sold by Hello Robot Inc. + +Copyright 2020 Hello Robot Inc. + +The Contents are licensed under the Creative Commons Attribution-NonCommercial-ShareAlike-4.0-International (CC BY-NC-SA 4.0) license (the "License"); you may not use the Contents except in compliance with the License. You may obtain a copy of the License at + +https://creativecommons.org/licenses/by-nc-sa/4.0/ + +Unless required by applicable law or agreed to in writing, the Contents distributed under the License are distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. + +Patents pending and trademark rights cover the Contents. As stated by the detailed License, "Patent and trademark rights are not licensed under this Public License." + +The Contents may incorporate some parts of the "RealSense Camera description package for Intel 3D D400 cameras" released with an Apache 2.0 license and Copyright 2017 Intel Corporation. The details of the Apache 2.0 license can be found via the following link: + +https://www.apache.org/licenses/LICENSE-2.0 + +Specifically, the Contents may include the d435.dae mesh file and content generated by the \_d435.urdf.xacro found within the GitHub repository available for download via the following link as of May 4, 2020: + +https://github.com/IntelRealSense/realsense-ros/tree/development/realsense2_description + +These specific materials are subject to the requirements of their original Apache 2.0 license. + +For further information about the Contents including inquiries about dual licensing, please contact Hello Robot Inc. diff --git a/stretch/urdf/stretch_aruco.xacro b/stretch/urdf/stretch_aruco.xacro new file mode 100644 index 0000000..d58c8bb --- /dev/null +++ b/stretch/urdf/stretch_aruco.xacro @@ -0,0 +1,283 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch/urdf/stretch_d435i.xacro b/stretch/urdf/stretch_d435i.xacro new file mode 100644 index 0000000..ea8d71c --- /dev/null +++ b/stretch/urdf/stretch_d435i.xacro @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + diff --git a/stretch/urdf/stretch_description.xacro b/stretch/urdf/stretch_description.xacro new file mode 100644 index 0000000..16e541e --- /dev/null +++ b/stretch/urdf/stretch_description.xacro @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/stretch/urdf/stretch_dry_erase_marker.xacro b/stretch/urdf/stretch_dry_erase_marker.xacro new file mode 100644 index 0000000..3b1e989 --- /dev/null +++ b/stretch/urdf/stretch_dry_erase_marker.xacro @@ -0,0 +1,133 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch/urdf/stretch_gripper.xacro b/stretch/urdf/stretch_gripper.xacro new file mode 100644 index 0000000..51de9e8 --- /dev/null +++ b/stretch/urdf/stretch_gripper.xacro @@ -0,0 +1,302 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch/urdf/stretch_gripper_with_puller.xacro b/stretch/urdf/stretch_gripper_with_puller.xacro new file mode 100644 index 0000000..3a9cf5f --- /dev/null +++ b/stretch/urdf/stretch_gripper_with_puller.xacro @@ -0,0 +1,359 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch/urdf/stretch_laser_range_finder.xacro b/stretch/urdf/stretch_laser_range_finder.xacro new file mode 100644 index 0000000..f57ff04 --- /dev/null +++ b/stretch/urdf/stretch_laser_range_finder.xacro @@ -0,0 +1,59 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch/urdf/stretch_main.xacro b/stretch/urdf/stretch_main.xacro new file mode 100644 index 0000000..38e6148 --- /dev/null +++ b/stretch/urdf/stretch_main.xacro @@ -0,0 +1,841 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + 1.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch/urdf/stretch_respeaker.xacro b/stretch/urdf/stretch_respeaker.xacro new file mode 100644 index 0000000..622a939 --- /dev/null +++ b/stretch/urdf/stretch_respeaker.xacro @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch/urdf/xacro_to_urdf.sh b/stretch/urdf/xacro_to_urdf.sh new file mode 100755 index 0000000..13afdd7 --- /dev/null +++ b/stretch/urdf/xacro_to_urdf.sh @@ -0,0 +1,5 @@ +#!/bin/bash + +rosrun xacro xacro `rospack find robowflex_resources`/stretch/urdf/stretch_description.xacro use_nominal_extrinsics:=true > `rospack find robowflex_resources`/stretch/urdf/stretch_uncalibrated.urdf + +cp `rospack find robowflex_resources`/stretch/urdf/stretch_uncalibrated.urdf `rospack find robowflex_resources`/stretch/urdf/stretch.urdf