DemoScripts.cpp
Go to the documentation of this file.
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 #include <ros/ros.h>
00032 
00033 #include <ias_drawer_executive/RobotDriver.h>
00034 #include <ias_drawer_executive/Gripper.h>
00035 #include <ias_drawer_executive/Perception3d.h>
00036 #include <ias_drawer_executive/Pressure.h>
00037 #include <ias_drawer_executive/Geometry.h>
00038 #include <ias_drawer_executive/Poses.h>
00039 #include <ias_drawer_executive/RobotArm.h>
00040 #include <ias_drawer_executive/AverageTF.h>
00041 #include <ias_drawer_executive/Approach.h>
00042 #include <ias_drawer_executive/Torso.h>
00043 #include <ias_drawer_executive/Head.h>
00044 #include <ias_drawer_executive/OperateHandleController.h>
00045 
00046 #include <boost/thread.hpp>
00047 
00048 #include <actionlib/client/simple_client_goal_state.h>
00049 #include <visualization_msgs/Marker.h>
00050 
00051 #include <ias_drawer_executive/DemoScripts.h>
00052 
00053 int DemoScripts::openFridge(int z)
00054 {
00055 
00056     Torso *torso = Torso::getInstance();
00057     boost::thread t2(&Torso::up, torso);
00058 
00059     OperateHandleController::plateTuckPose();
00060     RobotDriver::getInstance()->moveBaseP(0.165, -0.247, -0.044, 0.999, false);
00061     t2.join();
00062 
00063     tf::Stamped<tf::Pose> handleHint;
00064     handleHint.setOrigin(btVector3( 0.919, -0.553, 1.177 ));
00065     handleHint.frame_id_ = "/map";
00066     tf::Stamped<tf::Pose> handlePos = Perception3d::getHandlePoseFromLaser(handleHint);
00067     //handlePos = OperateHandleController::getHandlePoseFromLaser(handleHint);
00068     handlePos.setRotation(btQuaternion(0.999, 0.025, 0.025, -0.007));
00069 
00070     // we wait max 60 seconds for the laser handle, then go for map pose
00071     if (handlePos.getOrigin().z() < 0)
00072     {
00073         ROS_ERROR("--------------------------------------------------------------");
00074         ROS_ERROR("DIDNT GET LASER DETECTED HANDLE, FALLING BACK TO MAP POSE NOW!");
00075         ROS_ERROR("--------------------------------------------------------------");
00076         // fallback : map pose
00077         OperateHandleController::plateTuckPose();
00078         RobotDriver::getInstance()->moveBaseP(0.165, -0.247, -0.044, 0.999, false);
00079         tf::Stamped<tf::Pose> handleHint;
00080         handleHint.setOrigin(btVector3( 0.919, -0.553, 1.177 ));
00081         handleHint.frame_id_ = "/map";
00082         //tf::Stamped<tf::Pose> handle = OperateHandleController::getHandlePoseFromLaser(handleHint);
00083         //handle = OperateHandleController::getHandlePoseFromLaser(handleHint);
00084         //handle.setRotation(btQuaternion(0.999, 0.025, 0.025, -0.007));
00085 
00086         tf::Stamped<tf::Pose> handleMap;
00087         handleMap.frame_id_ = "map";
00088         handleMap.setOrigin(btVector3(0.920, -0.566, 1.174));
00089         handleMap.setRotation(btQuaternion(0.999, 0.014, -0.030, 0.008));
00090 
00091         handleMap = Geometry::getPoseIn("base_link", handleMap);
00092 
00093         return OperateHandleController::operateHandle(0,handleMap);
00094     }
00095     else
00096         return OperateHandleController::operateHandle(0, handlePos);
00097 }
00098 
00099 
00100 int DemoScripts::takeBottle(int z)
00101 {
00102 
00103     pr2_controllers_msgs::JointTrajectoryGoal goalB = RobotArm::getInstance(1)->twoPointTrajectory(Poses::prepDishL1,Poses::prepDishL1);
00104     boost::thread t3(&RobotArm::startTrajectory, RobotArm::getInstance(1), goalB,true);
00105 
00106 
00107     Torso *torso = Torso::getInstance();
00108     boost::thread t2(&Torso::up, torso);
00109 
00110 
00111     RobotArm::getInstance(0)->tucked = true;
00112     double p[] = { 0.255, -0.571, -0.025, 1.000};
00113     //double p[] = { 0.255, -0.571, -0.108, 0.994 };
00114     RobotDriver::getInstance()->moveBase(p);
00115 
00116     //t2.join();t3.join();
00117 
00118     RobotHead::getInstance()->lookAtThreaded("/map", 1.243111, -0.728864, 0.9);
00119     tf::Stamped<tf::Pose> bottle = Perception3d::getBottlePose();
00120 
00121     bottle = Geometry::getPoseIn("map",bottle);
00122 
00123     if (0)
00124     {
00125         std::vector<int> arm;
00126         std::vector<tf::Stamped<tf::Pose> > goal;
00127         //btVector3 result;
00128         tf::Stamped<tf::Pose> result;
00129 
00130         tf::Stamped<tf::Pose> p0;
00131         p0.frame_id_="map";
00132         p0.stamp_=ros::Time();
00133         p0.setOrigin(btVector3(0.8, -0.455, 1.251));
00134         p0.setRotation(btQuaternion(0.005, -0.053, -0.029, 0.998));
00135         goal.push_back(p0);
00136         arm.push_back(1);
00137 
00138         tf::Stamped<tf::Pose> p1;
00139         p1.frame_id_="map";
00140         p1.stamp_=ros::Time();
00141         p1.setOrigin(btVector3(0.8, -0.655, 1.251));
00142         p1.setRotation(btQuaternion(0.005, -0.053, -0.029, 0.998));
00143         goal.push_back(p1);
00144         arm.push_back(1);
00145 
00146         tf::Stamped<tf::Pose> p2;
00147         p2.frame_id_="map";
00148         p2.stamp_=ros::Time();
00149         //p2.setOrigin(btVector3(1.165 - fridgeLink, -0.655, 1.151));
00150         p2.setOrigin(bottle.getOrigin() + btVector3(-.1,0,.03));
00151         p2.setRotation(btQuaternion(0.005, -0.053, -0.029, 0.998));
00152         goal.push_back(p2);
00153         arm.push_back(1);
00154 
00155         tf::Stamped<tf::Pose> p3;
00156         p3.frame_id_="map";
00157         p3.stamp_=ros::Time();
00158         //p3.setOrigin(btVector3(1.265000 - fridgeLink, -0.655000, 0.951000));
00159         p3.setOrigin(bottle.getOrigin() + btVector3(.02,0,.03));
00160         p3.setRotation(btQuaternion(0.005001, -0.053009, -0.029005, 0.998160));
00161         goal.push_back(p3);
00162         arm.push_back(1);
00163 
00164         RobotArm::findBaseMovement(result, arm, goal,true, false);
00165         //RobotArm::findBaseMovement(result, arm, goal,false);
00166     }
00167 
00168     Gripper::getInstance(1)->open();
00169     RobotArm::getInstance(1)->universal_move_toolframe_ik(0.8, -0.455, 1.251, 0.005, -0.053, -0.029, 0.998, "map");
00170     Gripper::getInstance(1)->updatePressureZero();
00171     RobotArm::getInstance(1)->universal_move_toolframe_ik(0.8, -0.655, 1.251, 0.005, -0.053, -0.029, 0.998, "map");
00172     //RobotArm::getInstance(1)->universal_move_toolframe_ik(1.065, -0.655, 1.151, 0.005, -0.053, -0.029, 0.998, "map");
00173     RobotArm::getInstance(1)->universal_move_toolframe_ik(bottle.getOrigin().x() - .1, bottle.getOrigin().y(), bottle.getOrigin().z() + .03, 0.005, -0.053, -0.029005, 0.998160, "map");
00174     //RobotArm::getInstance(1)->universal_move_toolframe_ik(1.265000, -0.655000, 0.951000, 0.005001, -0.053009, -0.029005, 0.998160, "map");
00175     //RobotArm::getInstance(1)->universal_move_toolframe_ik(1.2, -0.655000, 0.951000, 0.005001, -0.053009, -0.029005, 0.998160, "map");
00176     RobotArm::getInstance(1)->universal_move_toolframe_ik(bottle.getOrigin().x(), bottle.getOrigin().y(), bottle.getOrigin().z() + .03, 0.005, -0.053, -0.029005, 0.998160, "map");
00177 
00178     Gripper::getInstance(1)->closeCompliant();
00179     Gripper::getInstance(1)->close(0.04);
00180     //RobotArm::getInstance(1)->universal_move_toolframe_ik(1.065, -0.655, 1.151, 0.005, -0.053, -0.029, 0.998, "map");
00181     RobotArm::getInstance(1)->universal_move_toolframe_ik(bottle.getOrigin().x() - .1, bottle.getOrigin().y(), bottle.getOrigin().z() + .03, 0.005, -0.053, -0.029005, 0.998160, "map");
00182     RobotArm::getInstance(1)->universal_move_toolframe_ik(0.8, -0.655, 1.251, 0.005, -0.053, -0.029, 0.998, "map");
00183     RobotArm::getInstance(1)->universal_move_toolframe_ik(0.8, -0.455, 1.251, 0.005, -0.053, -0.029, 0.998, "map");
00184 
00185     return 0;
00186 }
00187 
00188 
00189 int DemoScripts::closeFridge(int handle)
00190 {
00191     OperateHandleController::close(0,handle);
00192     //go to right tuck
00193     pr2_controllers_msgs::JointTrajectoryGoal goalAT = RobotArm::getInstance(0)->twoPointTrajectory(Poses::prepDishRT,Poses::prepDishRT);
00194     boost::thread t2T(&RobotArm::startTrajectory, RobotArm::getInstance(0), goalAT,true);
00195     RobotArm::getInstance(0)->tucked = true;
00196     t2T.join();
00197     return 0;
00198 }
00199 
00200 
00201 int DemoScripts::takeBottleFromFridge(int z)
00202 {
00203 
00204 
00205     //approach fridge and grasp handle
00206     {
00207 
00208         RobotHead::getInstance()->lookAtThreaded("/map", 0.919, -0.553, 1.35);
00209 
00210 
00211         RobotArm::RobotArm *arm = RobotArm::getInstance(0);
00212         Torso *torso = Torso::getInstance();
00213         Gripper *gripper = Gripper::getInstance(0);
00214 
00215         boost::thread t2(&Torso::up, torso);
00216 
00217         OperateHandleController::plateTuckPose();
00218 
00219         RobotDriver::getInstance()->moveBaseP(0.305, -0.515, -0.348, 0.938, false);
00220         t2.join();
00221 
00222 
00223         RobotHead::getInstance()->lookAtThreaded("/map", 0.919, -0.553, 1.35);
00224 
00225 
00226         tf::Stamped<tf::Pose> handleHint;
00227         handleHint.setOrigin(btVector3( 0.919, -0.553, 1.35 ));
00228         handleHint.frame_id_ = "/map";
00229         tf::Stamped<tf::Pose> handlePos = Perception3d::getHandlePoseFromLaser(handleHint);
00230 
00231         //yes TWO Times
00232         //handlePos = OperateHandleController::getHandlePoseFromLaser(handleHint);
00233 
00234 
00235         handlePos = Geometry::getPoseIn("map",handlePos);
00236 
00237         ROS_INFO("Handle in Map");
00238         arm->printPose(handlePos);
00239 
00240         handlePos.setRotation(btQuaternion( 0.998732, 0.0416648, 0.0273237, -0.00716123));
00241 
00242         tf::Stamped<tf::Pose> handlePosApp = handlePos;
00243         tf::Stamped<tf::Pose> handlePosAppB = handlePos;
00244         handlePosApp.setOrigin(handlePosApp.getOrigin() + btVector3(-.05,0,1.35 - handlePos.getOrigin().z() ));
00245         handlePosAppB.setOrigin(handlePosAppB.getOrigin() + btVector3(.05,0,1.35 - handlePos.getOrigin().z() ));
00246         tf::Stamped<tf::Pose> handlePosAppM = handlePos;
00247         handlePosAppM.setOrigin(handlePosAppM.getOrigin() + btVector3(-.10,0,1.35 - handlePos.getOrigin().z() ));
00248 
00249 
00250         arm->time_to_target = 2;
00251         arm->move_toolframe_ik_pose(handlePosAppM);
00252         arm->time_to_target = 1;
00253 
00254         {
00255             gripper->open();
00256             arm->move_toolframe_ik_pose(handlePos);
00257             gripper->close();
00258         }
00259 
00260         /*
00261 
00262         Approach *apr = new Approach();
00263         apr->init(0,handlePosApp, handlePosAppB, Approach::front);
00264 
00265         gripper->close();
00266 
00267         double distA = (apr->increment(0,0.5));
00268         if (distA == 0)
00269         {
00270             ROS_ERROR("DIDNT TOUCH IN THE FIRST 5 CM OF APPROACH");
00271             distA = (apr->increment(0.5,1));
00272         }
00273         //back up 5 centimeter
00274         apr->move_to((distA - .5));
00275         gripper->open();
00276         //go 2.75 cm forward from touch position
00277         apr->move_to((distA + .375));
00278 
00279         gripper->closeCompliant();
00280 
00281         gripper->close();*/
00282 
00283 
00284         arm->stabilize_grip();
00285     }
00286 
00287 // open fridge, take bottle and close it againg
00288 
00289     {
00290 
00291         //boost::thread t2(&cart_err_monit, 0);
00292 
00293         int side = 0;
00294         RobotArm::RobotArm *arm = RobotArm::getInstance(side);
00295 
00296         double pts[][7] =
00297         {
00298             {0.478704, -1.0355, 1.18101, 0.767433, 0.639987, 0.022135, 0.0311955},
00299             {0.489086, -0.984206, 1.17956, 0.797904, 0.601535, 0.01726, 0.0347398},
00300             {0.494529, -0.937741, 1.1803, 0.830545, 0.555891, 0.0110758, 0.0325103},
00301             {0.504333, -0.909376, 1.18066, 0.849808, 0.526105, 0.00709967, 0.03147},
00302             {0.507886, -0.884252, 1.17954, 0.8814, 0.471274, 0.00699274, 0.0313926},
00303             {0.516993, -0.854729, 1.18006, 0.903026, 0.428457, 0.00859376, 0.0299171},
00304             {0.527833, -0.832331, 1.1803, 0.920176, 0.390256, 0.0125722, 0.0286066},
00305             {0.541463, -0.80644, 1.18, 0.931353, 0.362808, 0.0186723, 0.0245782},
00306             {0.571712, -0.760535, 1.17887, 0.936451, 0.349496, 0.024334, 0.017896},
00307             {0.608236, -0.715618, 1.17839, 0.944274, 0.327791, 0.0273483, 0.0123364},
00308             {0.647457, -0.676296, 1.17812, 0.954053, 0.298037, 0.0302956, 0.00623379},
00309             {0.690692, -0.638766, 1.17999, 0.964469, 0.262043, 0.0336022, 0.00195834},
00310             {0.734141, -0.609302, 1.18042, 0.974717, 0.220844, 0.0339708, -0.00102721},
00311             {0.781735, -0.583995, 1.17916, 0.983083, 0.180164, 0.0327274, -0.00426907},
00312             {0.828575, -0.564397, 1.17937, 0.990023, 0.137179, 0.0315954, -0.00617472},
00313             {0.870116, -0.550422, 1.17831, 0.995336, 0.0920069, 0.0283872, -0.00586025},
00314             {0.921693, -0.544899, 1.17853, 0.998734, 0.0415909, 0.0273629, -0.00714236},
00315             {0.971471, -0.549669, 1.17854, 0.998732, 0.0416648, 0.0273237, -0.00716123}
00316         };
00317 
00318 
00319         //for (double z= 1.3; z <= 1.4; z +=0.025)
00320         double z = 1.35;
00321         {
00322             int numf = 0;
00323             bool found = true;
00324 
00325             double stA[7];
00326             arm->getJointState(stA);
00327 
00328             RobotHead::getInstance()->lookAtThreaded("/map", 1.243111, -0.728864, 0.7);
00329 
00330             for (int k = 0; k <= 16; ++k)
00331             {
00332                 tf::Stamped<tf::Pose> act;
00333                 act.frame_id_ = "map";
00334                 act.setOrigin(btVector3(pts[k][0],pts[k][1],z));
00335                 act.setRotation(btQuaternion(pts[k][3],pts[k][4],pts[k][5],pts[k][6]));
00336                 act = arm->tool2wrist(act);
00337 
00338                 geometry_msgs::PoseStamped stamped_pose;
00339                 stamped_pose.header.frame_id = "map";
00340                 stamped_pose.header.stamp = ros::Time::now();
00341                 stamped_pose.pose.position.x=act.getOrigin().x();
00342                 stamped_pose.pose.position.y=act.getOrigin().y();
00343                 stamped_pose.pose.position.z=z;
00344                 stamped_pose.pose.orientation.x=pts[k][3];
00345                 stamped_pose.pose.orientation.y=pts[k][4];
00346                 stamped_pose.pose.orientation.z=pts[k][5];
00347                 stamped_pose.pose.orientation.w=pts[k][6];
00348 
00349                 double stAs[7];
00350                 int ret = arm->run_ik(stamped_pose,stA,stAs,side == 0 ? "r_wrist_roll_link" : "l_wrist_roll_link");
00351                 ROS_INFO("       z %f k %i %s", z, k, ret ? "found." : "not found.");
00352                 numf += ret;
00353                 if (ret==0)
00354                     found = false;
00355                 else
00356                 {
00357                     for (int i = 0; i < 7; ++i)
00358                         stA[i] = stAs[i];
00359                 }
00360             }
00361             ROS_INFO("FOR HEIGHT z %f %s num %i", z, found ? "found." : "not found.", numf);
00362             if (found)
00363             {
00364 
00365                 double stA[7];
00366                 arm->getJointState(stA);
00367                 ros::Rate rate(3.0);
00368                 int idx[] = {16,15,14,13,12,11,10,9,8,7,6,5,4,3,2,1,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,16,16};
00369                 for (int o = 0; o <= 16+16; ++o)
00370                     //for (int k = 16; k >= 0; --k)
00371                 {
00372                     int k = idx[o];
00373                     tf::Stamped<tf::Pose> act;
00374                     act.frame_id_ = "map";
00375                     act.setOrigin(btVector3(pts[k][0],pts[k][1],z));
00376                     act.setRotation(btQuaternion(pts[k][3],pts[k][4],pts[k][5],pts[k][6]));
00377                     act = arm->tool2wrist(act);
00378 
00379                     geometry_msgs::PoseStamped stamped_pose;
00380                     stamped_pose.header.frame_id = "map";
00381                     stamped_pose.header.stamp = ros::Time::now();
00382                     stamped_pose.pose.position.x=act.getOrigin().x();
00383                     stamped_pose.pose.position.y=act.getOrigin().y();
00384                     stamped_pose.pose.position.z=z;
00385                     stamped_pose.pose.orientation.x=pts[k][3];
00386                     stamped_pose.pose.orientation.y=pts[k][4];
00387                     stamped_pose.pose.orientation.z=pts[k][5];
00388                     stamped_pose.pose.orientation.w=pts[k][6];
00389 
00390                     double stAs[7];
00391                     int ret = arm->run_ik(stamped_pose,stA,stAs,side == 0 ? "r_wrist_roll_link" : "l_wrist_roll_link");
00392 
00393                     //ROS_INFO("       z %f k %i %s", z, k, ret ? "found." : "not found.");
00394                     ROS_INFO("k : %i",k);
00395 
00396                     rate.sleep();
00397                     if (ret)
00398                     {
00399                         double pose[7];
00400                         double sum = 0;
00401                         for (int k = 0; k < 7; ++k)
00402                         {
00403                             pose[k] = stAs[k];
00404                             sum += stAs[k] * stAs[k];
00405                         }
00406                         if (sum > 0.01)
00407                             arm->startTrajectory(arm->goalTraj(pose),false);
00408                     }
00409                     if (k == 0)
00410                     {
00411 
00412                         RobotHead::getInstance()->lookAtThreaded("/map", 1.181542, -0.763771, 0.967066);
00413 
00414                         tf::Stamped<tf::Pose> bottle = Perception3d::getBottlePose();
00415 
00416                         Gripper::getInstance(1)->close();
00417 
00418                         double ptA[] = {0.41491862845470812, 1.3468554401788568, 1.501748997727044, -2.0247783614692936, -16.507431415382143, -1.3292235155277217, 15.027356561279952};
00419                         double ptB[] = {0.040263624618489424, 0.96465557759293075, 0.27150676981727662, -1.6130504582945409, -14.582800985450046, -1.1869058378819473, 14.819427432123987};
00420                         RobotArm *arml = RobotArm::getInstance(1);
00421                         arml->startTrajectory(arml->goalTraj(ptA,1.5));
00422                         arml->startTrajectory(arml->goalTraj(ptB,1.5));
00423                         Gripper::getInstance(1)->open();
00424                         RobotArm::getInstance(1)->universal_move_toolframe_ik(bottle.getOrigin().x() - .1, bottle.getOrigin().y(), bottle.getOrigin().z(), 0.005, -0.053, -0.029005, 0.998160, "map");
00425                         RobotArm::getInstance(1)->universal_move_toolframe_ik(bottle.getOrigin().x(), bottle.getOrigin().y(), bottle.getOrigin().z(), 0.005, -0.053, -0.029005, 0.998160, "map");
00426 
00427                         //Gripper::getInstance(1)->closeCompliant();
00428                         Gripper::getInstance(1)->close(0.02);
00429                         RobotArm::getInstance(1)->universal_move_toolframe_ik(bottle.getOrigin().x() - .1, bottle.getOrigin().y(), bottle.getOrigin().z(), 0.005, -0.053, -0.029005, 0.998160, "map");
00430                         arml->startTrajectory(arml->goalTraj(ptB,1.5));
00431                         RobotArm::getInstance(1)->universal_move_toolframe_ik(0.208, 0.120, 0.785 , -0.020, -0.101, -0.662, 0.742, "base_link");
00432                     }
00433                 }
00434             }
00435         }
00436 
00437         Gripper::getInstance(0)->open();
00438 
00439         double target[4];
00440         target[0] = -0.3;
00441         target[1] = -0.15;
00442         target[2] = 0;
00443         target[3] = 1;
00444         ROS_INFO("POSE IN BASE %f %f %f", target[0],target[1], target[2]);
00445         RobotDriver::getInstance()->driveInOdom(target, 1);
00446 
00447         pr2_controllers_msgs::JointTrajectoryGoal goalAT = RobotArm::getInstance(0)->twoPointTrajectory(Poses::prepDishRT,Poses::prepDishRT);
00448         boost::thread t2T(&RobotArm::startTrajectory, RobotArm::getInstance(0), goalAT,true);
00449         t2T.join();
00450 
00451         //RobotArm::getInstance(1)->universal_move_toolframe_ik(0.308, 0.120, 0.91 , -0.020, -0.101, -0.662, 0.742, "base_link");
00452 
00453         //[1.1811415860679706, 0.40573692064179873, 1.772267589813042, -2.0337541455750725, 0.15224039094452749, -0.61844366806403794, 5.0541238004106495]
00454 
00455     }
00456 
00457     return 0;
00458 }
00459 
00460 
00461 int DemoScripts::serveBottle(int z)
00462 {
00463     RobotArm::getInstance(0)->tucked =true;
00464     //double b1[] = {-0.016, -0.440, 0.013, 1.000};
00465 
00466     //RobotDriver::getInstance()->moveBase(b1, false);
00467 
00468     //RobotArm::getInstance(0)->startTrajectory(RobotArm::getInstance(0)->twoPointTrajectory(Poses::untuckPoseB, Poses::untuckPoseB));
00469 
00470     RobotHead::getInstance()->lookAtThreaded("/map",  -1.7, 2.2, 1);
00471 
00472     double b3[] = {-0.496, 2.203, 0.963, 0.270};
00473     RobotDriver::getInstance()->moveBase(b3, false);
00474 
00475 
00476     double b2[] = {-1.115, 2.683, 0.977, 0.214};
00477     boost::thread t1 (&RobotDriver::moveBase,RobotDriver::getInstance(), b2, false);
00478 
00479     RobotHead::getInstance()->lookAtThreaded("/l_gripper_tool_frame", 0 , 0, 0);
00480 
00481     RobotArm::getInstance(1)->universal_move_toolframe_ik(.466, 0.486, 0.95, -0.020, -0.101, -0.662, 0.742, "base_link");
00482     RobotArm::getInstance(1)->universal_move_toolframe_ik(.466, 0.486, 1.15, -0.020, -0.101, -0.662, 0.742, "base_link");
00483 
00484     //RobotArm::getInstance(1)->universal_move_toolframe_ik(0.5, 0.608, 0.94, -0.020, -0.101, -0.662, 0.742, "base_link");
00485 
00486     t1.join();
00487 
00488     ROS_ERROR("A");
00489 
00490     RobotArm::getInstance(1)->universal_move_toolframe_ik(0.5, 0.608, 0.94, -0.059, 0.008, 0.131, 0.990, "base_link");
00491 
00492     Gripper::getInstance(1)->open();
00493 
00494     ROS_ERROR("B");
00495 
00496     RobotArm::getInstance(1)->universal_move_toolframe_ik(0.3, 0.608, 1.0, -0.059, 0.008, 0.131, 0.990, "base_link");
00497     return 0;
00498 }
00499 
00500 int DemoScripts::openDrawer(int z)
00501 {
00502 
00503     RobotHead::getInstance()->lookAtThreaded("/map", 1.212, 1.240, 1.0);
00504 
00505     RobotDriver::getInstance()->moveBaseP(-0.020, 1.818,0.051, 0.999,false);
00506 
00507     tf::Stamped<tf::Pose> p0;
00508     p0.frame_id_="map";
00509     p0.stamp_=ros::Time();
00510     p0.setOrigin(btVector3(0.64, 1.321, 0.762 + 0.03));
00511     p0.setRotation(btQuaternion(-0.714, -0.010, 0.051, 0.698));
00512     p0 = Geometry::getPoseIn("base_link",p0);
00513 
00514     //tf::Stamped<tf::Pose> p0 = RobotArm::getInstance(0)->getToolPose("base_link");
00515 
00516     int handle = OperateHandleController::operateHandle(0,p0);
00517 
00518     //RobotArm::getInstance(0)->universal_move_toolframe_ik(0.169, 1.302, 0.796, -0.709, -0.050, 0.070, 0.700, "map");
00519 
00520     //Gripper::getInstance(0)->open();
00521 
00522 
00523     //- Translation: [0.169, 1.302, 0.796]
00524 //- Rotation: in Quaternion [-0.709, -0.050, 0.070, 0.700]
00525   //          in RPY [-1.580, 0.028, 0.170]
00526 
00527 
00528     RobotDriver::getInstance()->moveBaseP(-0.220, 1.818,0.051, 0.999,false);
00529 
00530     return 0;
00531 }
00532 
00533 
00534 int DemoScripts::takePlate(int z)
00535 {
00536     RobotHead::getInstance()->lookAtThreaded("/map", .44 ,1.1, .7);
00537 
00538     Torso *torso = Torso::getInstance();
00539     boost::thread t2u(&Torso::up, torso);
00540     double target[] = {-0.329, 1.017, 0.018, 1.000};
00541     ROS_INFO("POSE IN BASE %f %f %f", target[0],target[1], target[2]);
00542 
00543     OperateHandleController::plateTuckPose();
00544 
00545     RobotArm::getInstance(0)->tucked = true;
00546     RobotDriver::getInstance()->moveBase(target,false);
00547 
00548     OperateHandleController::plateAttackPose();
00549 
00550     t2u.join();
00551 
00552     //in drawer
00553     RobotHead::getInstance()->lookAtThreaded("/map", .44 ,1.1, .7);
00554 
00555     OperateHandleController::getPlate(0,0.79 - 0.035 + 0.03);
00556 
00557     RobotDriver::getInstance()->moveBaseP(-0.437, 1.053, 0.315, .949,false);
00558 
00559     double cl[] = {0.12, 1.053, 0.315, 0.949};
00560 
00561     RobotDriver::getInstance()->moveBase(cl,false);
00562 
00563     return 0;
00564 }
00565 
00566 
00567 int DemoScripts::servePlateToIsland(int z)
00568 {
00569 
00570     RobotHead::getInstance()->lookAtThreaded("/map", -1.85, 2.1, 1);
00571 
00572     RobotArm::getInstance(0)->tucked = true;
00573 
00574     //double b4[] = {-1.066, 1.564, 0.970, 0.244};
00575 
00576     double b4[] = {-0.960, 2.161,  0.999, -0.037};
00577 
00578     //OperateHandleController::plateCarryPose();
00579 
00580     RobotDriver::getInstance()->moveBase(b4, false);
00581 
00582     OperateHandleController::spinnerL(0.25,0,-.11);
00583 
00584     OperateHandleController::openGrippers();
00585 
00586     OperateHandleController::plateCarryPose();
00587 
00588     return 0;
00589 }
00590 
00591 
00592 int DemoScripts::takeSilverware(int z)
00593 {
00594     // get silverware
00595 //        [0.157, 1.154, 0.051] - Rotation: in Quaternion [0.001, -0.001, 0.044, 0.999]
00596     pr2_controllers_msgs::JointTrajectoryGoal goalA = RobotArm::getInstance(0)->twoPointTrajectory(Poses::prepDishR1,Poses::prepDishR1);
00597     pr2_controllers_msgs::JointTrajectoryGoal goalB = RobotArm::getInstance(1)->twoPointTrajectory(Poses::prepDishL1,Poses::prepDishL1);
00598     boost::thread t2(&RobotArm::startTrajectory, RobotArm::getInstance(0), goalA,true);
00599     boost::thread t3(&RobotArm::startTrajectory, RobotArm::getInstance(1), goalB,true);
00600 
00601     RobotHead::getInstance()->lookAtThreaded("/map", 0.812, 1.240, 1.0);
00602 
00603     RobotDriver::getInstance()->moveBaseP(0.157, 1.154,  0.044, 0.999);
00604     t2.join();
00605     t3.join();
00606 
00607     double zAdj = -.015;
00608     /* R
00609      Translation: [0.940, 1.032, 0.861]
00610     - Rotation: in Quaternion [-0.026, 0.733, 0.025, 0.679]
00611     rosrun tf tf_echo map l_gripper_tool_frame
00612     At time 1286912134.641
00613     - Translation: [0.922, 1.354, 0.864]
00614     - Rotation: in Quaternion [0.003, 0.723, 0.006, 0.690]
00615                 in RPY [2.856, 1.522, 2.860]
00616                 */
00617 
00618     double xR =  .940;
00619     double xL =  .922;
00620     double yR =  1.032;
00621     double yL =  1.354;
00622     double adj = 0.025;
00623 
00624     OperateHandleController::openGrippers();
00625 
00626     RobotHead::getInstance()->lookAtThreaded("/map", 0.812, 1.240, 1.0);
00627 
00628     RobotArm::getInstance(1)->universal_move_toolframe_ik(0.812, 1.240, 1.0, 0.009, 0.679, 0.002, 0.734, "map");
00629     RobotArm::getInstance(1)->universal_move_toolframe_ik(xL, yL, 1.0, 0.009, 0.679, 0.002, 0.734, "map");
00630     RobotArm::getInstance(1)->universal_move_toolframe_ik(xL, yL, 0.860 + zAdj, 0.009, 0.679, 0.002, 0.734, "map");
00631     Gripper::getInstance(1)->closeCompliant();
00632     Gripper::getInstance(1)->close();
00633     RobotArm::getInstance(1)->universal_move_toolframe_ik(xL, yL, 1.0, 0.009, 0.679, 0.002, 0.734, "map");
00634 
00635     if (Gripper::getInstance(1)->getAmountOpen() <  0.001)
00636     {
00637         Gripper::getInstance(1)->open();
00638         RobotArm::getInstance(1)->universal_move_toolframe_ik(0.812, 1.240, 1.0, 0.009, 0.679, 0.002, 0.734, "map");
00639         RobotArm::getInstance(1)->universal_move_toolframe_ik(xL, yL, 1.0, 0.009, 0.679, 0.002, 0.734, "map");
00640         RobotArm::getInstance(1)->universal_move_toolframe_ik(xL, yL, 0.860 + zAdj, 0.009, 0.679, 0.002, 0.734, "map");
00641         Gripper::getInstance(1)->closeCompliant();
00642         Gripper::getInstance(1)->close();
00643         RobotArm::getInstance(1)->universal_move_toolframe_ik(xL, yL, 1.0, 0.009, 0.679, 0.002, 0.734, "map");
00644 
00645         if (Gripper::getInstance(1)->getAmountOpen() <  0.001)
00646         {
00647             Gripper::getInstance(1)->open();
00648             RobotArm::getInstance(1)->universal_move_toolframe_ik(0.812, 1.240, 1.0, 0.009, 0.679, 0.002, 0.734, "map");
00649             RobotArm::getInstance(1)->universal_move_toolframe_ik(xL, yL - adj, 1.0, 0.009, 0.679, 0.002, 0.734, "map");
00650             RobotArm::getInstance(1)->universal_move_toolframe_ik(xL, yL - adj, 0.860 + zAdj, 0.009, 0.679, 0.002, 0.734, "map");
00651             Gripper::getInstance(1)->closeCompliant();
00652             Gripper::getInstance(1)->close();
00653             RobotArm::getInstance(1)->universal_move_toolframe_ik(0.912, 1.340, 1.0, 0.009, 0.679, 0.002, 0.734, "map");
00654             if (Gripper::getInstance(1)->getAmountOpen() <  0.001)
00655             {
00656                 Gripper::getInstance(1)->open();
00657                 RobotArm::getInstance(1)->universal_move_toolframe_ik(0.812, 1.240, 1.0, 0.009, 0.679, 0.002, 0.734, "map");
00658                 RobotArm::getInstance(1)->universal_move_toolframe_ik(xL, yL + adj, 1.0, 0.009, 0.679, 0.002, 0.734, "map");
00659                 RobotArm::getInstance(1)->universal_move_toolframe_ik(xL, yL + adj, 0.860 + zAdj, 0.009, 0.679, 0.002, 0.734, "map");
00660                 Gripper::getInstance(1)->closeCompliant();
00661                 Gripper::getInstance(1)->close();
00662                 RobotArm::getInstance(1)->universal_move_toolframe_ik(0.912, 1.340, 1.0, 0.009, 0.679, 0.002, 0.734, "map");
00663             }
00664         }
00665     }
00666 
00667     RobotHead::getInstance()->lookAtThreaded("/map", xR, yR, 1.0);
00668 
00669     RobotArm::getInstance(0)->universal_move_toolframe_ik(xR, yR, 1.0, 0.044, 0.691, -0.040, 0.720, "map");
00670     RobotArm::getInstance(0)->universal_move_toolframe_ik(xR, yR, 0.851 + zAdj, 0.044, 0.691, -0.040, 0.720, "map");
00671     Gripper::getInstance(0)->closeCompliant();
00672     Gripper::getInstance(0)->close();
00673     RobotArm::getInstance(0)->universal_move_toolframe_ik(0.911, 0.95, 1.0, 0.044, 0.691, -0.040, 0.720, "map");
00674 
00675     if (Gripper::getInstance(0)->getAmountOpen() <  0.001)
00676     {
00677         Gripper::getInstance(0)->open();
00678         RobotArm::getInstance(0)->universal_move_toolframe_ik(xR, yR, 1.0, 0.044, 0.691, -0.040, 0.720, "map");
00679         RobotArm::getInstance(0)->universal_move_toolframe_ik(xR, yR, 0.851 + zAdj, 0.044, 0.691, -0.040, 0.720, "map");
00680         Gripper::getInstance(0)->closeCompliant();
00681         Gripper::getInstance(0)->close();
00682         RobotArm::getInstance(0)->universal_move_toolframe_ik(0.911, 0.95, 1.0, 0.044, 0.691, -0.040, 0.720, "map");
00683 
00684 
00685         if (Gripper::getInstance(0)->getAmountOpen() <  0.001)
00686         {
00687             Gripper::getInstance(0)->open();
00688             RobotArm::getInstance(0)->universal_move_toolframe_ik(xR, yR - adj, 1.0, 0.044, 0.691, -0.040, 0.720, "map");
00689             RobotArm::getInstance(0)->universal_move_toolframe_ik(xR, yR - adj, 0.851 + zAdj, 0.044, 0.691, -0.040, 0.720, "map");
00690             Gripper::getInstance(0)->closeCompliant();
00691             Gripper::getInstance(0)->close();
00692             RobotArm::getInstance(0)->universal_move_toolframe_ik(0.911, 0.95, 1.0, 0.044, 0.691, -0.040, 0.720, "map");
00693             if (Gripper::getInstance(0)->getAmountOpen() <  0.001)
00694             {
00695                 Gripper::getInstance(0)->open();
00696                 RobotArm::getInstance(0)->universal_move_toolframe_ik(xR, yR + adj, 1.0, 0.044, 0.691, -0.040, 0.720, "map");
00697                 RobotArm::getInstance(0)->universal_move_toolframe_ik(xR, yR + adj, 0.851 + zAdj, 0.044, 0.691, -0.040, 0.720, "map");
00698                 Gripper::getInstance(0)->closeCompliant();
00699                 Gripper::getInstance(0)->close();
00700                 RobotArm::getInstance(0)->universal_move_toolframe_ik(0.911, 0.95, 1.0, 0.044, 0.691, -0.040, 0.720, "map");
00701             }
00702         }
00703     }
00704 
00705     OperateHandleController::plateCarryPose();
00706 
00707     return 0;
00708 }
00709 
00710 
00711 int DemoScripts::serveToTable(int z)
00712 {
00713 
00714     //OperateHandleController::plateCarryPose();
00715 
00716     RobotArm::getInstance(0)->tucked = true;
00717 
00718 
00719     double p[] = {-0.461, -0.836, 0.878, -0.479};
00720     //double p[] = {-0.428, -0.786, 0.870, -0.493};
00721     RobotDriver::getInstance()->moveBase(p, false);
00722     OperateHandleController::spinnerL(0.26, -0.08, -.16);
00723 
00724     OperateHandleController::openGrippers();
00725 
00726 
00727     pr2_controllers_msgs::JointTrajectoryGoal goalA = RobotArm::getInstance(0)->twoPointTrajectory(Poses::prepDishR1,Poses::prepDishR1);
00728     pr2_controllers_msgs::JointTrajectoryGoal goalB = RobotArm::getInstance(1)->twoPointTrajectory(Poses::prepDishL1,Poses::prepDishL1);
00729     boost::thread t2(&RobotArm::startTrajectory, RobotArm::getInstance(0), goalA,true);
00730     boost::thread t3(&RobotArm::startTrajectory, RobotArm::getInstance(1), goalB,true);
00731 
00732     t2.join();
00733     t3.join();
00734 
00735     double p2[] = {-0.261, -0.636, 0.878, -0.479};
00736     RobotDriver::getInstance()->moveBase(p2, false);
00737 
00738     return 0;
00739 }
00740 
00741 int DemoScripts::serveToTable2(int z)
00742 {
00743 
00744     //OperateHandleController::plateCarryPose();
00745 
00746     RobotArm::getInstance(0)->tucked = true;
00747 
00748     double p[] = {0.275, -1.160, -0.685, 0.729};
00749     //double p[] = {-0.428, -0.786, 0.870, -0.493};
00750     RobotDriver::getInstance()->moveBase(p, false);
00751     OperateHandleController::spinnerL(0.26, -0.08, -.16);
00752 
00753     OperateHandleController::openGrippers();
00754 
00755 
00756     //pr2_controllers_msgs::JointTrajectoryGoal goalA = RobotArm::getInstance(0)->twoPointTrajectory(Poses::prepDishR1,Poses::prepDishR1);
00757     //pr2_controllers_msgs::JointTrajectoryGoal goalB = RobotArm::getInstance(1)->twoPointTrajectory(Poses::prepDishL1,Poses::prepDishL1);
00758     //boost::thread t2(&RobotArm::startTrajectory, RobotArm::getInstance(0), goalA,true);
00759     //boost::thread t3(&RobotArm::startTrajectory, RobotArm::getInstance(1), goalB,true);
00760 
00761     //t2.join();
00762     //t3.join();
00763 
00764     double p2[] = {0.232, -0.555, -0.685, 0.729};
00765     //RobotDriver::getInstance()->moveBase(p2, false);
00766 
00767     tf::Stamped<tf::Pose> rP = RobotArm::getInstance(0)->getToolPose("base_link");
00768     rP.setOrigin(rP.getOrigin() + btVector3(0,0,0.03));
00769     rP = Geometry::getPoseIn("map", rP);
00770 
00771     tf::Stamped<tf::Pose> lP = RobotArm::getInstance(1)->getToolPose("base_link");
00772     lP.setOrigin(lP.getOrigin() + btVector3(0,0.0,0.03));
00773     lP = Geometry::getPoseIn("map", lP);
00774 
00775     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(rP);
00776     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(lP);
00777 
00778     OperateHandleController::plateAttackPose();
00779     OperateHandleController::plateTuckPose();
00780 
00781 
00782     return 0;
00783 }
00784 
00785 
00786 /*
00787 
00788 ruehr@pr2a:~$ rosrun tf tf_echo base_link r_gripper_tool_frame
00789 At time 1312975090.444
00790 - Translation: [0.448, -0.122, 0.935]
00791 - Rotation: in Quaternion [0.652, 0.287, -0.626, -0.317]
00792             in RPY [-1.590, 0.689, 1.504]
00793 ^CAt time 1312975091.083
00794 - Translation: [0.448, -0.122, 0.935]
00795 - Rotation: in Quaternion [0.652, 0.286, -0.626, -0.317]
00796             in RPY [-1.590, 0.689, 1.504]
00797 ^[[Aruehr@pr2a:~$ rosrun tf tf_echo base_link l_gripper_tool_frame
00798 At time 1312975094.413
00799 - Translation: [0.452, 0.122, 0.935]
00800 - Rotation: in Quaternion [-0.295, 0.628, -0.287, 0.660]
00801             in RPY [-1.523, 0.722, -1.510]
00802 ^CAt time 1312975094.933
00803 - Translation: [0.452, 0.122, 0.935]
00804 - Rotation: in Quaternion [-0.295, 0.628, -0.287, 0.660]
00805             in RPY [-1.523, 0.722, -1.510]
00806 
00807             */
00808 
00809 
00810 int DemoScripts::takePlateFromIsland(int z)
00811 {
00812     Torso *torso = Torso::getInstance();
00813     boost::thread t2u(&Torso::up, torso);
00814 
00815     RobotHead::getInstance()->lookAtThreaded("/map", -1.712, 2.088, 0.865);
00816 
00817 
00818     OperateHandleController::plateTuckPose();
00819     //RobotArm::getInstance(1)->startTrajectory(RobotArm::getInstance(1)->twoPointTrajectory(Poses::prepDishL0,Poses::prepDishL1));
00820 
00821     //RobotArm::getInstance(0)->tucked = true;
00822     //RobotDriver::getInstance()->moveBaseP(-0.866, 1.564, 0.970, 0.244,false);
00823 
00824 
00825     //RobotDriver::getInstance()->moveBaseP(-1.066, 1.564, 0.970, 0.244,false);
00826 
00827 
00828     //RobotDriver::getInstance()->moveBaseP(-0.957, 2.178, 0.999, -0.049,false);
00829     RobotDriver::getInstance()->moveBaseP(-1.03, 2.2, 0.999, -0.049,false);
00830 
00831     OperateHandleController::plateAttackPose();
00832 
00833     t2u.join();
00834 
00835 
00836     btVector3 pl(-1.679, 1.904, 0.865);
00837 
00838     pl = btVector3(-1.712, 2.088, 0.865);
00839     pl = btVector3(-1.686, 2.1, 0.865 + 0.03);
00840 
00841     //OperateHandleController::getPlate(0);
00842     OperateHandleController::pickPlate(pl,0.27);
00843 
00844     OperateHandleController::plateCarryPose();
00845 
00846     return 0;
00847 }
00848 
00849 int DemoScripts::putObjectIntoFridge(int z)
00850 {
00851 
00852 
00853     //approach fridge and grasp handle
00854     {
00855 
00856         RobotArm::RobotArm *arm = RobotArm::getInstance(0);
00857         Torso *torso = Torso::getInstance();
00858         Gripper *gripper = Gripper::getInstance(0);
00859 
00860         boost::thread t2(&Torso::up, torso);
00861 
00862         OperateHandleController::plateTuckPose();
00863 
00864         RobotDriver::getInstance()->moveBaseP(0.305, -0.515, -0.348, 0.938, false);
00865         t2.join();
00866 
00867 
00868         RobotHead::getInstance()->lookAt("/map", 0.919, -0.553, 1.35);
00869 
00870 
00871         tf::Stamped<tf::Pose> handleHint;
00872         handleHint.setOrigin(btVector3( 0.919, -0.553, 1.35 ));
00873         handleHint.frame_id_ = "/map";
00874         tf::Stamped<tf::Pose> handlePos = Perception3d::getHandlePoseFromLaser(handleHint);
00875         handlePos = Perception3d::getHandlePoseFromLaser(handleHint);
00876         handlePos = Perception3d::getHandlePoseFromLaser(handleHint);
00877 
00878         //yes TWO Times
00879         //handlePos = OperateHandleController::getHandlePoseFromLaser(handleHint);
00880 
00881 
00882         handlePos = Geometry::getPoseIn("map",handlePos);
00883 
00884         ROS_INFO("Handle in Map");
00885         arm->printPose(handlePos);
00886 
00887         handlePos.setRotation(btQuaternion( 0.998732, 0.0416648, 0.0273237, -0.00716123));
00888 
00889         tf::Stamped<tf::Pose> handlePosApp = handlePos;
00890         tf::Stamped<tf::Pose> handlePosAppB = handlePos;
00891         handlePosApp.setOrigin(handlePosApp.getOrigin() + btVector3(-.05,0,1.35 - handlePos.getOrigin().z() ));
00892         handlePosAppB.setOrigin(handlePosAppB.getOrigin() + btVector3(.05,0,1.35 - handlePos.getOrigin().z() ));
00893         tf::Stamped<tf::Pose> handlePosAppM = handlePos;
00894         handlePosAppM.setOrigin(handlePosAppM.getOrigin() + btVector3(-.10,0,1.35 - handlePos.getOrigin().z() ));
00895 
00896 
00897         arm->time_to_target = 2;
00898         arm->move_toolframe_ik_pose(handlePosAppM);
00899         arm->time_to_target = 1;
00900 
00901         Approach *apr = new Approach();
00902         apr->init(0,handlePosApp, handlePosAppB, Approach::front);
00903 
00904         gripper->close();
00905 
00906         double distA = (apr->increment(0,0.5));
00907         if (distA == 0)
00908         {
00909             ROS_ERROR("DIDNT TOUCH IN THE FIRST 5 CM OF APPROACH");
00910             distA = (apr->increment(0.5,1));
00911         }
00912         //back up 5 centimeter
00913         apr->move_to((distA - .5));
00914         gripper->open();
00915         //go 2.75 cm forward from touch position
00916         apr->move_to((distA + .375));
00917 
00918         gripper->closeCompliant();
00919 
00920         gripper->close();
00921 
00922         arm->stabilize_grip();
00923     }
00924 
00925 // open fridge, take bottle and close it againg
00926 
00927     {
00928 
00929         //boost::thread t2(&cart_err_monit, 0);
00930 
00931         int side = 0;
00932         RobotArm::RobotArm *arm = RobotArm::getInstance(side);
00933 
00934         double pts[][7] =
00935         {
00936             {0.478704, -1.0355, 1.18101, 0.767433, 0.639987, 0.022135, 0.0311955},
00937             {0.489086, -0.984206, 1.17956, 0.797904, 0.601535, 0.01726, 0.0347398},
00938             {0.494529, -0.937741, 1.1803, 0.830545, 0.555891, 0.0110758, 0.0325103},
00939             {0.504333, -0.909376, 1.18066, 0.849808, 0.526105, 0.00709967, 0.03147},
00940             {0.507886, -0.884252, 1.17954, 0.8814, 0.471274, 0.00699274, 0.0313926},
00941             {0.516993, -0.854729, 1.18006, 0.903026, 0.428457, 0.00859376, 0.0299171},
00942             {0.527833, -0.832331, 1.1803, 0.920176, 0.390256, 0.0125722, 0.0286066},
00943             {0.541463, -0.80644, 1.18, 0.931353, 0.362808, 0.0186723, 0.0245782},
00944             {0.571712, -0.760535, 1.17887, 0.936451, 0.349496, 0.024334, 0.017896},
00945             {0.608236, -0.715618, 1.17839, 0.944274, 0.327791, 0.0273483, 0.0123364},
00946             {0.647457, -0.676296, 1.17812, 0.954053, 0.298037, 0.0302956, 0.00623379},
00947             {0.690692, -0.638766, 1.17999, 0.964469, 0.262043, 0.0336022, 0.00195834},
00948             {0.734141, -0.609302, 1.18042, 0.974717, 0.220844, 0.0339708, -0.00102721},
00949             {0.781735, -0.583995, 1.17916, 0.983083, 0.180164, 0.0327274, -0.00426907},
00950             {0.828575, -0.564397, 1.17937, 0.990023, 0.137179, 0.0315954, -0.00617472},
00951             {0.870116, -0.550422, 1.17831, 0.995336, 0.0920069, 0.0283872, -0.00586025},
00952             {0.921693, -0.544899, 1.17853, 0.998734, 0.0415909, 0.0273629, -0.00714236},
00953             {0.971471, -0.549669, 1.17854, 0.998732, 0.0416648, 0.0273237, -0.00716123}
00954         };
00955 
00956 
00957         //for (double z= 1.3; z <= 1.4; z +=0.025)
00958         double z = 1.35;
00959         {
00960             int numf = 0;
00961             bool found = true;
00962 
00963             double stA[7];
00964             arm->getJointState(stA);
00965 
00966             RobotHead::getInstance()->lookAt("/map", 1.243111, -0.728864, 0.7);
00967 
00968             for (int k = 0; k <= 16; ++k)
00969             {
00970                 tf::Stamped<tf::Pose> act;
00971                 act.frame_id_ = "map";
00972                 act.setOrigin(btVector3(pts[k][0],pts[k][1],z));
00973                 act.setRotation(btQuaternion(pts[k][3],pts[k][4],pts[k][5],pts[k][6]));
00974                 act = arm->tool2wrist(act);
00975 
00976                 geometry_msgs::PoseStamped stamped_pose;
00977                 stamped_pose.header.frame_id = "map";
00978                 stamped_pose.header.stamp = ros::Time::now();
00979                 stamped_pose.pose.position.x=act.getOrigin().x();
00980                 stamped_pose.pose.position.y=act.getOrigin().y();
00981                 stamped_pose.pose.position.z=z;
00982                 stamped_pose.pose.orientation.x=pts[k][3];
00983                 stamped_pose.pose.orientation.y=pts[k][4];
00984                 stamped_pose.pose.orientation.z=pts[k][5];
00985                 stamped_pose.pose.orientation.w=pts[k][6];
00986 
00987                 double stAs[7];
00988                 int ret = arm->run_ik(stamped_pose,stA,stAs,side == 0 ? "r_wrist_roll_link" : "l_wrist_roll_link");
00989                 ROS_INFO("       z %f k %i %s", z, k, ret ? "found." : "not found.");
00990                 numf += ret;
00991                 if (ret==0)
00992                     found = false;
00993                 else
00994                 {
00995                     for (int i = 0; i < 7; ++i)
00996                         stA[i] = stAs[i];
00997                 }
00998             }
00999             ROS_INFO("FOR HEIGHT z %f %s num %i", z, found ? "found." : "not found.", numf);
01000             if (found)
01001             {
01002 
01003                 double stA[7];
01004                 arm->getJointState(stA);
01005                 ros::Rate rate(3.0);
01006                 int idx[] = {16,15,14,13,12,11,10,9,8,7,6,5,4,3,2,1,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,16,16};
01007                 for (int o = 0; o <= 16+16; ++o)
01008                     //for (int k = 16; k >= 0; --k)
01009                 {
01010                     int k = idx[o];
01011                     tf::Stamped<tf::Pose> act;
01012                     act.frame_id_ = "map";
01013                     act.setOrigin(btVector3(pts[k][0],pts[k][1],z));
01014                     act.setRotation(btQuaternion(pts[k][3],pts[k][4],pts[k][5],pts[k][6]));
01015                     act = arm->tool2wrist(act);
01016 
01017                     geometry_msgs::PoseStamped stamped_pose;
01018                     stamped_pose.header.frame_id = "map";
01019                     stamped_pose.header.stamp = ros::Time::now();
01020                     stamped_pose.pose.position.x=act.getOrigin().x();
01021                     stamped_pose.pose.position.y=act.getOrigin().y();
01022                     stamped_pose.pose.position.z=z;
01023                     stamped_pose.pose.orientation.x=pts[k][3];
01024                     stamped_pose.pose.orientation.y=pts[k][4];
01025                     stamped_pose.pose.orientation.z=pts[k][5];
01026                     stamped_pose.pose.orientation.w=pts[k][6];
01027 
01028                     double stAs[7];
01029                     int ret = arm->run_ik(stamped_pose,stA,stAs,side == 0 ? "r_wrist_roll_link" : "l_wrist_roll_link");
01030 
01031                     //ROS_INFO("       z %f k %i %s", z, k, ret ? "found." : "not found.");
01032                     ROS_INFO("k : %i",k);
01033 
01034                     rate.sleep();
01035                     if (ret)
01036                     {
01037                         double pose[7];
01038                         double sum = 0;
01039                         for (int k = 0; k < 7; ++k)
01040                         {
01041                             pose[k] = stAs[k];
01042                             sum += stAs[k] * stAs[k];
01043                         }
01044                         if (sum > 0.01)
01045                             arm->startTrajectory(arm->goalTraj(pose),false);
01046                     }
01047 
01048                     //this here shall actually get the bottle's pose and grasp it
01049                     if (k == 0)
01050                     {
01051 
01052                         RobotHead::getInstance()->lookAt("/map", 1.181542, -0.763771, 0.967066);
01053 
01054                         tf::Stamped<tf::Pose> bottle = Perception3d::getFridgePlaneCenterPose();
01055                         //bottle.pose.position.z = bottle.getOrigin().z() + 0.1;
01056                         Gripper::getInstance(1)->close();
01057 
01058                         //                        double ptA[] = {0.41491862845470812, 1.3468554401788568, 1.501748997727044, -2.0247783614692936, -16.507431415382143, -1.3292235155277217, 15.027356561279952};
01059                         //                        double ptB[] = {0.040263624618489424, 0.96465557759293075, 0.27150676981727662, -1.6130504582945409, -14.582800985450046, -1.1869058378819473, 14.819427432123987};
01060                         double ptA[] = {0.68376502811441964, 1.2012096482630152, 1.8365364116753793, -2.2751645879302225, -46.069969252840536, -1.5540038123036684, 33.476251846428482};
01061                         double ptB[] = {-0.076488653049139876, 0.79236238489918576, -0.066073603203320896, -1.5513110310125839, -46.176928516612122, -1.1200023343102063, 33.886323418367155};
01062 
01063                         RobotArm *arml = RobotArm::getInstance(1);
01064                         arml->startTrajectory(arml->goalTraj(ptA,1.5));
01065                         arml->startTrajectory(arml->goalTraj(ptB,1.5));
01066                         //Gripper::getInstance(1)->open();
01067                         double offset_above_plane = 0.1;
01068                         RobotArm::getInstance(1)->universal_move_toolframe_ik(bottle.getOrigin().x() - .1, bottle.getOrigin().y(), bottle.getOrigin().z() + offset_above_plane, 0.005, -0.053, -0.029005, 0.998160, "map");
01069                         RobotArm::getInstance(1)->universal_move_toolframe_ik(bottle.getOrigin().x(), bottle.getOrigin().y(), bottle.getOrigin().z() + offset_above_plane, 0.005, -0.053, -0.029005, 0.998160, "map");
01070 
01071                         //Gripper::getInstance(1)->closeCompliant();
01072                         //Gripper::getInstance(1)->close(0.04);
01073                         Gripper::getInstance(1)->open();
01074                         RobotArm::getInstance(1)->universal_move_toolframe_ik(bottle.getOrigin().x() - .1, bottle.getOrigin().y(), bottle.getOrigin().z() + offset_above_plane, 0.005, -0.053, -0.029005, 0.998160, "map");
01075                         arml->startTrajectory(arml->goalTraj(ptB,1.5));
01076                         arml->startTrajectory(arml->goalTraj(ptA,1.5));
01077                         Gripper::getInstance(1)->close();
01078                         RobotArm::getInstance(1)->universal_move_toolframe_ik(0.208, 0.120, 0.785 , -0.020, -0.101, -0.662, 0.742, "base_link");
01079                     }
01080                 }
01081             }
01082         }
01083         Gripper::getInstance(0)->open();
01084 
01085 
01086         double target[4];
01087         target[0] = -0.3;
01088         target[1] = -0.15;
01089         target[2] = 0;
01090         target[3] = 1;
01091         ROS_INFO("POSE IN BASE %f %f %f", target[0],target[1], target[2]);
01092         RobotDriver::getInstance()->driveInOdom(target, 1);
01093 
01094         pr2_controllers_msgs::JointTrajectoryGoal goalAT = RobotArm::getInstance(0)->twoPointTrajectory(Poses::prepDishRT,Poses::prepDishRT);
01095         boost::thread t2T(&RobotArm::startTrajectory, RobotArm::getInstance(0), goalAT,true);
01096         t2T.join();
01097 
01098         //RobotArm::getInstance(1)->universal_move_toolframe_ik(0.308, 0.120, 0.91 , -0.020, -0.101, -0.662, 0.742, "base_link");
01099 
01100         //[1.1811415860679706, 0.40573692064179873, 1.772267589813042, -2.0337541455750725, 0.15224039094452749, -0.61844366806403794, 5.0541238004106495]
01101 
01102     }
01103     return 0;
01104 }
01105 
01106 // XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
01107 
01108 
01109 int DemoScripts::sliceTheBread(int numslices_)
01110 {
01111     double numslices = numslices_;
01112 
01113     Torso::getInstance()->up();
01114 
01115     //double numslices = atoi(argv[2]); // 0 = one slice for computer scientists! -1 = none
01116     double slicethickness = 0.02;
01117 
01118     boost::thread t0(&OperateHandleController::plateTuckPose);
01119 
01120     RobotHead::getInstance()->lookAtThreaded("/slicer",0,0,-0.3);
01121 
01122     tf::Stamped<tf::Pose> newBasePose;
01123     newBasePose.frame_id_ = "/map";
01124     newBasePose.setOrigin(btVector3(-2.979, 1.397, 0.052));
01125     newBasePose.setRotation(btQuaternion(-0.001, 0.000, 0.046, 0.999));
01126 
01127     RobotDriver::getInstance()->driveInMap(newBasePose);
01128 
01129     t0.join();
01130     boost::thread t0b(&OperateHandleController::plateAttackPose);
01131 
01132     newBasePose.setOrigin(btVector3(-2.679, 1.397, 0.052));
01133 
01134     RobotDriver::getInstance()->driveInMap(newBasePose);
01135 
01136     t0b.join();
01137 
01138     //base
01139     //Translation: [-2.679, 1.397, 0.052]
01140     //- Rotation: in Quaternion [-0.001, 0.000, 0.046, 0.999]
01141     // in RPY [-0.002, 0.000, 0.092]
01142     //r_gripper_tool_frame // right of
01143     //- Translation: [-2.106, 1.153, 0.901]
01144     //- Rotation: in Quaternion [0.521, -0.491, -0.510, -0.477]
01145     //  in RPY [3.030, 1.546, -1.619]
01146     // l_gripper_tool_frame = button
01147     //- Translation: [-2.032, 1.389, 1.121]
01148     //- Rotation: in Quaternion [-0.503, 0.487, 0.502, 0.507]
01150 
01151 
01152     tf::Stamped<tf::Pose> leftEdge;
01153     leftEdge.setOrigin(btVector3(-2.032, 1.389, 1.121));
01154     leftEdge.frame_id_ = "map";
01155 
01156     tf::Stamped<tf::Pose> rightEdge;
01157     rightEdge.setOrigin(btVector3(-2.106, 1.153, 0.901));
01158     rightEdge.frame_id_ = "map";
01159 
01160     btVector3 rel = leftEdge.getOrigin() - rightEdge.getOrigin();
01161 
01162     double at2 = atan2(rel.y(), rel.x());
01163 
01164     double analog_synthesizer_tb = .303; // just to make it straight
01165 
01166     btQuaternion ori(btVector3(0,0,1), at2 + analog_synthesizer_tb);
01167 
01168     tf::Stamped<tf::Pose> midEdge;
01169 
01170     midEdge.frame_id_ = "map";
01171 
01172     rightEdge.setRotation(ori);
01173     leftEdge.setRotation(ori);
01174     midEdge.setRotation(ori);
01175 
01176     midEdge.setOrigin((rightEdge.getOrigin() + leftEdge.getOrigin()) * .5f);
01177 
01178     //Translation: [-0.229, 0.029, -0.109]
01179     //- Rotation: in Quaternion [-0.014, 0.692, -0.007, 0.722]
01180     //   in RPY [-0.613, 1.520, -0.603]
01181 
01182     //publish "midedge" as position of the slicer
01183     //rosrun tf static_transform_publisher -2.069000 1.271000 1.011000 0.000000 0.000000 0.706806 0.707407 map slicer 100
01184 
01185     tf::Stamped<tf::Pose> pur; //pick up the bread
01186     pur.setOrigin(btVector3(-0.2, 0.029, -0.109));
01187     pur.setRotation(btQuaternion(-0.014, 0.692, -0.007, 0.722));
01188     pur.frame_id_ = "slicer";
01189 
01190     //Translation: [-0.045, 0.191, -0.079]
01191     //- Rotation: in Quaternion [0.004, 0.720, 0.022, 0.693]
01192     //    in RPY [2.379, 1.518, 2.404]
01193     tf::Stamped<tf::Pose> pre; //preslicing pose
01194     pre.setOrigin(btVector3(-0.005, 0.191, -0.079));
01195     pre.setRotation(btQuaternion(0.004, 0.720, 0.022, 0.693));
01196     pre.frame_id_ = "slicer";
01197 
01198 
01199     //- Translation: [-0.061, -0.017, -0.074]
01200     //- Rotation: in Quaternion [-0.022, 0.738, 0.001, 0.675]
01201     //    in RPY [-2.835, 1.477, -2.802]
01202     tf::Stamped<tf::Pose> post; //after slicing
01203     post.setOrigin(btVector3(-0.005, -0.017, -0.079));
01204     post.setRotation(btQuaternion(-0.022, 0.738, 0.001, 0.675));
01205     post.frame_id_ = "slicer";
01206 
01207 
01208     //- Translation: [0.118, -0.037, 0.110]
01209     //- Rotation: in Quaternion [-0.011, 0.705, 0.000, 0.709]
01210     //    in RPY [-1.261, 1.555, -1.245]
01211     tf::Stamped<tf::Pose> butup;
01212     butup.setOrigin(btVector3(0.12, -0.03, 0.12));
01213     butup.setRotation(btQuaternion(-0.011, 0.705, 0.000, 0.709));
01214     butup.frame_id_ = "slicer";
01215 
01216     tf::Stamped<tf::Pose> prebutup;
01217     prebutup.setOrigin(btVector3(0.18, -0.03, 0.12));
01218     prebutup.setRotation(btQuaternion(-0.011, 0.705, 0.000, 0.709));
01219     prebutup.frame_id_ = "slicer";
01220 
01221     tf::Stamped<tf::Pose> butdown;
01222     //butdown.setOrigin(btVector3(0.118, -0.037, 0.08));
01223     //butdown.setOrigin(btVector3(0.118, -0.037, 0.07));
01224     butdown.setOrigin(btVector3(0.12, -0.03, 0.085));
01225     butdown.setRotation(btQuaternion(-0.011, 0.705, 0.000, 0.709));
01226     butdown.frame_id_ = "slicer";
01227 
01228     RobotArm *rarm = RobotArm::getInstance(0);
01229     Gripper *rgrip = Gripper::getInstance(0);
01230     RobotArm *larm = RobotArm::getInstance(1);
01231     Gripper *lgrip = Gripper::getInstance(1);
01232 
01233     ros::Rate onesec(1);
01234 
01235     boost::thread prebuttona(&RobotArm::move_toolframe_ik_pose, larm, prebutup);
01236 
01237     tf::Stamped<tf::Pose> nextPoseR = pur;
01238     nextPoseR.setOrigin(pur.getOrigin() + btVector3(-numslices * slicethickness,0,0.15));
01239     rarm->universal_move_toolframe_ik_pose(nextPoseR);
01240     rgrip->open();
01241 
01242     nextPoseR.setOrigin(pur.getOrigin() + btVector3(-numslices * slicethickness,0,0));
01243     rarm->universal_move_toolframe_ik_pose(nextPoseR);
01244     rgrip->close(0.04);
01245 
01246     //nextPoseR.setOrigin(pur.getOrigin() + btVector3(-numslices * slicethickness -0.05,0,0.02));
01247     //rarm->universal_move_toolframe_ik_pose(nextPoseR);
01248     prebuttona.join();
01249 
01250     boost::thread t1(&Gripper::close, Gripper::getInstance(1), 0.0);
01251 
01252     boost::thread buttona(&RobotArm::move_toolframe_ik_pose, larm, butup);
01253 
01254     nextPoseR.setOrigin(pur.getOrigin() + btVector3(-numslices * slicethickness -0.05,0,0.05));
01255     rarm->universal_move_toolframe_ik_pose(nextPoseR);
01256 
01257     buttona.join();
01258 
01259     //lgrip->close(); xxx t
01260 
01261     boost::thread button(&RobotArm::move_toolframe_ik_pose, larm, butdown);
01262 
01263     RobotHead::getInstance()->lookAtThreaded("/r_gripper_tool_frame",0,0,0);
01264 
01265     nextPoseR = pre;
01266     nextPoseR.setOrigin(pre.getOrigin() + btVector3(-numslices * slicethickness, 0,0.1));
01267     rarm->universal_move_toolframe_ik_pose(nextPoseR);
01268 
01269     for (double nums = numslices; nums >= -1.0; nums-=1.0)
01270     {
01271 
01272         nextPoseR = pre;
01273         nextPoseR.setOrigin(pre.getOrigin() + btVector3(-nums * slicethickness, 0,0));
01274         rarm->universal_move_toolframe_ik_pose(nextPoseR);
01275 
01276         button.join();
01277 
01278         onesec.sleep();
01279         onesec.sleep();
01280 
01281         nextPoseR = post;
01282         nextPoseR.setOrigin(post.getOrigin() + btVector3(-nums * slicethickness, 0,0));
01283 
01284         RobotArm::getInstance(0)->time_to_target = 2.7;
01285 
01286         rarm->universal_move_toolframe_ik_pose(nextPoseR);
01287 
01288         RobotArm::getInstance(0)->time_to_target = 1;
01289 
01290 
01291         if (nums > -1)
01292         {
01293             nextPoseR = pre;
01294             nextPoseR.setOrigin(pre.getOrigin()  + btVector3(-(nums + 1) * slicethickness, 0,0));
01295             rarm->universal_move_toolframe_ik_pose(nextPoseR);
01296         }
01297     }
01298 
01299     boost::thread t2(&RobotArm::universal_move_toolframe_ik_pose,larm,butup);
01300 
01301     nextPoseR.setOrigin(pur.getOrigin() + btVector3(0,0,0.15));
01302     rarm->universal_move_toolframe_ik_pose(nextPoseR);
01303     nextPoseR.setOrigin(pur.getOrigin() + btVector3(-0.02,0,0.02));
01304     rarm->universal_move_toolframe_ik_pose(nextPoseR);
01305     nextPoseR.setOrigin(pur.getOrigin() + btVector3(0.04,0,0.0));
01306     rarm->universal_move_toolframe_ik_pose(nextPoseR);
01307     rgrip->open();
01308     nextPoseR.setOrigin(pur.getOrigin() + btVector3(0,0,0.15));
01309     rarm->universal_move_toolframe_ik_pose(nextPoseR);
01310 
01311     //OperateHandleController::plateAttackPose();
01312     return 0;
01313 }
01314 
01315 
01316 int DemoScripts::takeBreadPlate(int zee)
01317 {
01318     Pressure::getInstance(0);
01319     Pressure::getInstance(1);
01320 
01321     double zoffs = 0.03;
01322 
01323     RobotHead::getInstance()->lookAtThreaded("/map",-2.15,1.6,0.5);
01324 
01325     //OperateHandleController::plateTuckPose();
01326 
01327     tf::Stamped<tf::Pose> newBasePose;
01328     newBasePose.frame_id_ = "/map";
01329     newBasePose.setOrigin(btVector3(-2.679, 1.397, 0.052));
01330     newBasePose.setRotation(btQuaternion(-0.001, 0.000, 0.046, 0.999));
01331 
01332     RobotDriver::getInstance()->driveInMap(newBasePose);
01333 
01334     OperateHandleController::plateAttackPose();
01335 
01336     tf::Stamped<tf::Pose> start;
01337     start.frame_id_ = "/map";
01338     start.setOrigin(btVector3(-2.144, 1.69, 0.889 + zoffs));
01339     start.setRotation(btQuaternion(-0.270, 0.666, -0.247, 0.650));
01340 
01341     tf::Stamped<tf::Pose> end;
01342     end.frame_id_ = "/map";
01343     end.setOrigin(btVector3(-2.153, 1.573, 0.898 + zoffs));
01344     end.setRotation(btQuaternion(-0.270, 0.666, -0.247, 0.650));
01345 
01346     OperateHandleController::singleSidedPick(1,start,end);
01347 
01348     double offset = -.05;
01349 
01350     tf::Stamped<tf::Pose> larm = RobotArm::getInstance(1)->getToolPose("/map");
01351     btVector3 pos = larm.getOrigin();
01352     pos.setY(2.15);
01353     pos.setX(pos.getX() + offset -.1);
01354     pos.setZ(pos.getZ() - .025);
01355     larm.setOrigin(pos);
01356 
01357     //RobotHead::getInstance()->lookAtThreaded("/map",-2.3,1.9,.5,false);
01358     RobotHead::getInstance()->lookAtThreaded("/l_gripper_tool_frame",0,0,0);
01359 
01360 
01361     RobotArm::getInstance(1)->time_to_target = 3;
01362     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(larm);
01363     RobotArm::getInstance(1)->time_to_target = 1;
01364 
01365 
01366     //Translation: [-2.278, 1.979, 0.868] - Rotation: in Quaternion [-0.671, 0.187, 0.279, 0.661]
01367     larm.setOrigin(btVector3(-2.278 + offset, 1.979, 0.87 + zoffs));
01368     larm.setRotation(btQuaternion(-0.671, 0.187, 0.279, 0.661));
01369 
01370     // [-2.226, 2.135, 0.891]
01371     //- Rotation: in Quaternion [-0.376, 0.550, -0.068, 0.742]
01372 
01373     larm.setOrigin(btVector3(-2.226 + offset, 2.135, 0.87 + zoffs));
01374     larm.setRotation(btQuaternion(-0.376, 0.550, -0.068, 0.742));
01375     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(larm);
01376 
01377 
01378     //Translation: [-2.300, 2.051, 0.892]
01379     //- Rotation: in Quaternion [-0.533, 0.468, 0.116, 0.695]
01380 
01381 
01382     //RobotHead::getInstance()->lookAtThreaded("/map",-2.3,2.05,.5,false);
01383 
01384     larm.setOrigin(btVector3(-2.300 + offset, 2.051, 0.87 + zoffs));
01385     larm.setRotation(btQuaternion(-0.533, 0.468, 0.116, 0.695));
01386     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(larm);
01387 
01388 
01389     //larm.setOrigin(btVector3(-2.375 + offset, 1.939, 0.873));
01390     larm.setOrigin(btVector3(-2.375 + offset + 0.05, 1.939, 0.873 + zoffs));
01391     larm.setRotation(btQuaternion(-0.691, 0.138, 0.192, 0.683));
01392     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(larm);
01393 
01394 
01395     //RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(larm);
01396 
01397     Gripper::getInstance(1)->open();
01398 
01399     //larm.setOrigin(btVector3(-2.37 + offset, 2.051, 0.87));
01400     larm.setOrigin(btVector3(-2.375 + offset - .05, 1.939, 0.873 + zoffs));
01401     larm.setRotation(btQuaternion(-0.691, 0.138, 0.192, 0.683));
01402     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(larm);
01403 
01404     //exit(0);
01405 
01406     RobotHead::getInstance()->lookAtThreaded("/map",-2.26, 1.92,.5);
01407 
01408     OperateHandleController::plateAttackPose();
01409 
01410     //drive to grasp plate pose
01411     //RobotDriver::getInstance()->moveBaseP(-2.844, 1.403,0.055, 0.998);
01412     RobotDriver::getInstance()->moveBaseP(-2.904, 1.95,  0.055, 0.998);
01413 
01414     //OperateHandleController::pickPlate(btVector3(-2.18+ offset, 1.95, 0.865),.26);
01415     //OperateHandleController::pickPlate(btVector3(-2.18 + offset - 0.025, 1.95, 0.865),.26);
01416     OperateHandleController::pickPlate(btVector3(-2.18 + offset - 0.025, 1.9, 0.865 + zoffs),.26);
01417 
01418     RobotHead::getInstance()->lookAtThreaded("/map",-3.260, -1.76,.5);
01419 
01420     tf::Stamped<tf::Pose> basePos;
01421     RobotDriver::getInstance()->getRobotPose(basePos);
01422     basePos.setOrigin(basePos.getOrigin() + btVector3(-.15,0,0));
01423 
01424     RobotDriver::getInstance()->driveInMap(basePos);
01425 
01426     RobotDriver::getInstance()->moveBaseP(-3.290, -1.027, -0.697, 0.717);
01427 
01428     OperateHandleController::spinnerL(0.25,0,-.25);
01429 
01430     OperateHandleController::openGrippers();
01431 
01432     OperateHandleController::plateAttackPose();
01433 
01434     return 0;
01435 }
01436 
01437 
01438 int DemoScripts::takeBowl(int zee)
01439 {
01440     Pressure::getInstance(0);
01441     Pressure::getInstance(1);
01442     OperateHandleController::plateTuckPose();
01443 
01444     double zoffs = 0.03;
01445 
01446     RobotHead::getInstance()->lookAtThreaded("/map",-1.8,1.6,.5);
01447 
01448 
01449     RobotDriver::getInstance()->moveBaseP(-2.785, 2.022,0,1);
01450 
01451     boost::thread t0(&OperateHandleController::plateAttackPose);
01452 
01453     ros::Rate rt(10);
01454     int looper = 50;
01455     while (--looper > 0)
01456         rt.sleep();
01457 
01458 
01459     tf::Stamped<tf::Pose> bowlPose_ = OperateHandleController::getBowlPose();
01460     btVector3 bowlPose = bowlPose_.getOrigin();// = OperateHandleController::getBowlPose();
01461 
01462     //-0.008453 -0.075438 0.013157  -0.687616 -0.104624 0.714747 0.073313
01463 
01464     tf::Stamped<tf::Pose> pur; //pick up the bread
01465     pur.setOrigin(btVector3(-0.008453, -0.075438 ,0.013157  ));
01466     pur.setRotation(btQuaternion(-0.687616, -0.104624, 0.714747, 0.073313));
01467     pur.frame_id_ = "bowly";
01468     btTransform pur_ = bowlPose_ * pur;//tf::Pose btrightGrasp =  midEdge * rRel ;
01469 
01470 
01471     tf::Stamped<tf::Pose> pickup;
01472     pickup.frame_id_ = "map";
01473     pickup.setOrigin(pur_.getOrigin());
01474     pickup.setRotation(pur_.getRotation());
01475 
01476     tf::Stamped<tf::Pose> prepickup;
01477     prepickup.frame_id_ = "map";
01478     prepickup.setOrigin(pur_.getOrigin() + btVector3(0,0,0.05));
01479     prepickup.setRotation(pur_.getRotation());
01480 
01481     t0.join();
01482 
01483     boost::thread t1(&Gripper::open, Gripper::getInstance(0), 0.08);
01484 
01485     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(prepickup);
01486 
01487     t1.join();
01488 
01489     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(pickup);
01490 
01491     Gripper::getInstance(0)->close();
01492 
01493 
01494     /*
01495     if (bowlPose.getZ() < 0.05)
01496     {
01497         ROS_ERROR("SOMETHING WENT WRONG WITH BOWL DETECTION");
01498         bowlPose = btVector3(-1.87,1.85,.909);
01499     }
01500 
01501     bowlPose.setZ(.909 + zoffs);
01502 
01503     tf::Stamped<tf::Pose> start;
01504     start.frame_id_ = "/map";
01505     //start.setOrigin(btVector3(-2.215, 1.955, 0.909));
01506     start.setOrigin(bowlPose - btVector3(.12,0,0.01));
01507     start.setRotation(btQuaternion(-0.631, 0.304, 0.379, 0.605));
01508 
01509     tf::Stamped<tf::Pose> end;
01510     end.frame_id_ = "/map";
01511     //end.setOrigin(btVector3(-2.07, 1.955, 0.909));
01512     end.setOrigin(bowlPose - btVector3(0.05,0,0.01));
01513     end.setRotation(btQuaternion(-0.631, 0.304, 0.379, 0.605));
01514 
01515     //bool RobotArm::findBaseMovement(btVector3 &result, std::vector<int> arm, std::vector<tf::Stamped<tf::Pose> > goal, bool drive, bool reach)
01516 
01517     std::vector<int> arm;
01518     std::vector<tf::Stamped<tf::Pose> > goal;
01519     btVector3 result;
01520 
01521     arm.push_back(0);
01522     goal.push_back(end);
01523     arm.push_back(0);
01524     goal.push_back(start);
01525 
01526     RobotArm::findBaseMovement(result, arm, goal,true, false);
01527 
01528     // XXXXXXXXXXXXXXXXXXX
01529 
01530 
01531     OperateHandleController::singleSidedPick(0,start,end);
01532     //- Translation: [-2.130, 1.967, 0.883]     -  Rotation: in Quaternion [-0.505, 0.472, 0.540, 0.480]
01533 
01534     //[-2.215, 1.955, 0.909] Rotation: in Quaternion [-2.215, 1.955, 0.909]
01535 
01536     tf::Stamped<tf::Pose> pitch;
01537     pitch = RobotArm::getInstance(0)->getToolPose("/map");
01538     pitch.setRotation(btQuaternion(-0.615489,0.343613,0.456079,0.543226));
01539 
01540     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(pitch);
01541 
01542     Gripper::getInstance(0)->close();
01543 
01544     //btTransform bpt = bowlPose;
01545     //btTransform hpt = pitch;
01546     btTransform rel = bowlPose_.inverseTimes(pitch);//bpt.inverseTimes(pitch);
01547 
01548     ROS_INFO("RELATIVE POSE AFTER ADJUSTMENT : %f %f %f  %f %f %f %f", rel.getOrigin().x(), rel.getOrigin().y(), rel.getOrigin().z(),
01549              rel.getRotation().x(), rel.getRotation().y(), rel.getRotation().z(), rel.getRotation().w());
01550 
01551              */
01552 
01553     RobotHead::getInstance()->stopThread();
01554 
01555     tf::Stamped<tf::Pose> carry;
01556     carry.frame_id_ = "/base_link";
01557     carry.setOrigin(btVector3(0.347, -0.045, 0.986));
01558     carry.setRotation(btQuaternion(-0.610, -0.094, 0.737, 0.276));
01559     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(carry);
01560 
01561     Gripper::getInstance(0)->close();
01562 
01563 
01564     RobotHead::getInstance()->lookAtThreaded("/map",-3.085, -1.792,.5);
01565 
01566     RobotDriver::getInstance()->moveBaseP(-2.981, 2.031, 0.002, 1.000);
01567 
01568     OperateHandleController::plateTuckPoseLeft();
01569 
01570     RobotDriver::getInstance()->moveBaseP(-3.180, -1.117,  -0.725, 0.689);
01571 
01572     //RobotArm::getInstance(0)->universal_move_toolframe_ik(-3.085, -1.792, 0.768, -0.371, 0.651, 0.132, 0.649,"/map");
01573     RobotArm::getInstance(0)->universal_move_toolframe_ik(-3.643, -1.772, 0.815, -0.371, 0.651, 0.132, 0.649,"/map");
01574 
01575     Gripper::getInstance(0)->open();
01576 
01577     OperateHandleController::plateAttackPose();
01578     OperateHandleController::plateTuckPose();
01579 
01580     RobotHead::getInstance()->stopThread();
01581 
01582     return 0;
01583 
01584 }
01585 
01586 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


ias_drawer_executive
Author(s): Thomas Ruehr
autogenerated on Thu May 23 2013 09:44:56