Poses.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2010, Thomas Ruehr <ruehr@cs.tum.edu>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */



#include <ias_drawer_executive/Poses.h>

double Poses::milehighPoseA[7] = {-1.2941619112744558, 0.49942431312780172, -0.12505826215869686, -2.1195908351955559, -196.75657241281269, -1.4749302507914408, 284.5875016118315};
double Poses::milehighPoseB[7] = {-0.75344495287219471, 0.0071664555829800761, -0.24500367228337172, -1.4238227962863217, -197.23479428063303, -0.094914323072497142, 284.39432200658928};
//{-0.82681730923312724, 0.15596892532371576, -0.16835406527856578, -1.7773528734848965, -197.19522687315879, -1.0987956365304499, 284.37391632306253};
// arm joint angles for seeing the marker with the hand camera
//high drawer
double Poses::highPoseA[7] = {-1.2658078481383668, 0.65194895948513354, -0.6651333173724735, -2.1146686310085161, -52.566807785671713, -1.1327560220881783, -29.66004589615245};
double Poses::highPoseB[7] = {-0.83999946139288795, 0.64805759302232857, -0.20651851395459886, -1.979887098710452, -52.732655383958971, -1.1379335835800284, -29.902173036506696};

//low drawer
double Poses::lowPoseA[7] = {-0.58879241080122102,1.2959701090793467,0.61289464879551958,-1.3123493485210054, -1.819574788110754,-1.1066071611083139,-0.425};
double Poses::lowPoseB[7] = {-0.56640762411483492,1.1399770708747323,0.49102498075440626,-0.9884972671560539,-1.8417302224128111,-0.88001270522963293,-0.425};

//middle drawer
double Poses::midPoseA[7] = {-1.136473525061469,0.87282630371086467,-0.20427354638542061,-2.1237891858256783,-2.2135217141215739,-1.2897797147276118,1.4621021956879214};
double Poses::midPoseB[7] = {-1.1291777427340541, 0.83112100662036859,-0.55432813235088196,-1.9421019430393511,-2.3539661773600271845,-0.76206336967963972,1.46};

//tucking pose, expects left arm to be tucked already
double Poses::tuckPose[7] = {0.00059073198959080919, 1.1486903479544912, -1.4332329356842286, -1.4462622565507686, -51.274908792804851, -0.2154139507083308, -31.660673060172002};
//untucking poses, hand follows the base in a circular motion
double Poses::untuckPoseA[7] = {-1.3239253869056138, 1.2547723815274774, -2.1150616574089849, -1.6996110014719421, -51.308807185757871, -0.15602427477238412, -31.606852526008804};
double Poses::untuckPoseB[7] = {-2.2269442631579031, 0.96740342774164501, -2.3171087386350413, -1.7653369044400637, -51.229440982461696, -0.10677217721964216, -31.598672849030081};

double Poses::tuckPoseForLeft[7] = {-2.1349672088362253, 1.1009839702456099, -2.2575564550627725, -2.1229357008475702, -0.93467766850553247, -0.96966653718245521, 0.17175967044035967};


double Poses::leftHighA[7] = {2.135148031994806, 1.2565080623032994, 1.8285521338688722, -2.1209061138279561, -10.032144196310302, -0.67309589196913922, 0.22977584996744049};
double Poses::leftHighB[7] = {0.58927124475644921, 1.1361294867256602, 0.50530410666457093, -1.8312199203495172, -9.7331326038628561, -0.70211634167556902, 0.08798027936286168};

// l parked
double Poses::lf0[7] =  {2.1351842858261678, 1.271126805223832, 2.0838860205058944, -1.9832158820169317, -11.019464301160765, -1.2738934655593956, 1.3565429096270885};

// l at shoulder
double Poses::lf1[7]  = { 1.4143112479071762, -0.35258814884307066, 1.071245291980063, -2.1200242042743653, -13.195324629719147, -0.72611616150550196, 1.5147413431092138 };

// front of head
double Poses::lf2[7]  = { 0.24781514169438634, -0.35318031330480182, 0.75358238094131813, -1.513145382154617, -11.227193190400229, -1.1311842076327259, 1.652838655505326 };

//almost in
double Poses::lf3[7]  = { -0.4896392196960021, -0.35292652853548845, 1.5119003548445102, -0.57300438242998797, -10.336405898448547, -0.622477913828268, 2.0416343655153022 };


double Poses::dishA[7] = {-2.1348350112742915, 0.64738083363749299, -1.595672374796923, -1.7539000182407651, -50.992267925964406, -0.56283267770359235, -37.575928292312355};
double Poses::dishB[7] = {-2.0394094946964008, -0.050442687051153672, -1.3657235537825063, -1.7921194860460163, -48.520404053480007, -0.07701207587152159, -37.641191672461751};
double Poses::dishC[7] =  {-0.80451542916409791, 0.098275187766477728, -1.4459009669674494, -0.39189716554453613, -48.893864846832827, -0.076881549111220027, -37.661771391668857};
//double Poses::dishD[7] = {-1.444720328

double Poses::prepDishL0[7] = {2.1204735607226191, 1.0488275260818631, 1.9481768343408072, -2.1217747380962573, 1.6931710931335584, -1.2368577038996227, -9.4751029760075802};
double Poses::prepDishR0[7] = {-2.0707974870019181, 1.0230720460664071, -1.510142609352733, -2.1229357008475702, -1.4089080873262274, -1.7744945411847524, 0.42158788965223049};
//double Poses::prepDishL1[7] = {1.5371426009988673, -0.15318173630933338, 1.0952495128793824, -2.1017963799253305, 0.66713731189146697, -0.7240180626857029, -10.049029141041331};
//loat Poses::prepDishR1[7] = {-1.6168837569724208, -0.15550442262485537, -0.81821153356667375, -2.1222118472906528, -0.90575412503025221, -1.3540678462623723, 1.7130631649684915};

//double Poses::prepDishL1[7] = {1.5371426009988673, 0.15318173630933338, 1.0952495128793824, -2.1017963799253305, 0.66713731189146697, -0.7240180626857029, -10.049029141041331};
//double Poses::prepDishR1[7] = {-1.6168837569724208, 0.15550442262485537, -0.81821153356667375, -2.1222118472906528, -0.90575412503025221, -1.3540678462623723, 1.7130631649684915};

double Poses::prepDishL1[7] = {1.5371426009988673, 0.15318173630933338, 1.25, -2.1017963799253305, 0.66713731189146697, -0.7240180626857029, -10.049029141041331};
double Poses::prepDishR1[7] = {-1.6168837569724208, 0.15550442262485537, -1.25, -2.1222118472906528, -0.90575412503025221, -1.3540678462623723, 1.7130631649684915};

double Poses::prepDishRT[7] = {-2.0669837826034967, 1.0311085637613304, -2.1344039484107, -2.0741479711113211, -8.0150454170802359, -1.1505933611566062, -11.782132050722778};
double Poses::prepDishLT[7] = { 2.135148031994806, 1.2355285213733944, 2.1335470136243955, -2.1226433623645584, 2.0510710200966762, -1.2228882327676454, 2.4566159301048049 };

// robot poses in map frame
double Poses::poseA[4] = {-.942, .485, 1, 0}; // island left
double Poses::poseB[4] = {-1.013, 1.111, 1, 0}; //island middle
double Poses::poseC[4] = {-1.089, 2.039, 1, 0}; //island right
//double poseD[4] = { .212, 2.89, 0, 1}; // below oven
double Poses::poseD[4] = { .09, 2.74, 0.179, .984}; // below oven
double Poses::poseE[4] = { .117, 1.957, 0, 1}; //sink left
double Poses::poseF[4] = { .115, 1.17, 0, 1}; // sink dishwasher
double Poses::poseG[4] = { .232, .501, 0.05, 0.999}; // sink righ / trash
double Poses::poseH[4] = { .242, 2.263, 0, 1}; // right of oven
//double Poses::poseI[4] = { .323, -0.05, .069, .998}; // fridge

//0.309, 0.040, 0.051]
//- Rotation: in Quaternion [-0.001, 0.000, -0.064, 0.998]
double Poses::poseI[4] = { 0.348, 0.060, -.064, .998}; // fridge
//0.348, 0.060, 0.056

//[0.115, 1.139, 0.053]
//- Rotation: in Quaternion [-0.000, 0.002, 0.035, 0.999]
double Poses::poseJ[4] = { 0.115, 1.139, 0.035, 0.999}; // fridge



//double poseD1[4] = { -0.996, 2.152, 0.921, -.390}; // %8
//double poseD2[4] = { -.754, 0.239, 0.952, .306}; // %9
//double poseD3[4] = { .242, 2.263, 0, 1}; // right of heater
//double poseD4[4] = { .242, 2.263, 0, 1}; // right of heater

double Poses::pose0[4] = { -0.377, 1.382, 0, 1}; // somewhere in between island and sink

double *Poses::poses[] = {Poses::poseA, Poses::poseB, Poses::poseC, Poses::poseD, Poses::poseE, Poses::poseF, Poses::poseG, Poses::poseH, Poses::poseI, Poses::poseJ};

int Poses::drw_top = 2;
int Poses::drw_mid = 1;
int Poses::drw_low = 0;

double Poses::inDrawer0[] = {0.515, 1.1, 0.5};