$search
00001 /* 00002 * Copyright (c) 2010, Thomas Ruehr <ruehr@cs.tum.edu> 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 00031 00032 #include <ias_drawer_executive/Poses.h> 00033 00034 double Poses::milehighPoseA[7] = {-1.2941619112744558, 0.49942431312780172, -0.12505826215869686, -2.1195908351955559, -196.75657241281269, -1.4749302507914408, 284.5875016118315}; 00035 double Poses::milehighPoseB[7] = {-0.75344495287219471, 0.0071664555829800761, -0.24500367228337172, -1.4238227962863217, -197.23479428063303, -0.094914323072497142, 284.39432200658928}; 00036 //{-0.82681730923312724, 0.15596892532371576, -0.16835406527856578, -1.7773528734848965, -197.19522687315879, -1.0987956365304499, 284.37391632306253}; 00037 // arm joint angles for seeing the marker with the hand camera 00038 //high drawer 00039 double Poses::highPoseA[7] = {-1.2658078481383668, 0.65194895948513354, -0.6651333173724735, -2.1146686310085161, -52.566807785671713, -1.1327560220881783, -29.66004589615245}; 00040 double Poses::highPoseB[7] = {-0.83999946139288795, 0.64805759302232857, -0.20651851395459886, -1.979887098710452, -52.732655383958971, -1.1379335835800284, -29.902173036506696}; 00041 00042 //low drawer 00043 double Poses::lowPoseA[7] = {-0.58879241080122102,1.2959701090793467,0.61289464879551958,-1.3123493485210054, -1.819574788110754,-1.1066071611083139,-0.425}; 00044 double Poses::lowPoseB[7] = {-0.56640762411483492,1.1399770708747323,0.49102498075440626,-0.9884972671560539,-1.8417302224128111,-0.88001270522963293,-0.425}; 00045 00046 //middle drawer 00047 double Poses::midPoseA[7] = {-1.136473525061469,0.87282630371086467,-0.20427354638542061,-2.1237891858256783,-2.2135217141215739,-1.2897797147276118,1.4621021956879214}; 00048 double Poses::midPoseB[7] = {-1.1291777427340541, 0.83112100662036859,-0.55432813235088196,-1.9421019430393511,-2.3539661773600271845,-0.76206336967963972,1.46}; 00049 00050 //tucking pose, expects left arm to be tucked already 00051 double Poses::tuckPose[7] = {0.00059073198959080919, 1.1486903479544912, -1.4332329356842286, -1.4462622565507686, -51.274908792804851, -0.2154139507083308, -31.660673060172002}; 00052 //untucking poses, hand follows the base in a circular motion 00053 double Poses::untuckPoseA[7] = {-1.3239253869056138, 1.2547723815274774, -2.1150616574089849, -1.6996110014719421, -51.308807185757871, -0.15602427477238412, -31.606852526008804}; 00054 double Poses::untuckPoseB[7] = {-2.2269442631579031, 0.96740342774164501, -2.3171087386350413, -1.7653369044400637, -51.229440982461696, -0.10677217721964216, -31.598672849030081}; 00055 00056 double Poses::tuckPoseForLeft[7] = {-2.1349672088362253, 1.1009839702456099, -2.2575564550627725, -2.1229357008475702, -0.93467766850553247, -0.96966653718245521, 0.17175967044035967}; 00057 00058 00059 double Poses::leftHighA[7] = {2.135148031994806, 1.2565080623032994, 1.8285521338688722, -2.1209061138279561, -10.032144196310302, -0.67309589196913922, 0.22977584996744049}; 00060 double Poses::leftHighB[7] = {0.58927124475644921, 1.1361294867256602, 0.50530410666457093, -1.8312199203495172, -9.7331326038628561, -0.70211634167556902, 0.08798027936286168}; 00061 00062 // l parked 00063 double Poses::lf0[7] = {2.1351842858261678, 1.271126805223832, 2.0838860205058944, -1.9832158820169317, -11.019464301160765, -1.2738934655593956, 1.3565429096270885}; 00064 00065 // l at shoulder 00066 double Poses::lf1[7] = { 1.4143112479071762, -0.35258814884307066, 1.071245291980063, -2.1200242042743653, -13.195324629719147, -0.72611616150550196, 1.5147413431092138 }; 00067 00068 // front of head 00069 double Poses::lf2[7] = { 0.24781514169438634, -0.35318031330480182, 0.75358238094131813, -1.513145382154617, -11.227193190400229, -1.1311842076327259, 1.652838655505326 }; 00070 00071 //almost in 00072 double Poses::lf3[7] = { -0.4896392196960021, -0.35292652853548845, 1.5119003548445102, -0.57300438242998797, -10.336405898448547, -0.622477913828268, 2.0416343655153022 }; 00073 00074 00075 double Poses::dishA[7] = {-2.1348350112742915, 0.64738083363749299, -1.595672374796923, -1.7539000182407651, -50.992267925964406, -0.56283267770359235, -37.575928292312355}; 00076 double Poses::dishB[7] = {-2.0394094946964008, -0.050442687051153672, -1.3657235537825063, -1.7921194860460163, -48.520404053480007, -0.07701207587152159, -37.641191672461751}; 00077 double Poses::dishC[7] = {-0.80451542916409791, 0.098275187766477728, -1.4459009669674494, -0.39189716554453613, -48.893864846832827, -0.076881549111220027, -37.661771391668857}; 00078 //double Poses::dishD[7] = {-1.444720328 00079 00080 double Poses::prepDishL0[7] = {2.1204735607226191, 1.0488275260818631, 1.9481768343408072, -2.1217747380962573, 1.6931710931335584, -1.2368577038996227, -9.4751029760075802}; 00081 double Poses::prepDishR0[7] = {-2.0707974870019181, 1.0230720460664071, -1.510142609352733, -2.1229357008475702, -1.4089080873262274, -1.7744945411847524, 0.42158788965223049}; 00082 //double Poses::prepDishL1[7] = {1.5371426009988673, -0.15318173630933338, 1.0952495128793824, -2.1017963799253305, 0.66713731189146697, -0.7240180626857029, -10.049029141041331}; 00083 //loat Poses::prepDishR1[7] = {-1.6168837569724208, -0.15550442262485537, -0.81821153356667375, -2.1222118472906528, -0.90575412503025221, -1.3540678462623723, 1.7130631649684915}; 00084 00085 //double Poses::prepDishL1[7] = {1.5371426009988673, 0.15318173630933338, 1.0952495128793824, -2.1017963799253305, 0.66713731189146697, -0.7240180626857029, -10.049029141041331}; 00086 //double Poses::prepDishR1[7] = {-1.6168837569724208, 0.15550442262485537, -0.81821153356667375, -2.1222118472906528, -0.90575412503025221, -1.3540678462623723, 1.7130631649684915}; 00087 00088 double Poses::prepDishL1[7] = {1.5371426009988673, 0.15318173630933338, 1.25, -2.1017963799253305, 0.66713731189146697, -0.7240180626857029, -10.049029141041331}; 00089 double Poses::prepDishR1[7] = {-1.6168837569724208, 0.15550442262485537, -1.25, -2.1222118472906528, -0.90575412503025221, -1.3540678462623723, 1.7130631649684915}; 00090 00091 double Poses::prepDishRT[7] = {-2.0669837826034967, 1.0311085637613304, -2.1344039484107, -2.0741479711113211, -8.0150454170802359, -1.1505933611566062, -11.782132050722778}; 00092 double Poses::prepDishLT[7] = { 2.135148031994806, 1.2355285213733944, 2.1335470136243955, -2.1226433623645584, 2.0510710200966762, -1.2228882327676454, 2.4566159301048049 }; 00093 00094 // robot poses in map frame 00095 double Poses::poseA[4] = {-.942, .485, 1, 0}; // island left 00096 double Poses::poseB[4] = {-1.013, 1.111, 1, 0}; //island middle 00097 double Poses::poseC[4] = {-1.089, 2.039, 1, 0}; //island right 00098 //double poseD[4] = { .212, 2.89, 0, 1}; // below oven 00099 double Poses::poseD[4] = { .09, 2.74, 0.179, .984}; // below oven 00100 double Poses::poseE[4] = { .117, 1.957, 0, 1}; //sink left 00101 double Poses::poseF[4] = { .115, 1.17, 0, 1}; // sink dishwasher 00102 double Poses::poseG[4] = { .232, .501, 0.05, 0.999}; // sink righ / trash 00103 double Poses::poseH[4] = { .242, 2.263, 0, 1}; // right of oven 00104 //double Poses::poseI[4] = { .323, -0.05, .069, .998}; // fridge 00105 00106 //0.309, 0.040, 0.051] 00107 //- Rotation: in Quaternion [-0.001, 0.000, -0.064, 0.998] 00108 double Poses::poseI[4] = { 0.348, 0.060, -.064, .998}; // fridge 00109 //0.348, 0.060, 0.056 00110 00111 //[0.115, 1.139, 0.053] 00112 //- Rotation: in Quaternion [-0.000, 0.002, 0.035, 0.999] 00113 double Poses::poseJ[4] = { 0.115, 1.139, 0.035, 0.999}; // fridge 00114 00115 00116 00117 //double poseD1[4] = { -0.996, 2.152, 0.921, -.390}; // %8 00118 //double poseD2[4] = { -.754, 0.239, 0.952, .306}; // %9 00119 //double poseD3[4] = { .242, 2.263, 0, 1}; // right of heater 00120 //double poseD4[4] = { .242, 2.263, 0, 1}; // right of heater 00121 00122 double Poses::pose0[4] = { -0.377, 1.382, 0, 1}; // somewhere in between island and sink 00123 00124 double *Poses::poses[] = {Poses::poseA, Poses::poseB, Poses::poseC, Poses::poseD, Poses::poseE, Poses::poseF, Poses::poseG, Poses::poseH, Poses::poseI, Poses::poseJ}; 00125 00126 int Poses::drw_top = 2; 00127 int Poses::drw_mid = 1; 00128 int Poses::drw_low = 0; 00129 00130 double Poses::inDrawer0[] = {0.515, 1.1, 0.5};