$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 #include <ias_drawer_executive/OperateHandleController.h> 00031 #include <ias_drawer_executive/RobotArm.h> 00032 #include <ias_drawer_executive/Geometry.h> 00033 #include <ias_drawer_executive/Gripper.h> 00034 #include <ias_drawer_executive/Perception3d.h> 00035 #include <ias_drawer_executive/Pressure.h> 00036 #include <ias_drawer_executive/Poses.h> 00037 #include <ias_drawer_executive/AverageTF.h> 00038 #include <ias_drawer_executive/Torso.h> 00039 #include <ias_drawer_executive/Head.h> 00040 #include <ias_drawer_executive/Approach.h> 00041 00042 #include <cop_client_cpp/cop_client.h> 00043 #include <geometry_msgs/PoseStamped.h> 00044 00045 #include <boost/thread.hpp> 00046 #include "boost/date_time/posix_time/posix_time.hpp" 00047 00048 00049 00050 tf::Stamped<tf::Pose> OperateHandleController::getCopPose(const char name[], const char frame[]) 00051 { 00052 ros::NodeHandle nh; 00053 CopClient cop(nh); 00054 ros::Rate r(100); 00055 00056 ROS_INFO("GET %s POSE in %s", name, frame); 00057 00058 unsigned long id_searchspace = cop.LONameQuery(frame); 00059 long vision_primitive = cop.CallCop(name, id_searchspace); 00060 00061 size_t num_results_expected = 1; 00062 00063 std::vector<vision_msgs::cop_answer> results; 00064 bool found = false; 00065 00066 ros::Time start = ros::Time::now(); 00067 00068 while (nh.ok() && !found && (ros::Time::now() - start < ros::Duration(2))) 00069 { 00070 if (cop.HasResult(vision_primitive) >= num_results_expected) 00071 found = true; 00072 ros::spinOnce(); 00073 r.sleep(); 00074 } 00075 00076 tf::Stamped<tf::Pose> bowlPose; 00077 00078 00079 if (!found) { 00080 ROS_ERROR("Object not found"); 00081 return bowlPose; 00082 } 00083 00084 results = cop.GetResult(vision_primitive); 00085 // if (results[0].error.length() > 0) 00086 // return btVector3(0,0,0); 00087 // double vec[] = {0,0,0}; 00088 if (results[0].error.length() == 0) 00089 { 00090 unsigned long frame = cop.LOFrameQuery(results[0].found_poses[0].position,1); 00091 00092 //cop.LOPointQuery(frame,vec); 00093 bowlPose = cop.LOPoseQuery(frame); 00094 } 00095 00096 btTransform adj; 00097 //adj.setOrigin(btVector3(-0.022,-0.022,0)); 00098 00099 btTransform bl_; 00100 bl_.setRotation(bowlPose.getRotation()); 00101 bl_.setOrigin(bowlPose.getOrigin()); 00102 00103 bl_ = bl_ * adj; 00104 bowlPose.setOrigin(bl_.getOrigin()); 00105 00106 return bowlPose; 00107 } 00108 00109 00110 00111 tf::Stamped<tf::Pose> OperateHandleController::getBowlPose() 00112 { 00113 ros::NodeHandle nh; 00114 CopClient cop(nh); 00115 ros::Rate r(100); 00116 00117 ROS_INFO("GET PLATE POSE"); 00118 00119 unsigned long id_searchspace = cop.LONameQuery("/openni_rgb_optical_frame"); 00120 long vision_primitive = cop.CallCop("Bowl2", id_searchspace); 00121 00122 size_t num_results_expected = 1; 00123 00124 std::vector<vision_msgs::cop_answer> results; 00125 bool found = false; 00126 while (nh.ok() && !found) 00127 { 00128 if (cop.HasResult(vision_primitive) >= num_results_expected) 00129 found = true; 00130 ros::spinOnce(); 00131 r.sleep(); 00132 } 00133 00134 tf::Stamped<tf::Pose> bowlPose; 00135 00136 results = cop.GetResult(vision_primitive); 00137 // if (results[0].error.length() > 0) 00138 // return btVector3(0,0,0); 00139 //double vec[] = {0,0,0}; 00140 if (results[0].error.length() == 0) 00141 { 00142 unsigned long frame = cop.LOFrameQuery(results[0].found_poses[0].position,1); 00143 00144 //cop.LOPointQuery(frame,vec); 00145 bowlPose = cop.LOPoseQuery(frame); 00146 } 00147 00148 btTransform adj; 00149 adj.setOrigin(btVector3(-0.022,-0.022,0)); 00150 00151 btTransform bl_; 00152 bl_.setRotation(bowlPose.getRotation()); 00153 bl_.setOrigin(bowlPose.getOrigin()); 00154 00155 bl_ = bl_ * adj; 00156 bowlPose.setOrigin(bl_.getOrigin()); 00157 00158 00159 00160 //ROS_INFO("PLATE POSE IN MAP %f %f %f", vec[0],vec[1],vec[2]); 00161 00162 //tf::Stamped<tf::Pose> pMb; 00163 //pMb.setOrigin(btVector3(vec[0],vec[1],vec[2])); 00164 //pMb.frame_id_ = "map"; 00165 00166 00167 //pMb = RobotArm::getInstance(0)->getPoseIn("base_link",pMb); 00168 //ROS_INFO("PLATE POSE IN BASE_LINK %f %f %f", pMb.getOrigin().x(), pMb.getOrigin().y(), pMb.getOrigin().z()); 00169 00170 //btVector3 aPose = pMb.getOrigin(); 00171 //ROS_INFO("aPose %f %f %f ", pMb.getOrigin().x(), pMb.getOrigin().y(),pMb.getOrigin().z()); 00172 00173 //return btVector3(pMb.getOrigin().x(), pMb.getOrigin().y(), pMb.getOrigin().z()); 00174 00175 return bowlPose; 00176 } 00177 00178 00179 btVector3 OperateHandleController::getPlatePose() 00180 { 00181 ros::NodeHandle nh; 00182 CopClient cop(nh); 00183 ros::Rate r(100); 00184 00185 ROS_INFO("GET PLATE POSE"); 00186 00187 unsigned long id_searchspace = cop.LONameQuery("/narrow_stereo_optical_frame"); 00188 long vision_primitive = cop.CallCop("RoundPlate", id_searchspace); 00189 00190 size_t num_results_expected = 1; 00191 00192 std::vector<vision_msgs::cop_answer> results; 00193 bool found = false; 00194 while (nh.ok() && !found) 00195 { 00196 if (cop.HasResult(vision_primitive) >= num_results_expected) 00197 found = true; 00198 ros::spinOnce(); 00199 r.sleep(); 00200 } 00201 00202 results = cop.GetResult(vision_primitive); 00203 // if (results[0].error.length() > 0) 00204 // return btVector3(0,0,0); 00205 float vec[] = {0,0,0}; 00206 if (results[0].error.length() == 0) 00207 { 00208 unsigned long frame = cop.LOFrameQuery(results[0].found_poses[0].position,1); 00209 00210 cop.LOPointQuery(frame,vec); 00211 } 00212 00213 ROS_INFO("PLATE POSE IN MAP %f %f %f", vec[0],vec[1],vec[2]); 00214 00215 tf::Stamped<tf::Pose> pMb; 00216 pMb.setOrigin(btVector3(vec[0],vec[1],vec[2])); 00217 pMb.frame_id_ = "map"; 00218 //pMb = RobotArm::getInstance(0)->getPoseIn("base_link",pMb); 00219 //ROS_INFO("PLATE POSE IN BASE_LINK %f %f %f", pMb.getOrigin().x(), pMb.getOrigin().y(), pMb.getOrigin().z()); 00220 btVector3 fakePose(0.438186, 1.079624 ,0.759398); 00221 btVector3 aPose = pMb.getOrigin(); 00222 ROS_INFO("aPose %f %f %f ", pMb.getOrigin().x(), pMb.getOrigin().y(),pMb.getOrigin().z()); 00223 ROS_INFO("fakePose %f %f %f ", fakePose.x(), fakePose.y(),fakePose.z()); 00224 ROS_INFO("DIST %f", (fakePose - aPose).length() ); 00225 if ((fakePose - aPose).length() > 0.5) 00226 { 00227 ROS_ERROR("------------------------------------------------------"); 00228 ROS_ERROR("SOMETHING WENT WRONG WITH GETTING A POSE FOR THE PLATE"); 00229 ROS_ERROR("------------------------------------------------------"); 00230 pMb.setOrigin(fakePose); 00231 } 00232 00233 return btVector3(pMb.getOrigin().x(), pMb.getOrigin().y(), pMb.getOrigin().z()); 00234 } 00235 00236 btVector3 OperateHandleController::getTabletPose() 00237 { 00238 ros::NodeHandle nh; 00239 CopClient cop(nh); 00240 ros::Rate r(100); 00241 00242 unsigned long id_searchspace = cop.LONameQuery("/wide_stereo_optical_frame"); 00243 long vision_primitive = cop.CallCop("tablet", id_searchspace); 00244 00245 size_t num_results_expected = 1; 00246 00247 std::vector<vision_msgs::cop_answer> results; 00248 while (nh.ok()) 00249 { 00250 if (cop.HasResult(vision_primitive) >= num_results_expected) 00251 break; 00252 ros::spinOnce(); 00253 r.sleep(); 00254 } 00255 00256 results = cop.GetResult(vision_primitive); 00257 if (results[0].error.length() > 0) 00258 return btVector3(0,0,0); 00259 00260 unsigned long frame = cop.LOFrameQuery(results[0].found_poses[0].position,1); 00261 float vec[] = {0,0,0}; 00262 cop.LOPointQuery(frame,vec); 00263 00264 ROS_INFO("TABLET POSE IN MAP %f %f %f", vec[0],vec[1],vec[2]); 00265 00266 tf::Stamped<tf::Pose> pMb; 00267 pMb.setOrigin(btVector3(vec[0],vec[1],vec[2])); 00268 pMb.frame_id_ = "map"; 00269 pMb = Geometry::getPoseIn("base_link",pMb); 00270 ROS_INFO("TABLET POSE IN BASE_LINK %f %f %f", pMb.getOrigin().x(), pMb.getOrigin().y(), pMb.getOrigin().z()); 00271 00272 return btVector3(pMb.getOrigin().x(), pMb.getOrigin().y(), pMb.getOrigin().z()); 00273 } 00274 00275 00276 int OperateHandleController::maxHandle = -1; 00277 00278 //static std::vector<std::vector<double> > openingTrajectories; 00279 std::vector<std::vector< tf::Stamped<tf::Pose> * > > OperateHandleController::openingTraj; 00280 00281 00282 #include <stdio.h> 00283 #include <stdlib.h> 00284 00285 00286 tf::Stamped<tf::Pose> OperateHandleController::getHandlePoseFromMarker(int arm_, int pos) 00287 { 00288 00289 int side_ = arm_; 00290 00291 Gripper *gripper = Gripper::getInstance(side_); 00292 00293 gripper->open(0.3); 00294 00295 RobotArm *arm = RobotArm::getInstance(side_); 00296 00297 if (arm->isTucked()) arm->startTrajectory(arm->twoPointTrajectory(Poses::untuckPoseA, Poses::untuckPoseB)); 00298 00299 Pressure::getInstance(side_)->reset(); 00300 00301 if (arm_ == 1) 00302 { 00303 RobotArm::getInstance(0)->twoPointTrajectory(Poses::tuckPoseForLeft, Poses::tuckPoseForLeft); 00304 } 00305 00306 // Start the trajectory 00307 if (pos == 0) arm->startTrajectory(arm->twoPointTrajectory(Poses::lowPoseA,Poses::lowPoseB)); 00308 if (pos == 1) arm->startTrajectory(arm->twoPointTrajectory(Poses::midPoseA,Poses::midPoseB)); 00309 if (pos == 2) arm->startTrajectory(arm->twoPointTrajectory(Poses::highPoseA,Poses::highPoseB)); 00310 if (pos == 3) arm->startTrajectory(arm->twoPointTrajectory(Poses::milehighPoseA,Poses::milehighPoseB)); 00311 if (pos == 4) arm->startTrajectory(arm->twoPointTrajectory(Poses::leftHighA,Poses::leftHighB)); 00312 00313 00314 // Wait for trajectory completion 00315 while (!arm->getState().isDone() && ros::ok()) 00316 { 00317 usleep(500); 00318 } 00319 00320 tf::Stamped<tf::Pose> aM = AverageTF::getMarkerTransform("/4x4_1",20); 00321 00322 if (pos == 0) arm->startTrajectory(arm->twoPointTrajectory(Poses::lowPoseB,Poses::lowPoseA)); 00323 if (pos == 1) arm->startTrajectory(arm->twoPointTrajectory(Poses::midPoseB,Poses::midPoseA)); 00324 if (pos == 2) arm->startTrajectory(arm->twoPointTrajectory(Poses::highPoseB,Poses::highPoseA)); 00325 if (pos == 3) arm->startTrajectory(arm->twoPointTrajectory(Poses::milehighPoseB,Poses::milehighPoseA)); 00326 if (pos == 4) arm->startTrajectory(arm->twoPointTrajectory(Poses::leftHighB,Poses::leftHighA)); 00327 00328 // Wait for trajectory completion 00329 while (!arm->getState().isDone() && ros::ok()) 00330 { 00331 usleep(500); 00332 } 00333 00334 ROS_INFO("AVERAGED MARKER POSa %f %f %f ROT %f %f %f %f", aM.getOrigin().x(),aM.getOrigin().y(),aM.getOrigin().z(),aM.getRotation().x(),aM.getRotation().y(),aM.getRotation().z(),aM.getRotation().w()); 00335 00336 ROS_INFO("AVERAGED MARKER POSb %f %f %f ROT %f %f %f %f", aM.getOrigin().x(),aM.getOrigin().y(),aM.getOrigin().z(),aM.getRotation().x(),aM.getRotation().y(),aM.getRotation().z(),aM.getRotation().w()); 00337 00338 tf::Stamped<tf::Pose> ret; 00339 ret.stamp_ = ros::Time::now(); 00340 ret.frame_id_ = aM.frame_id_; 00341 ret.setOrigin(aM.getOrigin()); 00342 ret.setRotation(aM.getRotation()); 00343 00344 return ret; 00345 } 00346 00347 btTransform scaleTransform(const btTransform &in, double scale) 00348 { 00349 btTransform ret; 00350 ret.setRotation(btQuaternion::getIdentity().slerp(in.getRotation(), scale)); 00351 ret.setOrigin(in.getOrigin() * scale); 00352 return ret; 00353 } 00354 00355 void printTransform(const char title[], btTransform a) 00356 { 00357 ROS_INFO("%s : %f %f %f %f %f %f %f", title, a.getOrigin().x(), a.getOrigin().y(), a.getOrigin().z(), 00358 a.getRotation().x(), a.getRotation().y(), a.getRotation().z(), a.getRotation().w()); 00359 } 00360 00361 00362 int OperateHandleController::graspHandle(int arm_, tf::Stamped<tf::Pose> aM) 00363 { 00364 boost::thread(&RobotHead::lookAt,RobotHead::getInstance(),aM.frame_id_, aM.getOrigin().x(), aM.getOrigin().y(), aM.getOrigin().z(),false); 00365 int side_ = arm_; 00366 RobotArm *arm = RobotArm::getInstance(side_); 00367 00368 Gripper *gripper = Gripper::getInstance(side_); 00369 00370 tf::Stamped<tf::Pose> p = Geometry::getPoseIn("base_link",aM); 00371 p.setOrigin(p.getOrigin() + btVector3(-0.05,0,0)); 00372 p = Geometry::getPoseIn("map",p); 00373 00374 gripper->close(); 00375 00376 //arm->excludeBaseProjectionFromWorkspace = true; 00377 00378 arm->universal_move_toolframe_ik_pose(p); 00379 00380 gripper->open(); 00381 arm->universal_move_toolframe_ik_pose(aM); 00382 00383 gripper->close(); 00384 00385 return 0; 00386 /* 00387 00388 gripper->close(); 00389 00390 char fixed_frame[] = "map"; 00391 00392 tf::Stamped<tf::Pose> p; 00393 p.setOrigin(aM.getOrigin()); 00394 p.setRotation(aM.getRotation()); 00395 p.frame_id_ = "base_link"; 00396 00397 tf::Stamped<tf::Pose> pMa = p; 00398 pMa.frame_id_ = "base_link"; 00399 pMa.setOrigin(pMa.getOrigin() + btVector3(-.03,0,0)); 00400 pMa = arm->getPoseIn(fixed_frame,pMa); 00401 00402 tf::Stamped<tf::Pose> pMb = p; 00403 //pMb.setOrigin(pMb.getOrigin() + btVector3(1,0,0)); 00404 pMb.setOrigin(pMb.getOrigin() + btVector3(.08,0,0)); 00405 pMb.frame_id_ = "base_link"; 00406 pMb = arm->getPoseIn(fixed_frame,pMb); 00407 btVector3 diff = pMb.getOrigin() - pMa.getOrigin(); // we define a line (pMa, diff) in map frame here that is used for the approach 00408 00409 p.frame_id_ = "map"; 00410 p.setRotation(pMa.getRotation()); 00411 00412 gripper->updatePressureZero(); 00413 00414 // approach xxx 00415 ROS_INFO("APPROACH"); 00416 00417 std::vector<int> armv; 00418 std::vector<tf::Stamped<tf::Pose> > goal; 00419 btVector3 result; 00420 armv.push_back(side_); 00421 goal.push_back(pMa); 00422 armv.push_back(side_); 00423 goal.push_back(pMb); 00424 00425 RobotArm::findBaseMovement(result, armv, goal, true, false); 00426 00427 Approach *apr = new Approach(); 00428 apr->init(side_,pMa, pMb, Approach::front); 00429 00430 apr->move_to(-.3); 00431 00432 gripper->close(); 00433 00434 double distA = (apr->increment(0,0.5)); 00435 if (distA == 0) 00436 { 00437 ROS_ERROR("DIDNT TOUCH IN THE FIRST 5 CM OF APPROACH"); 00438 distA = (apr->increment(0.5,1)); 00439 } 00440 00441 RobotArm::getInstance(arm_)->time_to_target = 0; 00442 00443 //back up 1 centimeter 00444 apr->move_to(((distA - .02) / .1)); 00445 gripper->open(); 00446 //go 2.75 cm forward from touch position 00447 apr->move_to(((distA + .0275) / .1)); 00448 00449 tf::Stamped<tf::Pose> actPose; 00450 00451 gripper->closeCompliant(); 00452 00453 gripper->close(); 00454 00455 RobotArm::getInstance(arm_)->time_to_target = 0; 00456 00457 arm->excludeBaseProjectionFromWorkspace = false;*/ 00458 } 00459 00460 int OperateHandleController::operateHandle(int arm_, tf::Stamped<tf::Pose> aM, int numretry) 00461 { 00462 //boost::thread(&RobotHead::lookAt,RobotHead::getInstance(),aM.frame_id_, aM.getOrigin().x(), aM.getOrigin().y(), aM.getOrigin().z(),false); 00463 00464 RobotHead::getInstance()->lookAtThreaded("/r_gripper_tool_frame", 0 , 0, 0); 00465 00466 int side_ = arm_; 00467 00468 int handle = ++maxHandle; 00469 00470 std::vector<tf::Stamped<tf::Pose> * > openingTrajAct; 00471 openingTraj.push_back (openingTrajAct); 00472 00473 Gripper *gripper = Gripper::getInstance(side_); 00474 00475 gripper->open(); 00476 00477 RobotArm *arm = RobotArm::getInstance(side_); 00478 00479 //arm->excludeBaseProjectionFromWorkspace = true; 00480 00481 char fixed_frame[] = "map"; 00482 00483 tf::Stamped<tf::Pose> p; 00484 p.setOrigin(aM.getOrigin()); 00485 p.setRotation(aM.getRotation()); 00486 p.frame_id_ = "base_link"; 00487 00488 tf::Stamped<tf::Pose> pMa = p; 00489 pMa.frame_id_ = "base_link"; 00490 pMa.setOrigin(pMa.getOrigin() + btVector3(-.05,0,0)); 00491 pMa = Geometry::getPoseIn(fixed_frame,pMa); 00492 00493 tf::Stamped<tf::Pose> pMb = p; 00494 //pMb.setOrigin(pMb.getOrigin() + btVector3(1,0,0)); 00495 pMb.setOrigin(pMb.getOrigin() + btVector3(.05,0,0)); 00496 pMb.frame_id_ = "base_link"; 00497 pMb = Geometry::getPoseIn(fixed_frame,pMb); 00498 btVector3 diff = pMb.getOrigin() - pMa.getOrigin(); // we define a line (pMa, diff) in map frame here that is used for the approach 00499 00500 p.frame_id_ = "map"; 00501 p.setRotation(pMa.getRotation()); 00502 00503 gripper->updatePressureZero(); 00504 00505 // approach xxx 00506 ROS_INFO("APPROACH"); 00507 00508 std::vector<int> armv; 00509 std::vector<tf::Stamped<tf::Pose> > goal; 00510 //btVector3 result; 00511 tf::Stamped<tf::Pose> result; 00512 armv.push_back(side_); 00513 goal.push_back(pMa); 00514 armv.push_back(side_); 00515 goal.push_back(pMb); 00516 00517 RobotArm::findBaseMovement(result, armv, goal, true, false); 00518 00519 Approach *apr = new Approach(); 00520 apr->init(side_,pMa, pMb, Approach::front); 00521 00522 apr->move_to(-.3); 00523 00524 gripper->close(); 00525 00526 double distA = (apr->increment(0,0.5)); 00527 if (distA == 0) 00528 { 00529 ROS_ERROR("DIDNT TOUCH IN THE FIRST 5 CM OF APPROACH"); 00530 distA = (apr->increment(0.5,1)); 00531 } 00532 //back up 5 centimeter 00533 apr->move_to(((distA - .05) / .1)); 00534 gripper->open(); 00535 //go 2.75 cm forward from touch position 00536 apr->move_to(((distA + .0275) / .1)); 00537 00538 tf::Stamped<tf::Pose> actPose; 00539 00540 gripper->closeCompliant(); 00541 00542 gripper->close(); 00543 00544 arm->stabilize_grip(); 00545 00546 tf::Stamped<tf::Pose> aMp; 00547 aMp.frame_id_ = "base_link"; 00548 arm->getToolPose(aMp,"base_link"); 00549 00550 tf::Stamped<tf::Pose> desiredPose = aMp; 00551 desiredPose.setOrigin(btVector3(aMp.getOrigin().x(),aMp.getOrigin().y(),aMp.getOrigin().z())); 00552 desiredPose = Geometry::getPoseIn(fixed_frame,desiredPose); 00553 00554 tf::Stamped<tf::Pose> startPose = aMp; 00555 startPose.setOrigin(btVector3(aMp.getOrigin().x() + .05,aMp.getOrigin().y(),aMp.getOrigin().z())); 00556 startPose = Geometry::getPoseIn(fixed_frame,startPose); 00557 00558 tf::Stamped<tf::Pose> nextPose = desiredPose; 00559 00560 double dx, dy, dz; 00561 double maxK = 18;//8; 00562 double lastlength = 0; 00563 //int firstbad = 2; 00564 00565 double gripOpen = gripper->getAmountOpen(); 00566 00567 bool slippedEarly = false; 00568 00569 //boost::thread(&RobotHead::lookAt,RobotHead::getInstance(),"/map", actPose.getOrigin().x(),actPose.getOrigin().y(),actPose.getOrigin().z(),false); 00570 00571 tf::Stamped<tf::Pose> *pushPose = new tf::Stamped<tf::Pose> (startPose); 00572 openingTraj[handle].push_back(pushPose); 00573 for (double k = 2; k <= maxK; k++) 00574 { 00575 00576 using namespace boost::posix_time; 00577 time_duration td = milliseconds(5); //00:00:00.001 00578 boost::this_thread::sleep(td); 00579 00580 tf::Stamped<tf::Pose> actPose; 00581 arm->getToolPose(actPose,fixed_frame); 00582 tf::Stamped<tf::Pose> *pushPose = new tf::Stamped<tf::Pose> (actPose); 00583 openingTraj[handle].push_back(pushPose); 00584 ROS_INFO_STREAM("rosrun ias_drawer_executive ias_drawer_executive -3 0 " << actPose.getOrigin().x() << " " << actPose.getOrigin().y() << " " << actPose.getOrigin().z() 00585 << " " << actPose.getRotation().x() << " " << actPose.getRotation().y() << " " << actPose.getRotation().z() << " " << actPose.getRotation().w()); 00586 ROS_INFO_STREAM("ACTUAL POSE k " << k << " " << actPose.getOrigin().x() << " " << actPose.getOrigin().y() << " " << actPose.getOrigin().z()) ; 00587 ROS_INFO_STREAM("DESIRED POSE " << desiredPose.getOrigin().x() << " " << desiredPose.getOrigin().y() << " " << desiredPose.getOrigin().z()) ; 00588 ROS_INFO_STREAM("START POSE " << startPose.getOrigin().x() << " " << startPose.getOrigin().y() << " " << startPose.getOrigin().z()) ; 00589 00590 //aim head at hand 00591 00592 dx = actPose.getOrigin().x() - startPose.getOrigin().x(); 00593 dy = actPose.getOrigin().y() - startPose.getOrigin().y(); 00594 dz = actPose.getOrigin().z() - startPose.getOrigin().z(); 00595 double length = sqrtf(dx * dx + dy * dy + dz * dz); 00596 ROS_INFO("lengt %f hdiff %f", length ,length - lastlength); 00597 /*if (fabs(length - lastlength) < 0.02) 00598 { 00599 ROS_INFO("no movement any more, get out of loop %f %i", k, firstbad); 00600 if (k - firstbad > 4) 00601 { 00602 ROS_ERROR("no movement any more, get out of loop %f %i", k, firstbad); 00603 break; 00604 } 00605 } 00606 else 00607 { 00608 firstbad = k; 00609 }*/ 00610 00611 //detect slippage via gripper opening 00612 ROS_INFO("GRIPPER OPENING %f diff %f ", gripper->getAmountOpen(), gripOpen - gripper->getAmountOpen()); 00613 //if (gripOpen - gripper->getAmountOpen() > 0.005) 00614 if (gripper->getAmountOpen() < 0.005) 00615 { 00616 ROS_ERROR("SLIPPED OFF at opening %f", length); 00617 if (length < 0.2) 00618 { 00619 slippedEarly = true; 00620 ROS_ERROR("SLIPPED EARLY, RETRY"); 00621 } 00622 break; 00623 } 00624 lastlength = length; 00625 00626 //calculate next target pose 00627 ROS_ERROR("D x %f y %f z %f length %f", dx , dy ,dz, length); 00628 dx *= 0.05 / length; 00629 dy *= 0.05 / length; 00630 dz *= 0.05 / length; 00631 double x = startPose.getOrigin().x() + dx * k; 00632 double y = startPose.getOrigin().y() + dy * k; 00633 double z = startPose.getOrigin().z() + dz * k; 00634 ROS_INFO("D %f %f %f NEXT POSE %f %f %f" ,dx,dy,dz, x,y,z); 00635 00636 nextPose.setOrigin(btVector3(x,y,z)); 00637 nextPose.setRotation(actPose.getRotation()); 00638 00639 // move to next pose 00640 if (openingTraj[handle].size() > 2) 00641 { 00642 size_t curr = openingTraj[handle].size() - 1; 00643 btTransform t_0, t_1, t_2; 00644 t_0.setOrigin(openingTraj[handle][curr -1]->getOrigin()); 00645 t_0.setRotation(openingTraj[handle][curr -1]->getRotation()); 00646 t_1.setOrigin(openingTraj[handle][curr -0]->getOrigin()); 00647 t_1.setRotation(openingTraj[handle][curr -0]->getRotation()); 00648 t_2 = t_1 * (t_0.inverseTimes(t_1)); 00649 nextPose.setOrigin(t_2.getOrigin()); 00650 nextPose.setRotation(t_2.getRotation()); 00651 00652 tf::Stamped<tf::Pose> start = *openingTraj[handle][1]; // is this the first nice one ? 00653 tf::Stamped<tf::Pose> last = *openingTraj[handle][curr]; 00654 //btTransform start_t = start.get nextPose. 00655 btTransform rel = start.inverseTimes(last); 00656 00657 double length = rel.getOrigin().length(); 00658 ROS_INFO("CURRENT distance travelled : @@@@@@@@@@ %f", length); 00659 if (length > 0.0001) 00660 { 00661 rel = scaleTransform(rel, (length + 0.05) / length); 00662 btTransform nxt = start * rel; 00663 printTransform("relative transform normalized to a 5cm more: ", rel); 00664 nextPose.setOrigin(nxt.getOrigin()); 00665 nextPose.setRotation(nxt.getRotation()); 00666 printTransform("next pose a 5cm more: ", nextPose); 00667 } 00668 } 00669 00670 btVector3 reply = arm->universal_move_toolframe_ik_pose(nextPose); 00671 00672 //end of trajectory detection via cartesian error 00673 tf::Stamped<tf::Pose> actPose1; 00674 arm->getToolPose(actPose1,fixed_frame); 00675 00676 btVector3 carterr = arm->cartError(); 00677 ROS_ERROR("cartesian error JNT %f XXX DIFF %f %f %f DISTANCE %f", arm->time_to_target, carterr.x(),carterr.y(), carterr.y(), carterr.length()); 00678 if (carterr.length() > 0.035) 00679 { 00680 ROS_ERROR("SLIPPED OFF: CART ERROR JOINT "); 00681 break; 00682 } 00683 00684 btVector3 carterr1 = (actPose1.getOrigin() - nextPose.getOrigin()); 00685 ROS_ERROR("cartesian error REL %f XXX DIFF %f %f %f DISTANCE %f", arm->time_to_target, carterr1.x(),carterr1.y(), carterr1.y(), carterr1.length()); 00686 if (carterr1.length() > 0.035) 00687 { 00688 ROS_ERROR("SLIPPED OFF: CART ERROR POSE DIFF"); 00689 break; 00690 } 00691 00692 arm->stabilize_grip(); 00693 } 00694 00695 arm->excludeBaseProjectionFromWorkspace = false; 00696 00697 if (slippedEarly) 00698 { 00699 if (numretry < 3) 00700 return OperateHandleController::operateHandle(arm_, aM, ++numretry); 00701 else return -1; 00702 } 00703 else 00704 { 00705 00706 arm->getToolPose(actPose,fixed_frame); 00707 arm->universal_move_toolframe_ik_pose(actPose); 00708 00709 gripper->open(); 00710 00711 for (int k = openingTraj[handle].size() - 1; k >= 0 ; k--) 00712 { 00713 tf::Stamped<tf::Pose> *actPose = openingTraj[handle][k]; 00714 ROS_INFO_STREAM("rosrun ias_drawer_executive ias_drawer_executive -3 0 " << actPose->getOrigin().x() << " " << actPose->getOrigin().y() << " " << actPose->getOrigin().z() 00715 << " " << actPose->getRotation().x() << " " << actPose->getRotation().y() << " " << actPose->getRotation().z() << " " << actPose->getRotation().w()); 00716 } 00717 00718 return handle; 00719 } 00720 } 00721 00722 void OperateHandleController::close(int side_c, int handle_) 00723 { 00724 int handle = handle_; 00725 00726 RobotArm *arm = RobotArm::getInstance(side_c); 00727 //Gripper::getInstance(side_c)->close(0); 00728 00729 ROS_INFO("OperateHandleController: traj size %zu", openingTraj[handle].size()); 00730 00731 for (size_t k = 0; k < openingTraj[handle].size() ; ++k) 00732 { 00733 tf::Stamped<tf::Pose> *actPose = openingTraj[handle][k]; 00734 ROS_INFO("K = %zu", k); 00735 ROS_INFO_STREAM("rosrun ias_drawer_executive ias_drawer_executive -3 0 " << actPose->getOrigin().x() << " " << actPose->getOrigin().y() << " " << actPose->getOrigin().z() 00736 << " " << actPose->getRotation().x() << " " << actPose->getRotation().y() << " " << actPose->getRotation().z() << " " << actPose->getRotation().w()); 00737 } 00738 00739 for (int k = openingTraj[handle].size() - 1; k >= 1 ; k-=3) 00740 { 00741 tf::Stamped<tf::Pose> *actPose = openingTraj[handle][k]; 00742 ROS_INFO_STREAM("rosrun ias_drawer_executive ias_drawer_executive -3 0 " << actPose->getOrigin().x() << " " << actPose->getOrigin().y() << " " << actPose->getOrigin().z() 00743 << " " << actPose->getRotation().x() << " " << actPose->getRotation().y() << " " << actPose->getRotation().z() << " " << actPose->getRotation().w()); 00744 00745 boost::thread(&RobotHead::lookAt,RobotHead::getInstance(),"/map", actPose->getOrigin().x(),actPose->getOrigin().y(),actPose->getOrigin().z(), false); 00746 arm->universal_move_toolframe_ik_pose(*actPose); 00747 } 00748 arm->universal_move_toolframe_ik_pose(*openingTraj[handle][0]); 00749 00750 00751 for (size_t k = 1; k <= openingTraj[handle].size() / 3 ; k+=2) 00752 { 00753 tf::Stamped<tf::Pose> *actPose = openingTraj[handle][k]; 00754 arm->universal_move_toolframe_ik_pose(*actPose); 00755 } 00756 } 00757 00758 void spinner(Approach *apr) 00759 { 00760 double dist = 0; 00761 //double pos = 0; 00762 //RobotArm::getInstance(apr->side_)->time_to_target = 0.5; 00763 RobotArm::getInstance(apr->side_)->time_to_target = 1.0; 00764 //dist = apr->increment(0,0.15); 00765 //dist = apr->increment(0,0.6); 00766 dist = apr->increment(0,1.0); 00767 //RobotArm::getInstance(apr->side_)->time_to_target = 2.5; 00768 //ROS_INFO("DIST = %f ", dist); 00769 //if (dist == 0) { 00770 //ROS_ERROR("side %i DIST == 0", apr->side_); 00771 //dist = apr->increment(.6,1.0); 00772 //} 00773 } 00774 00775 void OperateHandleController::spinnerL(double x, double y, double z) 00776 { 00777 //lft->increment(l); 00778 Lift ll, lr; 00779 boost::thread a0(&Lift::init,&ll,0); 00780 boost::thread a1(&Lift::init,&lr,1); 00781 //lr.init(1); 00782 a0.join(); 00783 a1.join(); 00784 00785 boost::thread a(&Lift::increment, &ll, x, y, z); 00786 boost::thread b(&Lift::increment, &lr, x, y, z); 00787 00788 a.join(); 00789 b.join(); 00790 } 00791 00792 void OperateHandleController::openGrippers(bool wait) 00793 { 00794 Gripper *l = Gripper::getInstance(0); 00795 Gripper *r = Gripper::getInstance(1); 00796 00797 boost::thread a(&Gripper::open,l,1); 00798 boost::thread b(&Gripper::open,r,1); 00799 00800 if (wait) 00801 { 00802 a.join(); 00803 b.join(); 00804 } 00805 } 00806 00807 void OperateHandleController::closeGrippers(bool wait) 00808 { 00809 Gripper *l = Gripper::getInstance(0); 00810 Gripper *r = Gripper::getInstance(1); 00811 00812 boost::thread a(&Gripper::close,l,0); 00813 boost::thread b(&Gripper::close,r,0); 00814 00815 if (wait) 00816 { 00817 a.join(); 00818 b.join(); 00819 } 00820 } 00821 00822 00823 void OperateHandleController::pickPlate(btVector3 plate, double width) 00824 { 00825 00826 if (plate.z() == 0) 00827 { 00828 ROS_ERROR("NO PLATE FOUND!"); 00829 return; 00830 } 00831 00832 OperateHandleController::openGrippers(); 00833 00834 RobotArm *rightArm = RobotArm::getInstance(0); 00835 RobotArm *leftArm = RobotArm::getInstance(1); 00836 00837 00838 tf::Stamped<tf::Pose> plateCenter; 00839 plateCenter.frame_id_ = "map"; 00840 plateCenter.stamp_ = ros::Time(0); 00841 plateCenter.setOrigin(plate + btVector3(0,0,0.035)); // magic numbers galore -> this is adjusted for the cop plate detector with the standard .27m plates 00842 plateCenter.setOrigin(btVector3(plateCenter.getOrigin().x(),plateCenter.getOrigin().y(),plateCenter.getOrigin().z())); 00843 tf::Stamped<tf::Pose> plateCenterInBase = Geometry::getPoseIn("base_link", plateCenter); 00844 plateCenterInBase.setOrigin(plateCenterInBase.getOrigin() + btVector3(0,0.03,0)); 00845 00846 // how far from the edge of the plate should the approach start ? 00847 double startDistance = 0.05; 00848 // target point is somewhat inwards from the edge of the plate 00849 double insideDistance = 0.1; 00850 //double approachDistance = object ? 0.33 : 0.2; //distnace to center to start approach from 00851 00852 tf::Stamped<tf::Pose> leftApproach; 00853 leftApproach.frame_id_ = "base_link"; 00854 leftApproach.setOrigin(plateCenterInBase.getOrigin() + btVector3(0,width/2.0f + startDistance,0)); 00855 leftApproach.stamp_ = ros::Time(0); 00856 leftApproach.setRotation(btQuaternion(-0.302, 0.626, -0.303, 0.652)); 00857 tf::Stamped<tf::Pose> leftApproachMap = Geometry::getPoseIn("map", leftApproach); 00858 leftApproach.setOrigin(plateCenterInBase.getOrigin() + btVector3(0.05,width/2.0f + startDistance,0)); 00859 tf::Stamped<tf::Pose> leftApproachMapPad = Geometry::getPoseIn("map", leftApproach); 00860 leftApproach.setOrigin(plateCenterInBase.getOrigin() + btVector3(0,width/2.0f - insideDistance,0)); 00861 tf::Stamped<tf::Pose> leftApproachMapTarget = Geometry::getPoseIn("map", leftApproach); 00862 00863 ROS_INFO("START AT %f END AT %f", width/2.0f + startDistance, width/2.0f - insideDistance); 00864 00865 tf::Stamped<tf::Pose> rightApproach; 00866 rightApproach.frame_id_ = "base_link"; 00867 rightApproach.stamp_ = ros::Time(0); 00868 rightApproach.setOrigin(plateCenterInBase.getOrigin() + btVector3(0,-width/2.0f - startDistance,0)); 00869 rightApproach.setRotation(btQuaternion(0.651, 0.295, -0.621, -0.322)); 00870 tf::Stamped<tf::Pose> rightApproachMap = Geometry::getPoseIn("map", rightApproach); 00871 rightApproach.setOrigin(plateCenterInBase.getOrigin() + btVector3(0.05,-width/2.0f - startDistance,0)); 00872 tf::Stamped<tf::Pose> rightApproachMapPad = Geometry::getPoseIn("map", rightApproach); 00873 rightApproach.setOrigin(plateCenterInBase.getOrigin() + btVector3(0,-width/2.0f + insideDistance,0)); 00874 tf::Stamped<tf::Pose> rightApproachMapTarget = Geometry::getPoseIn("map", rightApproach); 00875 00876 ROS_INFO("START AT %f END AT %f", -width/2.0f - startDistance, - width/2.0f + insideDistance); 00877 00878 00879 tf::Stamped<tf::Pose> rightApproachMapHigh = rightApproachMap; 00880 rightApproachMapHigh.setOrigin(rightApproachMapHigh.getOrigin() + btVector3(0,0,.20)); 00881 00882 tf::Stamped<tf::Pose> leftApproachMapHigh = leftApproachMap; 00883 leftApproachMapHigh.setOrigin(leftApproachMapHigh.getOrigin() + btVector3(0,0,.20)); 00884 00885 00886 RobotArm::getInstance(0)->time_to_target = 5; 00887 RobotArm::getInstance(1)->time_to_target = 5; 00888 00889 std::vector<int> armv; 00890 std::vector<tf::Stamped<tf::Pose> > goal; 00891 //btVector3 result; 00892 tf::Stamped<tf::Pose> result; 00893 armv.push_back(0); 00894 goal.push_back(rightApproachMapPad); 00895 armv.push_back(0); 00896 goal.push_back(rightApproachMap); 00897 armv.push_back(1); 00898 goal.push_back(leftApproachMapPad); 00899 armv.push_back(1); 00900 goal.push_back(leftApproachMap); 00901 RobotArm::findBaseMovement(result, armv, goal, true, false); 00902 00903 00904 ROS_ERROR("LEFT APPROACH MAP"); 00905 leftArm->printPose(leftApproachMap); 00906 ROS_ERROR("RIGHT APPROACH MAP"); 00907 rightArm->printPose(rightApproachMap); 00908 00909 ROS_INFO("PLATE CENTER"); 00910 rightArm->printPose(plateCenter); 00911 00912 if (false) 00913 { 00914 00915 leftArm->preset_angle = 1.4; 00916 rightArm->preset_angle = 1.4; 00917 00918 leftArm->raise_elbow = true; 00919 rightArm->raise_elbow = true; 00920 } 00921 00922 leftApproachMap.stamp_ = ros::Time::now(); 00923 rightApproachMap.stamp_ = ros::Time::now(); 00924 00925 boost::thread tA(&RobotArm::move_toolframe_ik_pose, leftArm, leftApproachMap); 00926 boost::thread tB(&RobotArm::move_toolframe_ik_pose, rightArm, rightApproachMap); 00927 tA.join(); 00928 tB.join(); 00929 00930 Approach apl; 00931 Approach apr; 00932 apl.init(0,rightApproachMap,rightApproachMapTarget,Approach::inside); 00933 apr.init(1,leftApproachMap,leftApproachMapTarget,Approach::inside); 00934 boost::thread t1(spinner,&apl); 00935 boost::thread t2(spinner,&apr); 00936 00937 t1.join(); 00938 t2.join(); 00939 00940 boost::thread t3(&Approach::finish, apl); 00941 boost::thread t4(&Approach::finish, apr); 00942 t3.join(); 00943 t4.join(); 00944 00945 00946 if ((Gripper::getInstance(0)->getAmountOpen() < 0.005) || (Gripper::getInstance(1)->getAmountOpen() < 0.005)) 00947 { 00948 return pickPlate(plate); 00949 } 00950 00951 spinnerL(0,0,.2); 00952 00953 leftArm->preset_angle = -1; 00954 rightArm->preset_angle = -1; 00955 OperateHandleController::plateCarryPose(); 00956 leftArm->raise_elbow = false; 00957 rightArm->raise_elbow = false; 00958 } 00959 00960 00961 void OperateHandleController::singleSidedPick(int side,tf::Stamped<tf::Pose> start, tf::Stamped<tf::Pose> end) 00962 { 00963 Gripper *grip = Gripper::getInstance(side); 00964 RobotArm *arm = RobotArm::getInstance(side); 00965 00966 boost::thread t1(&Gripper::open, grip, 0.09); 00967 00968 arm->universal_move_toolframe_ik_pose(start); 00969 00970 t1.join(); 00971 00972 Approach apl; 00973 00974 apl.init(side,start,end,Approach::inside); 00975 spinner(&apl); 00976 apl.finish(); 00977 00978 } 00979 00980 00981 void OperateHandleController::getPlate(int object, double zHint) 00982 { 00983 00984 OperateHandleController::openGrippers(); 00985 00986 00987 pr2_controllers_msgs::JointTrajectoryGoal goalA = RobotArm::getInstance(0)->twoPointTrajectory(Poses::prepDishR1,Poses::prepDishR1); 00988 pr2_controllers_msgs::JointTrajectoryGoal goalB = RobotArm::getInstance(1)->twoPointTrajectory(Poses::prepDishL1,Poses::prepDishL1); 00989 boost::thread t7(&RobotArm::startTrajectory, RobotArm::getInstance(0), goalA,true); 00990 boost::thread t8(&RobotArm::startTrajectory, RobotArm::getInstance(1), goalB,true); 00991 00992 t7.join(); 00993 t8.join(); 00994 00995 btVector3 plate = object ? OperateHandleController::getTabletPose() : OperateHandleController::getPlatePose(); 00996 //btVector3 plate = btVector3(0.502,1.093,.785); 00997 00998 // account for hard coded / laser detected plate height 00999 if (zHint > 0.01) 01000 { 01001 plate.setZ(zHint); 01002 } 01003 01004 01005 01006 pickPlate(plate,0.3); 01007 } 01008 01009 01010 void OperateHandleController::plateCarryPose() 01011 { 01012 01013 tf::Stamped<tf::Pose> rightTip = RobotArm::getInstance(0)->getToolPose(); 01014 tf::Stamped<tf::Pose> leftTip = RobotArm::getInstance(1)->getToolPose(); 01015 01016 leftTip.setOrigin(btVector3(leftTip.getOrigin().x(), leftTip.getOrigin().y(), rightTip.getOrigin().z())); 01017 01018 RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(leftTip); 01019 01020 rightTip = RobotArm::getInstance(0)->getToolPose(); 01021 leftTip = RobotArm::getInstance(1)->getToolPose(); 01022 01023 double averageX = (rightTip.getOrigin().x() + leftTip.getOrigin().x()) * 0.5; 01024 double averageY = (rightTip.getOrigin().y() + leftTip.getOrigin().y()) * 0.5; 01025 double averageZ = (rightTip.getOrigin().z() + leftTip.getOrigin().z()) * 0.5; 01026 01027 //ROS_INFO("AVERAGE Y pre: %f", averageY); 01028 01029 RobotArm::getInstance(0)->raise_elbow= true; 01030 RobotArm::getInstance(1)->raise_elbow= true; 01031 01032 //RobotArm::getInstance(0)->preset_angle = .8; 01033 //RobotArm::getInstance(1)->preset_angle = .8; 01034 01035 OperateHandleController::spinnerL(.45 - averageX,0 - averageY,.935 - averageZ); 01036 01037 RobotArm::getInstance(0)->raise_elbow= false; 01038 RobotArm::getInstance(1)->raise_elbow= false; 01039 01040 01041 } 01042 01043 void OperateHandleController::plateTuckPose() 01044 { 01045 //pr2_controllers_msgs::JointTrajectoryGoal goalAT = RobotArm::getInstance(0)->twoPointTrajectory(Poses::prepDishRT,Poses::prepDishRT); 01046 //pr2_controllers_msgs::JointTrajectoryGoal goalBT = RobotArm::getInstance(1)->twoPointTrajectory(Poses::prepDishLT,Poses::prepDishLT); 01047 pr2_controllers_msgs::JointTrajectoryGoal goalAT = RobotArm::getInstance(0)->goalTraj(Poses::prepDishRT,2); 01048 pr2_controllers_msgs::JointTrajectoryGoal goalBT = RobotArm::getInstance(1)->goalTraj(Poses::prepDishLT,2); 01049 01050 boost::thread t2T(&RobotArm::startTrajectory, RobotArm::getInstance(0), goalAT,true); 01051 boost::thread t3T(&RobotArm::startTrajectory, RobotArm::getInstance(1), goalBT,true); 01052 t2T.join(); 01053 t3T.join(); 01054 } 01055 01056 void OperateHandleController::plateTuckPoseLeft() 01057 { 01058 //pr2_controllers_msgs::JointTrajectoryGoal goalBT = RobotArm::getInstance(1)->twoPointTrajectory(Poses::prepDishLT,Poses::prepDishLT); 01059 pr2_controllers_msgs::JointTrajectoryGoal goalBT = RobotArm::getInstance(0)->goalTraj(Poses::prepDishLT,2); 01060 boost::thread t3T(&RobotArm::startTrajectory, RobotArm::getInstance(1), goalBT,true); 01061 t3T.join(); 01062 } 01063 01064 01065 void OperateHandleController::plateTuckPoseRight() 01066 { 01067 //pr2_controllers_msgs::JointTrajectoryGoal goalAT = RobotArm::getInstance(0)->twoPointTrajectory(Poses::prepDishRT,Poses::prepDishRT); 01068 pr2_controllers_msgs::JointTrajectoryGoal goalAT = RobotArm::getInstance(0)->goalTraj(Poses::prepDishRT,2); 01069 boost::thread t2T(&RobotArm::startTrajectory, RobotArm::getInstance(0), goalAT,true); 01070 t2T.join(); 01071 } 01072 01073 01074 void OperateHandleController::plateAttackPose() 01075 { 01076 //pr2_controllers_msgs::JointTrajectoryGoal goalA = RobotArm::getInstance(0)->twoPointTrajectory(Poses::prepDishR1,Poses::prepDishR1); 01077 //pr2_controllers_msgs::JointTrajectoryGoal goalB = RobotArm::getInstance(1)->twoPointTrajectory(Poses::prepDishL1,Poses::prepDishL1); 01078 pr2_controllers_msgs::JointTrajectoryGoal goalA = RobotArm::getInstance(0)->goalTraj(Poses::prepDishR1,2); 01079 pr2_controllers_msgs::JointTrajectoryGoal goalB = RobotArm::getInstance(1)->goalTraj(Poses::prepDishL1,2); 01080 boost::thread t2(&RobotArm::startTrajectory, RobotArm::getInstance(0), goalA,true); 01081 boost::thread t3(&RobotArm::startTrajectory, RobotArm::getInstance(1), goalB,true); 01082 t2.join(); 01083 t3.join(); 01084 } 01085 01086 void OperateHandleController::plateAttackPoseLeft() 01087 { 01088 //pr2_controllers_msgs::JointTrajectoryGoal goalB = RobotArm::getInstance(1)->twoPointTrajectory(Poses::prepDishL1,Poses::prepDishL1); 01089 pr2_controllers_msgs::JointTrajectoryGoal goalB = RobotArm::getInstance(1)->goalTraj(Poses::prepDishL1,2); 01090 boost::thread t3(&RobotArm::startTrajectory, RobotArm::getInstance(1), goalB,true); 01091 t3.join(); 01092 } 01093 01094 void OperateHandleController::plateAttackPoseRight() 01095 { 01096 //pr2_controllers_msgs::JointTrajectoryGoal goalA = RobotArm::getInstance(0)->twoPointTrajectory(Poses::prepDishR1,Poses::prepDishR1); 01097 pr2_controllers_msgs::JointTrajectoryGoal goalA = RobotArm::getInstance(0)->goalTraj(Poses::prepDishR1,2); 01098 boost::thread t2(&RobotArm::startTrajectory, RobotArm::getInstance(0), goalA,true); 01099 t2.join(); 01100 } 01101 01102