$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 #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