Current.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 
00003 #include <iostream>
00004 #include <fstream>
00005 using namespace std;
00006 
00007 #include <ias_drawer_executive/Approach.h>
00008 #include <ias_drawer_executive/RobotDriver.h>
00009 #include <ias_drawer_executive/Gripper.h>
00010 #include <ias_drawer_executive/Geometry.h>
00011 #include <ias_drawer_executive/Pressure.h>
00012 #include <ias_drawer_executive/RobotArm.h>
00013 #include <ias_drawer_executive/Torso.h>
00014 #include <ias_drawer_executive/Head.h>
00015 #include <ias_drawer_executive/OperateHandleController.h>
00016 #include <ias_drawer_executive/Geometry.h>
00017 
00018 #include <ias_drawer_executive/Current.h>
00019 
00020 #include <ias_drawer_executive/ObjectLocalizer.h>
00021 
00022 #include <pcl/common/common.h>
00023 #include <pcl/filters/extract_indices.h>
00024 #include <pcl/ModelCoefficients.h>
00025 #include <pcl/point_types.h>
00026 #include <pcl/sample_consensus/method_types.h>
00027 #include <pcl/sample_consensus/model_types.h>
00028 #include <pcl/segmentation/sac_segmentation.h>
00029 #include <pcl/features/boundary.h>
00030 #include <pcl/features/normal_3d.h>
00031 
00032 #include <pcl/ros/conversions.h>
00033 #include <pcl_ros/point_cloud.h>
00034 #include <pcl_ros/transforms.h>
00035 
00036 #include <boost/thread.hpp>
00037 
00038 #include <visualization_msgs/Marker.h>
00039 
00040 
00041 #include "rosbag/bag.h"
00042 #include "rosbag/query.h"
00043 #include "rosbag/view.h"
00044 
00045 #include <ias_drawer_executive/Kinect.h>
00046 
00047 
00048 ros::Publisher perc_announcer; // perception announcer, sends a marker whenever something is perceived
00049 bool perc_init = false;
00050 int marker_id = 1;
00051 
00052 
00053 
00054 void announce(const std::string text, const tf::Stamped<tf::Pose> &poseStamped)
00055 {
00056     if (!perc_init)
00057     {
00058         perc_init = true;
00059         ros::NodeHandle node_handle;
00060         perc_announcer =  node_handle.advertise<visualization_msgs::Marker>( "/perception_announcer", 0 , true);
00061     }
00062 
00063     tf::Pose pose;
00064     pose.setOrigin(poseStamped.getOrigin());
00065     pose.setRotation(poseStamped.getRotation());
00066 
00067     visualization_msgs::Marker marker;
00068     marker.header.frame_id = poseStamped.frame_id_;
00069     marker.header.stamp = ros::Time::now();
00070     marker.ns = "perception_announcer";
00071     marker.id =  marker_id++;
00072     marker.text = text;
00073 
00074     marker.type = visualization_msgs::Marker::CUBE;
00075     marker.action = visualization_msgs::Marker::ADD;
00076 
00077     marker.pose.orientation.x = pose.getRotation().x();
00078     marker.pose.orientation.y = pose.getRotation().y();
00079     marker.pose.orientation.z = pose.getRotation().z();
00080     marker.pose.orientation.w = pose.getRotation().w();
00081     marker.pose.position.x = pose.getOrigin().x();
00082     marker.pose.position.y = pose.getOrigin().y();
00083     marker.pose.position.z = pose.getOrigin().z();
00084     marker.scale.x = 0.05;
00085     marker.scale.y = 0.05;
00086     marker.scale.z = 0.05;
00087     marker.color.r = 0.0;
00088     marker.color.g = 1.0;
00089     marker.color.b = 0.0;
00090     marker.color.a = 0.4;
00091 
00092     ROS_INFO("PUBLISH MARKER %i", marker.id);
00093 
00094     perc_announcer.publish( marker );
00095     ros::Duration(0.001).sleep();
00096 
00097     marker.id =  marker_id++;
00098     marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00099     marker.text = text;
00100 
00101     marker.color.r = 0.0;
00102     marker.color.g = 0.5;
00103     marker.color.b = 0.0;
00104     marker.color.a = 1.0;
00105     marker.scale.x = 0.1;
00106     marker.scale.y = 0.1;
00107     marker.scale.z = 0.1;
00108     //marker.pose.position.z = pose.getOrigin().z() + 0.05;
00109 
00110     perc_announcer.publish( marker );
00111     ros::Duration(0.001).sleep();
00112 }
00113 
00114 
00115 void announce_box(const std::string text, const btVector3 min, const btVector3 max)
00116 {
00117     if (!perc_init)
00118     {
00119         perc_init = true;
00120         ros::NodeHandle node_handle;
00121         perc_announcer =  node_handle.advertise<visualization_msgs::Marker>( "/perception_announcer", 0 , true);
00122     }
00123 
00124     visualization_msgs::Marker marker;
00125     marker.header.frame_id = "/map";
00126     marker.header.stamp = ros::Time::now();
00127     marker.ns = "perception_announcer";
00128     marker.id =  marker_id++;
00129     marker.text = text;
00130 
00131     marker.type = visualization_msgs::Marker::CUBE;
00132     marker.action = visualization_msgs::Marker::ADD;
00133 
00134     marker.pose.orientation.x = 0;
00135     marker.pose.orientation.y = 0;
00136     marker.pose.orientation.z = 0;
00137     marker.pose.orientation.w = 1;
00138     marker.pose.position.x = 0.5 * (max.x() + min.x());
00139     marker.pose.position.y = 0.5 * (max.y() + min.y());
00140     marker.pose.position.z = 0.5 * (max.z() + min.z());
00141     marker.scale.x = max.x() - min.x();
00142     marker.scale.y = max.y() - min.y();
00143     marker.scale.z = max.z() - min.z();
00144     marker.color.r = 0.0;
00145     marker.color.g = 0.0;
00146     marker.color.b = 0.4;
00147     marker.color.a = 0.2;
00148 
00149     ROS_INFO("PUBLISH MARKER %i", marker.id);
00150 
00151     perc_announcer.publish( marker );
00152     ros::Duration(0.001).sleep();
00153 
00154     marker.id =  marker_id++;
00155     marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00156     marker.text = text;
00157 
00158     marker.color.r = 0.0;
00159     marker.color.g = 0.0;
00160     marker.color.b = 0.4;
00161     marker.color.a = 0.2;
00162     marker.scale.x = 0.1;
00163     marker.scale.y = 0.1;
00164     marker.scale.z = 0.1;
00165     marker.pose.position.z = marker.pose.position.z + 0.05;
00166 
00167     perc_announcer.publish( marker );
00168     ros::Duration(0.001).sleep();
00169 }
00170 
00171 bool cloud_pub_initialized = false;
00172 ros::Publisher cloud_pub;
00173 
00174 void pubCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
00175 {
00176 
00177     //ROS_INFO("start pub");
00178 
00179     if (!cloud_pub_initialized)
00180     {
00181         ros::NodeHandle node_handle;
00182         cloud_pub = node_handle.advertise<sensor_msgs::PointCloud2>("/debug_cloud",0,true);
00183         cloud_pub_initialized=true;
00184     }
00185 
00186     sensor_msgs::PointCloud2 out; //in map frame
00187 
00188     pcl::toROSMsg(*cloud,out);
00189     out.header.frame_id = "/map";
00190     cloud_pub.publish(out);
00191 
00192     ROS_INFO("published %i x %i points", out.height, out.width);
00193     ros::Duration(0.001).sleep();
00194 
00195 }
00196 
00197 
00198 
00199 void printPose(const char title[], tf::Stamped<tf::Pose> pose)
00200 {
00201     ROS_INFO("rosrun tf static_transform_publisher %f %f %f %f %f %f %f %s %s 100", pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z()
00202              , pose.getRotation().x(), pose.getRotation().y(), pose.getRotation().z(), pose.getRotation().w(), pose.frame_id_.c_str(), title);
00203 }
00204 
00205 void printPoseSimple(tf::Stamped<tf::Pose> pose)
00206 {
00207     ROS_INFO("{%f,%f,%f,%f,%f,%f,%f},", pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z(),
00208              pose.getRotation().x(), pose.getRotation().y(), pose.getRotation().z(), pose.getRotation().w());
00209 }
00210 
00211 
00212 int development_tools(int argc, char** argv)
00213 {
00214 
00215 
00216     if (atoi(argv[1]) == -901)
00217     {
00218         tf::Stamped<tf::Pose> lid;
00219         lid.frame_id_ = argv[4];
00220         lid.setOrigin(btVector3(0 ,0,0));
00221         lid.setRotation(btQuaternion(0,0,0,1));
00222 
00223         lid = Geometry::getPoseIn(argv[3], lid);
00224 
00225         printf("\ntf::Stamped<tf::Pose> %s;\n",argv[2]);
00226         printf("%s.frame_id_ = \"%s\";\n",argv[2],argv[3]);
00227         printf("%s.setOrigin(btVector3(%f, %f, %f));\n",argv[2], lid.getOrigin().x(), lid.getOrigin().y(), lid.getOrigin().z());
00228         printf("%s.setRotation(btQuaternion(%f, %f, %f, %f));\n",argv[2], lid.getRotation().x(), lid.getRotation().y(), lid.getRotation().z(), lid.getRotation().w());
00229 
00230         printf("\nRobotArm::getInstance(%s)->universal_move_toolframe_ik_pose(%s);\n",argv[5],argv[2]);
00231 
00232         printf("\nbin/ias_drawer_executive -3 %s %f %f %f %f %f %f %f\n", argv[5] ,lid.getOrigin().x(), lid.getOrigin().y(), lid.getOrigin().z(), lid.getRotation().x(), lid.getRotation().y(), lid.getRotation().z(), lid.getRotation().w());
00233     }
00234 
00235 
00236     if (atoi(argv[1]) == -911)
00237     {
00238         {
00239             tf::Stamped<tf::Pose> lid;
00240             lid.frame_id_ = "r_gripper_tool_frame";
00241             lid.setOrigin(btVector3(0 ,0,0));
00242             lid.setRotation(btQuaternion(0,0,0,1));
00243 
00244             lid = Geometry::getPoseIn("/map", lid);
00245 
00246             printf("\ntf::Stamped<tf::Pose> r%s;\n",argv[2]);
00247             printf("r%s.frame_id_ = \"%s\";\n",argv[2],"/map");
00248             printf("r%s.setOrigin(btVector3(%f, %f, %f));\n",argv[2], lid.getOrigin().x(), lid.getOrigin().y(), lid.getOrigin().z());
00249             printf("r%s.setRotation(btQuaternion(%f, %f, %f, %f));\n",argv[2], lid.getRotation().x(), lid.getRotation().y(), lid.getRotation().z(), lid.getRotation().w());
00250 
00251             printf("\nRobotArm::getInstance(0)->universal_move_toolframe_ik_pose(r%s);\n",argv[2]);
00252         }
00253 
00254         {
00255             tf::Stamped<tf::Pose> lid;
00256             lid.frame_id_ = "l_gripper_tool_frame";
00257             lid.setOrigin(btVector3(0 ,0,0));
00258             lid.setRotation(btQuaternion(0,0,0,1));
00259 
00260             lid = Geometry::getPoseIn("/map", lid);
00261 
00262             printf("\ntf::Stamped<tf::Pose> l%s;\n",argv[2]);
00263             printf("l%s.frame_id_ = \"%s\";\n",argv[2],"/map");
00264             printf("l%s.setOrigin(btVector3(%f, %f, %f));\n",argv[2], lid.getOrigin().x(), lid.getOrigin().y(), lid.getOrigin().z());
00265             printf("l%s.setRotation(btQuaternion(%f, %f, %f, %f));\n",argv[2], lid.getRotation().x(), lid.getRotation().y(), lid.getRotation().z(), lid.getRotation().w());
00266 
00267             printf("\nRobotArm::getInstance(1)->universal_move_toolframe_ik_pose(l%s);\n",argv[2]);
00268         }
00269         printf("\nRobotArm::moveBothArms(l%s,r%s);\n",argv[2],argv[2]);
00270 
00271     }
00272 
00273     if (atoi(argv[1]) == -912)
00274     {
00275         {
00276             tf::Stamped<tf::Pose> lid;
00277             lid.frame_id_ = "r_gripper_tool_frame";
00278             lid.setOrigin(btVector3(0 ,0,0));
00279             lid.setRotation(btQuaternion(0,0,0,1));
00280 
00281             lid = Geometry::getPoseIn("base_link", lid);
00282 
00283             printf("\ntf::Stamped<tf::Pose> r%s;\n",argv[2]);
00284             printf("r%s.frame_id_ = \"%s\";\n",argv[2],"base_link");
00285             printf("r%s.setOrigin(btVector3(%f, %f, %f));\n",argv[2], lid.getOrigin().x(), lid.getOrigin().y(), lid.getOrigin().z());
00286             printf("r%s.setRotation(btQuaternion(%f, %f, %f, %f));\n",argv[2], lid.getRotation().x(), lid.getRotation().y(), lid.getRotation().z(), lid.getRotation().w());
00287 
00288             printf("\nRobotArm::getInstance(0)->universal_move_toolframe_ik_pose(r%s);\n",argv[2]);
00289         }
00290 
00291         {
00292             tf::Stamped<tf::Pose> lid;
00293             lid.frame_id_ = "l_gripper_tool_frame";
00294             lid.setOrigin(btVector3(0 ,0,0));
00295             lid.setRotation(btQuaternion(0,0,0,1));
00296 
00297             lid = Geometry::getPoseIn("base_link", lid);
00298 
00299             printf("\ntf::Stamped<tf::Pose> l%s;\n",argv[2]);
00300             printf("l%s.frame_id_ = \"%s\";\n",argv[2],"base_link");
00301             printf("l%s.setOrigin(btVector3(%f, %f, %f));\n",argv[2], lid.getOrigin().x(), lid.getOrigin().y(), lid.getOrigin().z());
00302             printf("l%s.setRotation(btQuaternion(%f, %f, %f, %f));\n",argv[2], lid.getRotation().x(), lid.getRotation().y(), lid.getRotation().z(), lid.getRotation().w());
00303 
00304             printf("\nRobotArm::getInstance(1)->universal_move_toolframe_ik_pose(l%s);\n",argv[2]);
00305         }
00306         printf("\nRobotArm::moveBothArms(l%s,r%s);\n",argv[2],argv[2]);
00307 
00308     }
00309 
00310 
00311     if (atoi(argv[1]) == -910)
00312     {
00313         tf::Stamped<tf::Pose> lid;
00314         lid.frame_id_ = argv[4];
00315         lid.setOrigin(btVector3(atof(argv[6]),atof(argv[7]),atof(argv[8])));
00316         lid.setRotation(btQuaternion(0,0,0,1));
00317 
00318         lid = Geometry::getPoseIn(argv[3], lid);
00319 
00320         printf("\ntf::Stamped<tf::Pose> %s;\n",argv[2]);
00321         printf("%s.frame_id_ = \"%s\";\n",argv[2],argv[3]);
00322         printf("%s.setOrigin(btVector3(%f, %f, %f));\n",argv[2], lid.getOrigin().x(), lid.getOrigin().y(), lid.getOrigin().z());
00323         printf("%s.setRotation(btQuaternion(%f, %f, %f, %f));\n",argv[2], lid.getRotation().x(), lid.getRotation().y(), lid.getRotation().z(), lid.getRotation().w());
00324 
00325         printf("\nRobotArm::getInstance(%s)->universal_move_toolframe_ik_pose(%s);\n",argv[5],argv[2]);
00326 
00327         printf("\nbin/ias_drawer_executive -3 %s %f %f %f %f %f %f %f\n", argv[5] ,lid.getOrigin().x(), lid.getOrigin().y(), lid.getOrigin().z(), lid.getRotation().x(), lid.getRotation().y(), lid.getRotation().z(), lid.getRotation().w());
00328     }
00329 
00330     if (atoi(argv[1]) == -940)
00331     {
00332         tf::Stamped<tf::Pose> lid;
00333         lid.frame_id_ = "base_link";
00334         lid.setOrigin(btVector3(0 ,0,0));
00335         lid.setRotation(btQuaternion(0,0,0,1));
00336 
00337         lid = Geometry::getPoseIn("/map", lid);
00338 
00339         printf("\nRobotDriver::moveBase4(%f, %f, %f, %f);\n",lid.getOrigin().x(), lid.getOrigin().y(), lid.getRotation().z(), lid.getRotation().w());
00340 
00341     }
00342     return 0;
00343 }
00344 
00345 /*
00346 tf::Stamped<tf::Pose> getPotPose()
00347 {
00348 
00349     //if (atoi(argv[1]) == -617)
00350     {
00351         ros::NodeHandle node_handle;
00352         ros::Publisher pose_pub = node_handle.advertise<geometry_msgs::PoseStamped>( "/cop_detection", 0 , true);
00353         ros::Publisher best_pose_pub = node_handle.advertise<geometry_msgs::PoseStamped>( "/cop_detection_best", 0, true );
00354 
00355         std::vector<tf::Stamped<tf::Pose> > poses;
00356         std::vector<double > votes;
00357 
00358         while (ros::ok())
00359         {
00360             tf::Stamped<tf::Pose> pose;
00361             pose = OperateHandleController::getCopPose("PotWW", "/openni_rgb_optical_frame");
00362             ros::Duration(0.05).sleep();
00363             if (pose.frame_id_ == "NO_ID_STAMPED_DEFAULT_CONSTRUCTION")
00364             {
00365                 ROS_ERROR("dropped %s", pose.frame_id_.c_str());
00366                 continue;
00367             }
00368 
00369             if (pose.getOrigin().length() < 0.1)
00370                 continue;
00371             pose = Geometry::getPoseIn("/map", pose);
00372 
00373             geometry_msgs::PoseStamped pose_stamped, best_pose_stamped;
00374 
00375             tf::poseStampedTFToMsg(pose,pose_stamped);
00376             pose_pub.publish(pose_stamped);
00377             ros::Duration(0.05).sleep();
00378 
00379             if ((btVector3(-1.836059, 2.612557, 1.016513) - pose.getOrigin()).length() > 0.5)
00380                 continue;
00381 
00382             {
00383                 poses.push_back(pose);
00384                 votes.push_back(.0);
00385 
00386                 ROS_INFO("tick");
00387 
00388                 if (poses.size() > 1)
00389                 {
00390                     for (size_t k = 0; k < poses.size(); ++k)
00391                     {
00392                         votes[k] = 0;
00393                     }
00394                     double max_score = 0;
00395                     for (size_t k = 0; k < poses.size(); ++k)
00396                     {
00397                         for (size_t j = 0; j < poses.size(); ++j)
00398                         {
00399                             btTransform a,b,c;
00400                             a.setOrigin(poses[k].getOrigin());
00401                             a.setRotation(poses[k].getRotation());
00402                             b.setOrigin(poses[j].getOrigin());
00403                             b.setRotation(poses[j].getRotation());
00404                             c = b.inverseTimes(a);
00405                             double score = c.getOrigin().length() + c.getRotation().getAngle() * .001;
00406                             votes[j] += score;
00407                             votes[k] += score;
00408                             if (votes[k] > max_score)
00409                                 max_score = votes[k];
00410                             if (votes[j] > max_score)
00411                                 max_score = votes[j];
00412                         }
00413                     }
00414                     double minscore = 1000000;
00415                     tf::Stamped<tf::Pose> best;
00416                     for (size_t k = 0; k < poses.size(); ++k)
00417                     {
00418                         double act = votes[k] / max_score;
00419                         ROS_INFO("%zu score %f: %f", k, votes[k], act);
00420                         if (act < minscore)
00421                         {
00422                             minscore = act;
00423                             best = poses[k];
00424                         }
00425                     }
00426                     tf::poseStampedTFToMsg(best,best_pose_stamped);
00427                     ROS_INFO("min score = %f", minscore);
00428                     best_pose_pub.publish(best_pose_stamped);
00429                     if (poses.size() > 3)
00430                         return best;
00431                     ros::Duration(0.05).sleep();
00432                 }
00433             }
00434         }
00435 
00436     }
00437 
00438     tf::Stamped<tf::Pose> t;
00439     return t;
00440 
00441 }
00442 */
00443 
00444 
00445 void pubMarker(tf::Stamped<tf::Pose> pose, double radius)
00446 {
00447     ros::NodeHandle node_handle;
00448     ros::Publisher vis_pub = node_handle.advertise<visualization_msgs::Marker>( "/robot_arm_marker", 0 );
00449 
00450     visualization_msgs::Marker marker;
00451     marker.header.frame_id = "/map";
00452     marker.header.stamp = ros::Time::now();
00453     marker.ns = "goalpoints";
00454     marker.id =  10000;
00455     marker.type = visualization_msgs::Marker::CYLINDER;
00456     marker.action = visualization_msgs::Marker::ADD;
00457 
00458     marker.pose.orientation.x = pose.getRotation().x();
00459     marker.pose.orientation.y = pose.getRotation().y();
00460     marker.pose.orientation.z = pose.getRotation().z();
00461     marker.pose.orientation.w = pose.getRotation().w();
00462     marker.pose.position.x = pose.getOrigin().x();
00463     marker.pose.position.y = pose.getOrigin().y();
00464     marker.pose.position.z = pose.getOrigin().z();
00465     marker.scale.x = radius;
00466     marker.scale.y = radius;
00467     marker.scale.z = 0.3;
00468     marker.color.r = 1.0;
00469     marker.color.g = 0.0;
00470     marker.color.b = 0.0;
00471     marker.color.a = 0.5;
00472 
00473     ROS_INFO("PUBLISH MARKER %i", marker.id);
00474 
00475     ros::Rate rate(10.0);
00476     int k = 0;
00477     while (k < 10)
00478     {
00479         marker.id = ++marker.id;
00480         vis_pub.publish( marker );
00481         ros::spinOnce();
00482         rate.sleep();
00483     }
00484 }
00485 
00486 
00487 /*void getKinectCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, ros::Time after = ros::Time(0,0))
00488 {
00489 
00490     sensor_msgs::PointCloud2 pc;// = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/kinect/cloud_throttled"));
00491     bool found = false;
00492     while (!found)
00493     {
00494         //pc  = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/kinect/cloud_throttled"));
00495         pc  = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/camera/rgb/points"));
00496         if ((after == ros::Time(0,0)) || (pc.header.stamp > after))
00497             found = true;
00498         else
00499         {
00500             ROS_ERROR("getKinectCloudXYZ cloud too old : stamp %f , target time %f",pc.header.stamp.toSec(), after.toSec());
00501         }
00502     }
00503     tf::Stamped<tf::Pose> net_stamped = RobotArm::getPose("/map","/openni_rgb_optical_frame");
00504     tf::Transform net_transform;
00505     net_transform.setOrigin(net_stamped.getOrigin());
00506     net_transform.setRotation(net_stamped.getRotation());
00507 
00508     sensor_msgs::PointCloud2 pct; //in map frame
00509 
00510     pcl_ros::transformPointCloud("/map",net_transform,pc,pct);
00511     pct.header.frame_id = "/map";
00512 
00513     pcl::fromROSMsg(pct, *cloud);
00514 }
00515 
00516 void getKinectCloudXYZ(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, ros::Time after = ros::Time(0,0))
00517 {
00518 
00519     sensor_msgs::PointCloud2 pc;
00520     bool found = false;
00521     while (!found)
00522     {
00523         //pc  = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/kinect/cloud_throttled"));
00524         pc  = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/camera/rgb/points"));
00525         if ((after == ros::Time(0,0)) || (pc.header.stamp > after))
00526             found = true;
00527         else
00528         {
00529             ROS_ERROR("getKinectCloudXYZ cloud too old : stamp %f , target time %f",pc.header.stamp.toSec(), after.toSec());
00530         }
00531     }
00532     tf::Stamped<tf::Pose> net_stamped = RobotArm::getPose("/map","/openni_rgb_optical_frame");
00533     tf::Transform net_transform;
00534     net_transform.setOrigin(net_stamped.getOrigin());
00535     net_transform.setRotation(net_stamped.getRotation());
00536 
00537     sensor_msgs::PointCloud2 pct; //in map frame
00538 
00539     pcl_ros::transformPointCloud("/map",net_transform,pc,pct);
00540     pct.header.frame_id = "/map";
00541 
00542     pcl::fromROSMsg(pct, *cloud);
00543 }
00544 */
00545 
00546 void discardKinectFrame()
00547 {
00548     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00549     Kinect::getInstance()->getCloud(cloud,"/map");
00550 }
00551 
00552 
00553 
00554 void getPointsInBox(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, btVector3 min, btVector3 max, pcl::PointCloud<pcl::PointXYZRGB>::Ptr inBox)
00555 {
00556 
00557     Eigen::Vector4f min_pt, max_pt;
00558 
00559     min_pt = Eigen::Vector4f(min.x(),min.y(),min.z(), 1);
00560     max_pt = Eigen::Vector4f(max.x(),max.y(),max.z(), 1);
00561 
00562     //{
00563 
00564     //btVector3 roi_min(min_pt[0],min_pt[1],min_pt[2]);
00565     //btVector3 roi_max(max_pt[0],max_pt[1],max_pt[2]);
00566 
00567     //announce_box("roi", roi_min, roi_max);
00568 
00569     //}
00570 
00571 
00572     ROS_INFO("min %f %f %f" ,min_pt[0],min_pt[1],min_pt[2]);
00573     ROS_INFO("max %f %f %f" ,max_pt[0],max_pt[1],max_pt[2]);
00574 
00575     ROS_INFO("cloud size : %zu", cloud->points.size());
00576 
00577     boost::shared_ptr<std::vector<int> > indices( new std::vector<int> );
00578     pcl::getPointsInBox(*cloud,min_pt,max_pt,*indices);
00579 
00580     pcl::ExtractIndices<pcl::PointXYZRGB> ei;
00581     ei.setInputCloud(cloud);
00582     ei.setIndices(indices);
00583     ei.filter(*inBox);
00584 
00585     pubCloud(inBox);
00586 
00587     ROS_INFO("cloud size after box filtering: %zu", inBox->points.size());
00588 }
00589 
00590 
00591 //get circle2d in xy plane within searchspace min->max, takin pointcloud on its own
00592 bool getCircle(const btVector3 min, const btVector3 max, btVector3 &center, double &radius)
00593 {
00594 
00595     ros::NodeHandle node_handle;
00596     ros::Publisher pose_pub = node_handle.advertise<geometry_msgs::PoseStamped>( "/cop_detection", 0 , true);
00597     ros::Publisher pose_pubr = node_handle.advertise<geometry_msgs::PoseStamped>( "/cop_detectionr", 0 , true);
00598     ros::Publisher pose_publ = node_handle.advertise<geometry_msgs::PoseStamped>( "/cop_detectionl", 0 , true);
00599 
00600     sensor_msgs::PointCloud2 pc = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/kinect/cloud_throttled"));
00601 
00602     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
00603     pcl::PointCloud<pcl::PointXYZ>::Ptr pot_cyl_cloud(new pcl::PointCloud<pcl::PointXYZ>);
00604 
00605     tf::Stamped<tf::Pose> net_stamped = Geometry::getPose("/map","/openni_rgb_optical_frame");
00606     tf::Transform net_transform;
00607     net_transform.setOrigin(net_stamped.getOrigin());
00608     net_transform.setRotation(net_stamped.getRotation());
00609 
00610     sensor_msgs::PointCloud2 pct; //in map frame
00611 
00612     pcl_ros::transformPointCloud("/map",net_transform,pc,pct);
00613     pct.header.frame_id = "/map";
00614 
00615     pcl::fromROSMsg(pct, *cloud);
00616 
00617     Eigen::Vector4f min_pt, max_pt;
00618 
00619     min_pt = Eigen::Vector4f(min.x(),min.y(),min.z(), 1);
00620     max_pt = Eigen::Vector4f(max.x(),max.y(),max.z(), 1);
00621     {
00622         tf::Stamped<tf::Pose> res;
00623         res.setOrigin(min);
00624         res.setRotation(btQuaternion(0,0,0,1));
00625         res.frame_id_ = "/map";
00626 
00627         geometry_msgs::PoseStamped res_msg;
00628         tf::poseStampedTFToMsg(res,res_msg);
00629 
00630         pose_pubr.publish(res_msg);
00631     }
00632 
00633     {
00634         tf::Stamped<tf::Pose> res;
00635         res.setOrigin(max);
00636         res.setRotation(btQuaternion(0,0,0,1));
00637         res.frame_id_ = "/map";
00638 
00639         geometry_msgs::PoseStamped res_msg;
00640         tf::poseStampedTFToMsg(res,res_msg);
00641 
00642         pose_publ.publish(res_msg);
00643     }
00644 
00645 
00646 
00647     ROS_INFO("min %f %f %f" ,min.x(),min.y(),min.z());
00648     ROS_INFO("max %f %f %f" ,max.x(),max.y(),max.z());
00649 
00650     ROS_INFO("cloud size : %zu", cloud->points.size());
00651 
00652     boost::shared_ptr<std::vector<int> > indices( new std::vector<int> );
00653     pcl::getPointsInBox(*cloud,min_pt,max_pt,*indices);
00654 
00655     pcl::ExtractIndices<pcl::PointXYZ> ei;
00656     ei.setInputCloud(cloud);
00657     ei.setIndices(indices);
00658     ei.filter(*pot_cyl_cloud);
00659 
00660     ROS_INFO("cloud size : %zu", pot_cyl_cloud->points.size());
00661 
00662     pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00663     pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00664     // Create the segmentation object
00665     pcl::SACSegmentation<pcl::PointXYZ> seg;
00666     // Optional
00667     seg.setOptimizeCoefficients (true);
00668     // Mandatory
00669     seg.setModelType (pcl::SACMODEL_CIRCLE2D);
00670     seg.setMethodType (pcl::SAC_RANSAC);
00671     seg.setDistanceThreshold (0.01);
00672 
00673     seg.setInputCloud (pot_cyl_cloud);
00674     seg.segment (*inliers, *coefficients);
00675 
00676     if (inliers->indices.size () == 0)
00677     {
00678         PCL_ERROR ("Could not estimate a planar model for the given dataset.");
00679         return false;
00680     }
00681     else
00682     {
00683         ROS_INFO("MODEL :");
00684         //coefficients.get()
00685         ROS_INFO("%f %f %f %f", coefficients->values[0], coefficients->values[1], coefficients->values[2], coefficients->values[3]);
00686         center = btVector3(coefficients->values[0], coefficients->values[1],0.5*(min.z() + max.z()));
00687         radius = coefficients->values[2];
00688 
00689 
00690         tf::Stamped<tf::Pose> res;
00691         res.setOrigin(center);
00692         res.setRotation(btQuaternion(0,0,0,1));
00693         res.frame_id_ = "/map";
00694 
00695         geometry_msgs::PoseStamped res_msg;
00696         tf::poseStampedTFToMsg(res,res_msg);
00697 
00698         pose_pub.publish(res_msg);
00699 
00700         //ros::Duration(1.0).sleep();
00701 
00702         return true;
00703     }
00704 }
00705 
00707 bool getCircleFromCloud(pcl::PointCloud<pcl::PointXYZRGB> &pot_cyl_cloud, btVector3 &center, double &radius)
00708 {
00709 
00710     ros::NodeHandle node_handle;
00711     ros::Publisher pose_pub = node_handle.advertise<geometry_msgs::PoseStamped>( "/cop_detection", 0 , true);
00712 
00713     pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00714     pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00715     // Create the segmentation object
00716     pcl::SACSegmentation<pcl::PointXYZRGB> seg;
00717     // Optional
00718     seg.setOptimizeCoefficients (true);
00719     // Mandatory
00720     seg.setModelType (pcl::SACMODEL_CIRCLE2D);
00721     seg.setMethodType (pcl::SAC_RANSAC);
00722     seg.setDistanceThreshold (0.01);
00723 
00724     seg.setInputCloud (pot_cyl_cloud.makeShared());
00725     seg.segment (*inliers, *coefficients);
00726 
00727     seg.setRadiusLimits(radius - .01, radius + .01);
00728 
00729     if (inliers->indices.size () == 0)
00730     {
00731         PCL_ERROR ("Could not estimate a planar model for the given dataset.");
00732         return false;
00733     }
00734     else
00735     {
00736 
00737         double z = 0;
00738         for (size_t k=0; k < inliers->indices.size(); ++k)
00739         {
00740             z += pot_cyl_cloud.points[inliers->indices[k]].z;
00741         }
00742         z /=  inliers->indices.size();
00743         ROS_INFO("MODEL :");
00744         //coefficients.get()
00745         ROS_INFO("%f %f %f %f SIZE : %zu", coefficients->values[0], coefficients->values[1], coefficients->values[2], coefficients->values[3], inliers->indices.size());
00746         center = btVector3(coefficients->values[0], coefficients->values[1],z);
00747         radius = coefficients->values[2];
00748 
00749         tf::Stamped<tf::Pose> res;
00750         res.setOrigin(center);
00751         res.setRotation(btQuaternion(0,0,0,1));
00752         res.frame_id_ = "/map";
00753 
00754         geometry_msgs::PoseStamped res_msg;
00755         tf::poseStampedTFToMsg(res,res_msg);
00756 
00757         pose_pub.publish(res_msg);
00758 
00759         //ros::Duration(1.0).sleep();
00760 
00761         return true;
00762     }
00763 }
00764 
00765 
00766 
00767 
00768 bool getCirclesFromCloud(pcl::PointCloud<pcl::PointXYZRGB> &pot_cyl_cloud, double radius_goal, double radius_tolerance,
00769                          std::vector<btVector3> &center,
00770                          std::vector<double> &radius,
00771                          std::vector<int> &numinliers,
00772                          size_t  iterations = 5)
00773 {
00774 
00775 
00776     ros::NodeHandle node_handle;
00777 
00778     //ros::Publisher cloud_pub = node_handle.advertise<sensor_msgs::PointCloud2>("/debug_cloud",0,true);
00779 
00780     center.clear();
00781     radius.clear();
00782     numinliers.clear();
00783 
00784     center.resize(iterations);
00785     radius.resize(iterations);
00786     numinliers.resize(iterations);
00787 
00788     pcl::PointCloud<pcl::PointXYZRGB> act = pot_cyl_cloud;
00789     pcl::PointCloud<pcl::PointXYZRGB> flip;
00790 
00791     for (size_t k = 0; k < iterations; k++)
00792     {
00793 
00794         //sensor_msgs::PointCloud2 out; //in map frame
00795         //pcl::toROSMsg(act,out);
00796         //out.header.frame_id = "/map";
00797         //cloud_pub.publish(out);
00798         pubCloud(act.makeShared());
00799 
00800         ros::NodeHandle node_handle;
00801         ros::Publisher pose_pub = node_handle.advertise<geometry_msgs::PoseStamped>( "/cop_detection", 0 , true);
00802 
00803         pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00804         pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00805         // Create the segmentation object
00806         pcl::SACSegmentation<pcl::PointXYZRGB> seg;
00807         // Optional
00808         seg.setOptimizeCoefficients (true);
00809         // Mandatory
00810         seg.setModelType (pcl::SACMODEL_CIRCLE2D);
00811         seg.setMethodType (pcl::SAC_RANSAC);
00812         seg.setDistanceThreshold (0.01);
00813 
00814         seg.setInputCloud (act.makeShared());
00815         seg.segment (*inliers, *coefficients);
00816 
00817         seg.setRadiusLimits(radius_goal - radius_tolerance, radius_goal + radius_tolerance);
00818 
00819         if (inliers->indices.size () == 0)
00820         {
00821             PCL_ERROR ("Could not estimate a planar model for the given dataset.");
00822             return false;
00823         }
00824         else
00825         {
00826             double z = 0;
00827             for (size_t g=0; g < inliers->indices.size(); ++g)
00828             {
00829                 z += act.points[inliers->indices[g]].z;
00830             }
00831             z /=  inliers->indices.size();
00832             ROS_INFO("MODEL :");
00833             //coefficients.get()
00834             ROS_INFO("%f %f %f %f SIZE : %zu", coefficients->values[0], coefficients->values[1], coefficients->values[2], coefficients->values[3], inliers->indices.size());
00835             center[k] = btVector3(coefficients->values[0], coefficients->values[1],z);
00836             radius[k] = coefficients->values[2];
00837             numinliers[k] = inliers->indices.size();
00838 
00839             tf::Stamped<tf::Pose> res;
00840             res.setOrigin(center[k]);
00841             res.setRotation(btQuaternion(0,0,0,1));
00842             res.frame_id_ = "/map";
00843 
00844             geometry_msgs::PoseStamped res_msg;
00845             tf::poseStampedTFToMsg(res,res_msg);
00846 
00847             pose_pub.publish(res_msg);
00848 
00849         }
00850         flip = act;
00851         pcl::ExtractIndices<pcl::PointXYZRGB> ei;
00852         ei.setInputCloud(flip.makeShared());
00853         ei.setIndices(inliers);
00854         ei.setNegative(true);
00855         ei.filter(act);
00856 
00857         ROS_INFO("BLIP");
00858         //ros::Duration(1).sleep();
00859     }
00860     return true;
00861 }
00862 
00863 bool getLidPose(tf::Stamped<tf::Pose> &lid, btVector3 min = btVector3(-2.134, 2.625,0.955), btVector3 max = btVector3(-1.663, 3.215,0.972))
00864 {
00865 
00866     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00867 
00868     Kinect::getInstance()->getCloud(cloud);
00869 
00870     //btVector3 min(-2.134, 2.625,0.953);
00871     //btVector3 max(-1.663, 3.215,0.972);
00872 
00873     pcl::PointCloud<pcl::PointXYZRGB>::Ptr inBox (new pcl::PointCloud<pcl::PointXYZRGB>);
00874 
00875     getPointsInBox(cloud,min,max,inBox);
00876 
00877     pubCloud(inBox);
00878 
00879     std::vector<btVector3> center;
00880     std::vector<double> radius;
00881     std::vector<int> numinliers;
00882 
00883     getCirclesFromCloud(*inBox, .2, .04,center,radius,numinliers,1);
00884 
00885     if (numinliers[0] < 300)
00886         return false;
00887 
00888     lid.setOrigin(center[0]);
00889     lid.setRotation(btQuaternion(0,0,0,1));
00890     lid.frame_id_ = "/map";
00891 
00892     lid = Geometry::getPoseIn("base_link", lid);
00893     lid.setRotation(btQuaternion(0,0,1,0));
00894     lid = Geometry::getPoseIn("map", lid);
00895 
00896     announce("Lid", lid);
00897 
00898     return true;
00899 }
00900 
00901 
00902 void calcPotGraspPoints(tf::Stamped<tf::Pose> det, tf::Stamped<tf::Pose> &leftpreg, tf::Stamped<tf::Pose> &leftg, tf::Stamped<tf::Pose> &rightpreg, tf::Stamped<tf::Pose> &rightg)
00903 {
00904     btTransform potww;
00905     potww.setOrigin(det.getOrigin());
00906     potww.setRotation(det.getRotation());
00907     btTransform leftrel, rightrel;
00908     btTransform leftgrasp, rightgrasp;
00909 
00910     rightrel.setOrigin(btVector3(0.136, -0.012, -0.014));
00911     rightrel.setRotation(btQuaternion(-0.179, -0.684, 0.685, -0.175));
00912 
00913     leftrel.setOrigin(btVector3(-0.130, -0.008, -0.013));
00914     leftrel.setRotation(btQuaternion(-0.677, 0.116, 0.168, 0.707));
00915 
00916     btTransform pregrasp(btQuaternion(0,0,0,1), btVector3(-.06,0,0));
00917     btTransform leftpregrasp,rightpregrasp;
00918 
00919     rightpregrasp = potww * rightrel * pregrasp;
00920     leftpregrasp = potww * leftrel * pregrasp;
00921 
00922     rightgrasp = potww * rightrel;
00923     leftgrasp = potww * leftrel;
00924 
00925     leftg = det;
00926     rightg = det;
00927     leftg.frame_id_ = "/map";
00928     rightg.frame_id_ = "/map";
00929 
00930     leftg.setOrigin(leftgrasp.getOrigin());
00931     leftg.setRotation(leftgrasp.getRotation());
00932     rightg.setOrigin(rightgrasp.getOrigin());
00933     rightg.setRotation(rightgrasp.getRotation());
00934 
00935     leftpreg.frame_id_ = "/map";
00936     rightpreg.frame_id_ = "/map";
00937     leftpreg.setOrigin(leftpregrasp.getOrigin());
00938     leftpreg.setRotation(leftpregrasp.getRotation());
00939     rightpreg.setOrigin(rightpregrasp.getOrigin());
00940     rightpreg.setRotation(rightpregrasp.getRotation());
00941 }
00942 
00943 
00945 bool getPotPose(tf::Stamped<tf::Pose> &potPose, tf::Stamped<tf::Pose> &leftpreg, tf::Stamped<tf::Pose> &leftg, tf::Stamped<tf::Pose> &rightpreg, tf::Stamped<tf::Pose> &rightg, bool via_lid=true)
00946 {
00947 
00948     ros::Time query_time = ros::Time::now();
00949 
00950     double table_height = 0.865;
00951     double pot_cyl_min = 0.02;
00952     double pot_cyl_max = 0.05;
00953     double pot_handle_min = 0.06;
00954     double pot_handle_max = 0.09;
00955     double pot_radius = 0.20 / 2.0;
00956     double pot_handle_radius_max = 0.28 / 2.0;
00957     double pot_handle_radius_min = 0.25 / 2.0;
00958     //-1.77 3.09
00959     //-2.06 2.41
00960     //btVector3 roi_min(-2.134, 2.625, table_height + pot_cyl_min);
00961     //btVector3 roi_max(-1.663, 3.215, table_height + pot_cyl_max);
00962     btVector3 roi_min(-2.06, 2.41, table_height + pot_cyl_min);
00963     btVector3 roi_max(-1.7, 3.09, table_height + pot_cyl_max);
00964 
00965     announce_box("pot search", roi_min, roi_max);
00966     announce_box("pot search", roi_min, roi_max);
00967 
00968     ros::NodeHandle node_handle;
00969 
00970     ros::Publisher cloud_pub = node_handle.advertise<sensor_msgs::PointCloud2>("/debug_cloud",0,true);
00971     ros::Publisher pose_pub = node_handle.advertise<geometry_msgs::PoseStamped>( "/cop_detection", 0 , true);
00972     ros::Publisher pose_pubr = node_handle.advertise<geometry_msgs::PoseStamped>( "/cop_detectionr", 0 , true);
00973     ros::Publisher pose_publ = node_handle.advertise<geometry_msgs::PoseStamped>( "/cop_detectionl", 0 , true);
00974 
00975 
00976     //  sensor_msgs::PointCloud2 pc = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/kinect/cloud_throttled"));
00977 
00978     //tf::Stamped<tf::Pose> net_stamped = RobotArm::getPose("/map","/openni_rgb_optical_frame");
00979     //tf::Transform net_transform;
00980     //net_transform.setOrigin(net_stamped.getOrigin());
00981     //net_transform.setRotation(net_stamped.getRotation());
00982 
00983     //sensor_msgs::PointCloud2 pct; //in map frame
00984 
00985     //pcl_ros::transformPointCloud("/map",net_transform,pc,pct);
00986     //pct.header.frame_id = "/map";
00987 
00988     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
00989 
00990     Kinect::getInstance()->getCloud(cloud, "/map", query_time);
00991     //pcl::fromROSMsg(pct, *cloud);
00992 
00993     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
00994 
00995     Eigen::Vector4f     min_pt, max_pt;
00996 
00997     // find the pot center
00998     btVector3 center;
00999 
01000 
01001     double radius;
01002     if (!via_lid)
01003     {
01005 
01006         pcl::PointCloud<pcl::PointXYZ>::Ptr pot_cyl_cloud(new pcl::PointCloud<pcl::PointXYZ>);
01007         Eigen::Vector4f min_pt, max_pt;
01008 
01009         //min_pt = Eigen::Vector4f(-2.134, 2.625, 0.95 - 0.03, 0);
01010         //max_pt = Eigen::Vector4f(-1.663, 3.215, 0.95 + 0.03, 0);
01011 
01012         min_pt = Eigen::Vector4f(roi_min.x(), roi_min.y(), roi_min.z(),1);
01013         max_pt = Eigen::Vector4f(roi_max.x(), roi_max.y(), roi_max.z(),1);
01014 
01015         boost::shared_ptr<std::vector<int> > indices( new std::vector<int> );
01016         pcl::getPointsInBox(*cloud,min_pt,max_pt,*indices);
01017         pcl::ExtractIndices<pcl::PointXYZ> ei;
01018         ei.setInputCloud(cloud);
01019         ei.setIndices(indices);
01020 
01021         ei.filter(*pot_cyl_cloud);
01022 
01023         pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
01024         pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
01025         // Create the segmentation object
01026         pcl::SACSegmentation<pcl::PointXYZ> seg;
01027         // Optional
01028         seg.setOptimizeCoefficients (true);
01029         // Mandatory
01030         seg.setModelType (pcl::SACMODEL_CIRCLE2D);
01031         seg.setMethodType (pcl::SAC_RANSAC);
01032         seg.setDistanceThreshold (0.01);
01033 
01034         seg.setInputCloud (pot_cyl_cloud);
01035 
01036         seg.setProbability(0.9999);
01037         seg.setOptimizeCoefficients(true);
01038 
01039         seg.segment (*inliers, *coefficients);
01040 
01041         if (inliers->indices.size () == 0)
01042         {
01043             PCL_ERROR ("Could not estimate a planar model for the given dataset.");
01044             return false;
01045         }
01046         else
01047         {
01048             ROS_INFO("MODEL :");
01049             //coefficients.get()
01050             ROS_INFO("%f %f %f %f", coefficients->values[0], coefficients->values[1], coefficients->values[2], coefficients->values[3]);
01051             center = btVector3(coefficients->values[0], coefficients->values[1],0.96);
01052             radius = coefficients->values[2];
01053         }
01054     }
01055 
01056     //min_pt = Eigen::Vector4f(center.x(),center.y(),1,1) - Eigen::Vector4f(.25,.25,.0, 0);
01057     //max_pt = Eigen::Vector4f(center.x(),center.y(),1,1) + Eigen::Vector4f(.25,.25,.1, 0);
01058 
01059     if (via_lid)
01060     {
01061         tf::Stamped<tf::Pose> lidPose;
01062         bool got_lid_pose = false;
01063         while (!got_lid_pose)
01064             got_lid_pose = getLidPose(lidPose);
01065         center = lidPose.getOrigin();
01066     }
01067     ROS_INFO("CENTER %f %f %f",center.x(),center.y(),center.z());
01068 
01069 
01070     min_pt = Eigen::Vector4f(center.x(),center.y(),table_height,1) - Eigen::Vector4f(pot_handle_radius_max,pot_handle_radius_max,-pot_handle_min, 0);
01071     max_pt = Eigen::Vector4f(center.x(),center.y(),table_height,1) + Eigen::Vector4f(pot_handle_radius_max,pot_handle_radius_max,pot_handle_max, 0);
01072 
01073     boost::shared_ptr<std::vector<int> > indices( new std::vector<int> );
01074     pcl::getPointsInBox(*cloud,min_pt,max_pt,*indices);
01075     pcl::ExtractIndices<pcl::PointXYZ> ei;
01076     ei.setInputCloud(cloud);
01077     ei.setIndices(indices);
01078     ei.filter(*cloud_filtered);
01079 
01080     indices->clear();
01081     double perc = 0;
01082 
01083     ROS_INFO("cloud filtered size : %zu", cloud_filtered->points.size());
01084 
01085     if (cloud_filtered->points.size() < 10)
01086         return false;
01087 
01088     ROS_INFO("center %f %f %f", center.x(),center.y(),center.z());
01089 
01090     //double pot_radius;
01091 
01092     //ros::param::param<double>("pot_radius", pot_radius, .155);
01093 
01094     ROS_INFO("pot_radius=%f", pot_radius);
01095 
01096     for (size_t k = 0; k < cloud_filtered->points.size(); ++k)
01097     {
01098         btVector3 act(cloud_filtered->points[k].x,cloud_filtered->points[k].y,cloud_filtered->points[k].z);
01099         btVector3 rel = center - act;
01100         rel.setZ(0);
01101         double radius = rel.length();
01102         //ROS_INFO("radius : %f", radius);
01103 
01104         //if (((pot_radius > 0) && (radius > pot_radius)) || ((pot_radius < 0) && (radius < fabs(pot_radius))))
01105         //if ((radius > .155) && (radius < .1925))
01106         if ((radius > pot_handle_radius_min) && (radius < pot_handle_radius_max))
01107         {
01108             indices->push_back(k);
01109             perc+=1.0;
01110         }
01111     }
01112 
01113     ROS_INFO("Percentage of points : %f/%zu= %f",perc,cloud_filtered->points.size(), perc / cloud_filtered->points.size());
01114 
01115     if (cloud_filtered->points.size() < 150)
01116     {
01117         ROS_ERROR("did not get enough pot handle candidates");
01118         return false;
01119     }
01120 
01121     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered2 (new pcl::PointCloud<pcl::PointXYZ>);
01122     ei.setInputCloud(cloud_filtered);
01123     ei.setIndices(indices);
01124     ei.setIndices(indices);
01125     ei.filter(*cloud_filtered2);
01126 
01127     double atansum = 0;
01128     double atannum = 0;
01129     for (size_t k = 0; k < cloud_filtered2->points.size(); ++k)
01130     {
01131         //y/x, i
01132         btVector3 act(cloud_filtered2->points[k].x- center.x(),cloud_filtered2->points[k].y - center.y(),0);
01133         if (act.x() != 0.0)
01134         {
01135             double at = atan2(act.y(),act.x());
01136             //ROS_INFO("atan %f %f = %f", act.x(), act.y(), at);
01137             if (at < 0)
01138                 at += M_PI;
01139             //double at = atan(act.x()/act.y());
01140             atansum += at;
01141             atannum += 1.0;
01142         }
01143     }
01144     ROS_INFO("atansum %f num %f = %f", atansum, atannum, atansum/atannum);
01145 
01146 
01147     if (atansum < 20)
01148     {
01149         ROS_ERROR("did not get enough pot handle inliers");
01150         return false;
01151     }
01152 
01153 
01154 
01155     tf::Stamped<tf::Pose> det;
01156     det.frame_id_ = "/map";
01157     det.setOrigin(center);
01158     atansum /= atannum;
01159     //if (atansum < 0)
01160 //                  atansum += M_PI;
01161     det.setRotation(btQuaternion(btVector3(0,0,1),atansum));
01162 
01163     potPose = det;
01164 
01165     ROS_INFO("angle : %f", det.getRotation().getAngle());
01166 
01167     //if (0)
01168     {
01169         sensor_msgs::PointCloud2 filtered_msg;
01170         pcl::toROSMsg(*cloud_filtered2,filtered_msg);
01171         cloud_pub.publish(filtered_msg);
01172     }
01173 
01175     calcPotGraspPoints(det, leftpreg, leftg, rightpreg, rightg);
01176 
01177     {
01178         geometry_msgs::PoseStamped det_msg;
01179         tf::poseStampedTFToMsg(det,det_msg);
01180         pose_pub.publish(det_msg);
01181 
01182         geometry_msgs::PoseStamped rightg_msg, leftg_msg;
01183         tf::poseStampedTFToMsg(rightg,rightg_msg);
01184         tf::poseStampedTFToMsg(leftg,leftg_msg);
01185 
01186         pose_pubr.publish(rightg_msg);
01187         pose_publ.publish(leftg_msg);
01188 
01189         announce("Pot", potPose);
01190 
01191         printPose("pot", potPose);
01192 
01193         announce("Pot_L", leftg);
01194 
01195         announce("Pot_R", rightg);
01196     }
01197 
01198     //pubMarker(det,radius);
01199 
01200     ROS_INFO("XXXXXXXXXXXXXXXXXXXXXXXXXX");
01201 
01202     return true;
01203 }
01204 
01205 
01206 
01207 void findBoundary1(pcl::PointCloud<pcl::PointXYZRGB>& in,pcl::PointCloud<pcl::PointXYZRGB>& edges, pcl::PointCloud<pcl::PointXYZRGB>&other, std::vector<int>& edge_indices )
01208 {
01209 
01210     pcl::KdTreeFLANN<pcl::PointXYZRGB>::Ptr knntree (new pcl::KdTreeFLANN<pcl::PointXYZRGB>  () );
01211     pcl::KdTreeFLANN<pcl::PointXYZRGB>::Ptr tree2 (new pcl::KdTreeFLANN<pcl::PointXYZRGB>  () );
01212     knntree->setInputCloud(in.makeShared());
01213 
01214     pcl::PointCloud<pcl::Normal> normals;
01215     pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
01216     ne.setSearchMethod (tree2);
01217     ne.setKSearch (20);
01218     ne.setInputCloud (in.makeShared());
01219     ne.compute (normals);
01220 
01221 //        getNormals(in, normals);          printf("normals.size     %d\n", normals.points.size());
01222 
01223     pcl::BoundaryEstimation<pcl::PointXYZRGB ,pcl::Normal, pcl::Boundary> be;
01224     be.setInputCloud(in.makeShared());
01225     be.setInputNormals(normals.makeShared());
01226     Eigen::Vector3f u = Eigen::Vector3f::Zero ();
01227     Eigen::Vector3f v = Eigen::Vector3f::Zero ();
01228     //be.compute(
01229     for (size_t idx = 0; idx < in.points.size(); idx++) //            printf("it %d\n", idx);
01230     {
01231         be.getCoordinateSystemOnPlane(normals.points[idx], u, v);
01232         pcl::Vector3fMap n4uv = normals.points[idx].getNormalVector3fMap();//               std::cout<<" n4uv "<<std::endl<<n4uv<<std::endl;
01233 
01234         std::vector<int> nb_idxs;
01235         std::vector<float> nb_dists;
01236         //               knntree->nearestKSearch(idx, 100, nb_idxs, nb_dists);
01237         knntree->radiusSearch(idx,0.01, nb_idxs, nb_dists, 100);
01238 
01239         //               pcl::Vector3fMap n4uv =
01240         normals.points[idx].getNormalVector3fMap ();
01241         bool pt = false;
01242         pt = be.isBoundaryPoint (in, idx, nb_idxs, u, v, M_PI * 0.75);//
01243         //printf("it %zu\n", idx);
01244         if (pt)
01245         {
01246             edges.points.push_back(in.points[idx]);
01247             edge_indices.push_back(idx);
01248         }
01249         else
01250         {
01251             other.points.push_back(in.points[idx]);
01252         }
01253 
01254     }
01255 }
01256 
01257 void findBoundary2(pcl::PointCloud<pcl::PointXYZRGB>& in,pcl::PointCloud<pcl::PointXYZRGB>& edges, pcl::PointCloud<pcl::PointXYZRGB>&other, std::vector<int>& edge_indices )
01258 {
01259 
01260     pcl::KdTreeFLANN<pcl::PointXYZRGB>::Ptr knntree (new pcl::KdTreeFLANN<pcl::PointXYZRGB>  () );
01261     pcl::KdTreeFLANN<pcl::PointXYZRGB>::Ptr tree2 (new pcl::KdTreeFLANN<pcl::PointXYZRGB>  () );
01262 
01263     ROS_INFO("before knntree");
01264     knntree->setInputCloud(in.makeShared());
01265     ROS_INFO("before knntree");
01266 
01267     pcl::PointCloud<pcl::Normal> normals;
01268     pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
01269     ne.setSearchMethod (tree2);
01270     ne.setKSearch (50);
01271     ne.setInputCloud (in.makeShared());
01272     ROS_INFO("before normals");
01273     ne.compute (normals);
01274     ROS_INFO("before normals");
01275 
01276 //        getNormals(in, normals);          printf("normals.size     %d\n", normals.points.size());
01277 
01278     pcl::BoundaryEstimation<pcl::PointXYZRGB ,pcl::Normal, pcl::Boundary> be;
01279     be.setInputCloud(in.makeShared());
01280     be.setInputNormals(normals.makeShared());
01281     Eigen::Vector3f u = Eigen::Vector3f::Zero ();
01282     Eigen::Vector3f v = Eigen::Vector3f::Zero ();
01283     pcl::PointCloud<pcl::Boundary> boundary;
01284 
01285     be.setSearchMethod(tree2);
01286     be.setKSearch (50);
01287 
01288     ROS_INFO("before boundary");
01289     be.compute(boundary);
01290     ROS_INFO("after boundary");
01291 
01292     for (size_t idx = 0; idx < boundary.points.size(); idx++) //            printf("it %d\n", idx);
01293     {
01294         if (boundary.points[idx].boundary_point)
01295         {
01296             edges.points.push_back(in.points[idx]);
01297             edge_indices.push_back(idx);
01298         }
01299         else
01300         {
01301             other.points.push_back(in.points[idx]);
01302         }
01303 
01304     }
01305 }
01306 
01307 int Current::manipulateKnob(int direction)
01308 {
01309 
01310     btVector3 correction(0.006,0,0);
01311 
01312     tf::Stamped<tf::Pose> knob_pre_grasp;
01313     knob_pre_grasp.frame_id_ = "/map";
01314     knob_pre_grasp.setOrigin(btVector3(0.815572 - .1, 2.204640, 1.408645));
01315     knob_pre_grasp.setRotation(btQuaternion(-0.005306, -0.020663, 0.023385, 0.999499));
01316 
01317     tf::Stamped<tf::Pose> knob_grasp;
01318     knob_grasp.frame_id_ = "/map";
01319     knob_grasp.setOrigin(btVector3(0.815572 + correction.x(), 2.204640, 1.408645));
01320     knob_grasp.setRotation(btQuaternion(-0.005306, -0.020663, 0.023385, 0.999499));
01321 
01322 
01323     tf::Stamped<tf::Pose> k3;
01324     k3.frame_id_ = "/map";
01325     k3.setOrigin(btVector3(0.811484 + correction.x(), 2.207182, 1.416687));
01326     k3.setRotation(btQuaternion(0.978148, 0.024668, 0.015891, -0.205827));
01327 
01328     tf::Stamped<tf::Pose> k3post;
01329     k3post.frame_id_ = "/map";
01330     k3post.setOrigin(btVector3(0.811484 - .1, 2.207182, 1.416687));
01331     k3post.setRotation(btQuaternion(0.978148, 0.024668, 0.015891, -0.205827));
01332 
01333 //RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(k3);
01334 
01335     //- Translation: [0.818, 2.214, 1.413]
01336     //- Rotation: in Quaternion [0.988, 0.026, -0.029, 0.148]
01337 
01338     tf::Stamped<tf::Pose> knob_rot;
01339     knob_rot.frame_id_ = "/map";
01340     knob_rot.setOrigin(btVector3(0.818 + correction.x(), 2.214, 1.413));
01341     knob_rot.setRotation(btQuaternion(0.988, 0.026, -0.029, 0.148));
01342     //knob_rot.setOrigin(btVector3(0.811130, 2.201299, 1.409015));
01343     //knob_rot.setRotation(btQuaternion(0.431538, -0.021031, 0.064140, 0.899566));
01344 
01345     tf::Stamped<tf::Pose> knob_post_rot;
01346     knob_post_rot.frame_id_ = "/map";
01347     knob_post_rot.setOrigin(btVector3(0.818 - .1, 2.214, 1.413));
01348     knob_post_rot.setRotation(btQuaternion(0.988, 0.026, -0.029, 0.148));
01349     //knob_post_rot.setOrigin(btVector3(0.811130 - .1, 2.201299, 1.409015));
01350     //knob_post_rot.setRotation(btQuaternion(0.431538, -0.021031, 0.064140, 0.899566));
01351 
01352     knob_post_rot = k3post;
01353     knob_rot = k3;
01354 
01355     RobotHead::getInstance()->lookAtThreaded("/map",0.815572, 2.204640, 1.408645);
01356 
01357     OperateHandleController::plateAttackPose();
01358 
01359     RobotDriver::moveBase4(0.206569, 2.269677, 0.021550, 0.999766);
01360 
01361     announce("Knob", knob_grasp);
01362 
01363 
01364     if (direction == 1)
01365     {
01366 
01367         bool got_it = false;
01368         while (!got_it)
01369         {
01370             RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(knob_pre_grasp);
01371             Gripper::getInstance(0)->open(.056108);
01372             RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(knob_grasp);
01373             Gripper::getInstance(0)->close();
01374             got_it = Gripper::getInstance(0)->getAmountOpen() > 0.016;
01375 
01376             ROS_INFO("manipulateKnob %f",Gripper::getInstance(0)->getAmountOpen());
01377         }
01378 
01379 
01380         RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(knob_rot);
01381         Gripper::getInstance(0)->open(.056108);
01382         RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(knob_post_rot);
01383     }
01384     if (direction == -1)
01385     {
01386 
01387         bool got_it = false;
01388         while (!got_it)
01389         {
01390             Gripper::getInstance(0)->open(.056108);
01391             RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(knob_post_rot);
01392             RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(knob_rot);
01393             Gripper::getInstance(0)->close();
01394             got_it = Gripper::getInstance(0)->getAmountOpen() > 0.016;
01395 
01396             ROS_INFO("manipulateKnob %f",Gripper::getInstance(0)->getAmountOpen());
01397         }
01398 
01399         RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(knob_grasp);
01400         Gripper::getInstance(0)->open(.056108);
01401         RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(knob_pre_grasp);
01402 
01403     }
01404     return 0;
01405 }
01406 
01407 int Current::getPotOut_openDrawer(int jump)
01408 {
01409 
01410     Torso *torso = Torso::getInstance();
01411 
01412     double lift = 0.29;
01413 
01414     RobotHead::getInstance()->lookAtThreaded("/map",-1.636, 2.664, 0.354);
01415 
01416     boost::thread t(&Torso::pos, torso, .29);
01417 
01418     boost::thread t1(&OperateHandleController::plateTuckPose);
01419 
01420     RobotDriver::moveBase4(-1.011, 2.110,  0.999, -0.033);
01421 
01422     t1.join();
01423     t.join();
01424 
01425     announce("Handle",Geometry::make_pose(-1.636, 2.664, 0.354 + lift, -0.023, 0.721, 0.691, -0.054,"/map"));
01426 
01427     RobotHead::getInstance()->lookAtThreaded("/r_gripper_tool_frame",0,0,0);
01428 
01429     // open drawer
01430     Gripper::getInstance(0)->openThreaded();
01431     RobotArm::getInstance(0)->universal_move_toolframe_ik(-1.536, 2.664, 0.354 + lift, -0.023, 0.721, 0.691, -0.054, "/map");
01432     RobotArm::getInstance(0)->universal_move_toolframe_ik(-1.636, 2.664, 0.354 + lift, -0.023, 0.721, 0.691, -0.054,"/map");
01433     Gripper::getInstance(0)->close();
01434 
01435     ROS_ERROR("getPotOut_openDrawerAMOUNT OPEN: %f", Gripper::getInstance(1)->getAmountOpen());
01436 
01437     ros::Duration(0.5).sleep();
01438 
01439     if (Gripper::getInstance(0)->getAmountOpen() < 0.01)
01440     {
01441         RobotArm::getInstance(0)->universal_move_toolframe_ik(-1.536, 2.664, 0.354 + lift, -0.023, 0.721, 0.691, -0.054, "/map");
01442         OperateHandleController::plateTuckPoseRight();
01443         return Current::getPotOut_openDrawer(jump);
01444     }
01445 
01446     RobotArm::getInstance(0)->universal_move_toolframe_ik(-1.159, 2.680, 0.348 + lift, -0.043, 0.708, 0.705, -0.004,"/map");
01447     Gripper::getInstance(0)->openThreaded();
01448 
01450 
01451     //RobotHead::getInstance()->lookAtThreaded("/map",-1.373, 2.951, 0.70);
01452     RobotHead::getInstance()->lookAtThreaded("/map",-1.636, 2.664, 0.354);
01453 
01454 
01455     RobotDriver::moveBase4(-0.756, 2.093,1,0);
01456 
01457     return 0;
01458 }
01459 
01460 
01461 int Current::getPotOut_pickPlacePot(int jump)
01462 {
01463 
01464     double modifier = -.06;
01465 
01466     RobotHead::getInstance()->lookAtThreaded("/map",-1.636, 2.664, 0.354);
01467 
01468 
01469     boost::thread t2(&OperateHandleController::plateTuckPose);
01470 
01471     Torso *torso = Torso::getInstance();
01472 
01473     double lift = 0.29;
01474 
01475     boost::thread tt(&Torso::pos, torso,  + lift);
01476     tt.join();
01477 
01478     lift += modifier;
01479 
01480     Gripper::getInstance(0)->closeThreaded();
01481     Gripper::getInstance(1)->closeThreaded();
01482     RobotDriver::moveBase4(-0.812520, 2.580165, 0.999515, -0.031016);
01483 
01484     tf::Stamped<tf::Pose> ravoid, rappr, lappr, rpick, lpick, rplace, lplace, rplaceup, lplaceup;
01485 
01486     rpick.frame_id_ = "/map";
01487     rpick.setOrigin(btVector3(-1.373, 2.951, 0.280  + lift));
01488     rpick.setRotation(btQuaternion(-0.661, 0.035, 0.748, 0.056));
01489     rappr = rpick;
01490     rappr.setOrigin(rappr.getOrigin() + btVector3(0,0,0.3));
01491 
01492     lpick.frame_id_ = "/map";
01493     lpick.setOrigin(btVector3(-1.403, 2.711, 0.290  + lift));
01494     lpick.setRotation(btQuaternion(-0.627, 0.039, 0.778, -0.019));
01495     lappr = lpick;
01496     lappr.setOrigin(lappr.getOrigin() + btVector3(0,0,0.3));
01497 
01498 
01499     //tf::Stamped<tf::Pose> rpick;
01500     rpick.frame_id_ = "/map";
01501     rpick.setOrigin(btVector3(-1.378332, 2.929956, 0.513234));
01502     rpick.setRotation(btQuaternion(-0.659041, 0.029168, 0.750170, 0.045374));
01503 
01504 //RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(rpick);
01505 
01506 //tf::Stamped<tf::Pose> lpick;
01507     lpick.frame_id_ = "/map";
01508     lpick.setOrigin(btVector3(-1.399699, 2.720151, 0.52270));
01509     lpick.setRotation(btQuaternion(-0.625505, 0.034901, 0.779373, -0.010184));
01510 
01511 //RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(rpick);
01512 
01513 //RobotArm::moveBothArms(lpick,rpick);
01514 
01515 
01516     ravoid = rpick;
01517     ravoid.setOrigin(btVector3(-1.223, 2.960, 0.593  + lift));
01518     ravoid.setRotation(btQuaternion(-0.137, 0.840, -0.069, 0.521));
01519 
01520     tf::Stamped<tf::Pose> pot = lpick;
01521     pot.setOrigin((lpick.getOrigin() + rpick.getOrigin()) * 0.5);
01522 
01523     announce("Pot", pot);
01524 
01525     //OperateHandleController::closeGrippers(false);
01526 
01527     RobotHead::getInstance()->lookAtThreaded("/r_gripper_tool_frame",0,0,0);
01528 
01529     RobotArm::getInstance()->startTrajectory(RobotArm::getInstance(0)->goalTraj(-0.91654420464731023, 0.55680337139226421, -1.0197552147091691, -1.6391094508469026, -33.205223487262245, -1.5794848996581, 26.217879010657214),true);
01530 
01531     RobotArm::moveBothArms(lappr, rappr);
01532 
01533     //RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(rappr);
01534 
01535     OperateHandleController::openGrippers(true);
01536 
01537     RobotArm::moveBothArms(lpick, rpick);
01538 
01539     OperateHandleController::closeGrippers();
01540 
01541     //exit(0);
01542 
01543     boost::thread t3(&Torso::up, torso);
01544 
01545     OperateHandleController::spinnerL(0,0,0.5);
01546 
01547     RobotHead::getInstance()->lookAtThreaded("/map",-1.866, 3.027, 1);
01548 
01549 
01550     t3.join();
01551 
01552     //OperateHandleController::spinnerL(0,0,0.1);
01553 
01554     //RobotDriver::moveBase4(-1.235, 2.64,1,-0.029); // this was the one used
01555 
01556     rplace.frame_id_ = "/map";
01557     //rplace.setOrigin(btVector3(-1.866, 3.027, 1));
01558     //rplace.setRotation(btQuaternion(-0.652, 0.052, 0.729, 0.200));
01559     //rplace.setOrigin(btVector3(-1.796 - 0.02, 2.993, 0.997 + modifier));
01560     rplace.setOrigin(btVector3(-1.796 - 0.02, 2.993 + 0.02, 0.997 + modifier));
01561     rplace.setRotation(btQuaternion(-0.582, 0.259, 0.7019, 0.317));
01562     rplaceup = rplace;
01563     rplaceup.setOrigin(rplaceup.getOrigin() + btVector3(0,0,0.1));
01564 
01565     lplace.frame_id_ = "/map";
01566     //lplace.setOrigin(btVector3(-1.920, 2.802, 1));
01567     //lplace.setRotation(btQuaternion(-0.639, 0.028, 0.767, 0.052));
01568     //lplace.setOrigin(btVector3(-1.946 - 0.02 , 2.817, 1.010 + modifier));
01569     lplace.setOrigin(btVector3(-1.946 - 0.02, 2.817 + 0.02, 1.010 + modifier));
01570     lplace.setRotation(btQuaternion(-0.629, 0.166, 0.731, 0.208));
01571     lplaceup = lplace;
01572     lplaceup.setOrigin(lplaceup.getOrigin() + btVector3(0,0,0.1));
01573 
01574     // TODO findbasemove
01575     {
01576         tf::Stamped<tf::Pose> result;
01577         std::vector<int> arm;
01578         std::vector<tf::Stamped<tf::Pose> > goal;
01579         arm.push_back(1);
01580         arm.push_back(1);
01581         arm.push_back(0);
01582         arm.push_back(0);
01583         goal.push_back(lplace);
01584         goal.push_back(lplaceup);
01585         goal.push_back(rplace);
01586         goal.push_back(rplaceup);
01587         //bool RobotArm::findBaseMovement(tf::Stamped<tf::Pose> &result, std::vector<int> arm, std::vector<tf::Stamped<tf::Pose> > goal, bool drive, bool reach)
01588 
01589         RobotArm::findBaseMovement(result, arm, goal, true, false);
01590     }
01591 
01592 
01593     RobotArm::moveBothArms(lplaceup,rplaceup);
01594 
01595     RobotArm::moveBothArms(lplace,rplace);
01596 
01597     OperateHandleController::openGrippers();
01598 
01599     RobotArm::moveBothArms(lplaceup,rplaceup);
01600 
01601 
01602     RobotDriver::moveBase4(-0.868230, 2.383544, 0.997340, 0.072885);
01603 
01604     OperateHandleController::plateAttackPose();
01605 
01606     OperateHandleController::plateTuckPose();
01607 
01608     tf::Stamped<tf::Pose> predrawerclose;
01609     predrawerclose.frame_id_ = "/map";
01610     predrawerclose.setOrigin(btVector3(-1.420218, 2.811923, 0.620214));
01611     predrawerclose.setRotation(btQuaternion(0.027181, -0.678301, 0.732514, -0.050909));
01612 
01613     RobotHead::getInstance()->lookAtThreaded("/r_gripper_tool_frame",0,0,0);
01614 
01615 
01616     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(predrawerclose);
01617 
01618     tf::Stamped<tf::Pose> postdrawerclose;
01619     postdrawerclose.frame_id_ = "/map";
01620     //postdrawerclose.setOrigin(btVector3(-1.660690, 2.574179, 0.667216));
01621     postdrawerclose.setOrigin(btVector3(-1.610690, 2.574179, 0.667216));
01622     postdrawerclose.setRotation(btQuaternion(0.018551, -0.684924, 0.724965, -0.070439));
01623 
01624     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(postdrawerclose);
01625 
01626     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(predrawerclose);
01627 
01628     OperateHandleController::plateTuckPose();
01629 
01630     return 0;
01631 }
01632 
01633 void getPotOut(int jump)
01634 {
01635     RobotHead::getInstance()->lookAtThreaded("/r_gripper_tool_frame",0,0,0);
01636 
01637     switch (jump)
01638     {
01639     case 0:
01640         Current::getPotOut_openDrawer(jump);
01641     case 1:
01642         Current::getPotOut_pickPlacePot(jump);
01643     }
01644 }
01645 
01646 int Current::openDrawerUnderOven(int jump)
01647 {
01648     RobotHead::getInstance()->lookAtThreaded("/map",0.527460,2.304037,0.832586);
01649 
01650     Gripper::getInstance(1)->openThreaded();
01651     Gripper::getInstance(0)->openThreaded();
01652 
01653     Torso *torso = Torso::getInstance();
01654 
01655     double lift = 0.29;
01656 
01657 
01658     boost::thread t(&Torso::pos, torso, lift);
01659 
01660     boost::thread t1(&OperateHandleController::plateTuckPose);
01661 
01662     RobotDriver::moveBase4(-0.031 - 0.1, 2.211,   0.034, 0.999);
01663 
01664     t1.join();
01665 
01666     announce("Handle",Geometry::make_pose(0.77, 2.367, 0.846, -0.719, -0.023, 0.032, 0.693, "/map"));
01667 
01668     RobotArm::getInstance(1)->universal_move_toolframe_ik(0.7, 2.367, 0.846, -0.719, -0.023, 0.032, 0.693, "/map");
01669     //RobotArm::getInstance(1)->universal_move_toolframe_ik(0.759, 2.367, 0.846, -0.719, -0.023, 0.032, 0.693, "/map");
01670     RobotArm::getInstance(1)->universal_move_toolframe_ik(0.77, 2.367, 0.846, -0.719, -0.023, 0.032, 0.693, "/map");
01671     //RobotArm::getInstance(1)->universal_move_toolframe_ik(0.77, 2.367, 0.846, -0.719, -0.023, 0.032, 0.693, "/map");
01672     Gripper::getInstance(1)->close();
01673 
01674 
01675     ros::Duration(0.5).sleep();
01676 
01677     ROS_ERROR("openFavouriteopenDrawerUnderOvenDrawer OPEN: %f", Gripper::getInstance(1)->getAmountOpen());
01678 
01679     if (Gripper::getInstance(1)->getAmountOpen() < 0.01)
01680     {
01681         RobotArm::getInstance(1)->universal_move_toolframe_ik(0.7, 2.367, 0.846, -0.719, -0.023, 0.032, 0.693, "/map");
01682         OperateHandleController::plateTuckPoseRight();
01683         return Current::openDrawerUnderOven(jump);
01684     }
01685 
01686 
01687     RobotArm::getInstance(1)->universal_move_toolframe_ik( 0.291, 2.335, 0.841,  -0.732, -0.008, 0.039, 0.680 , "/map"); // drawer_open
01688 
01689     return 0;
01690 }
01691 
01692 
01693 int Current::getLidOut(int jump)
01694 {
01695 
01696     tf::Stamped<tf::Pose> lid;
01697     lid.frame_id_ = "/map";
01698     lid.setOrigin(btVector3(0.527460,2.304037,0.832586));
01699     lid.setRotation(btQuaternion(-0.155035,0.112869,0.756283,0.625509));
01700 
01701     announce("Lid", lid);
01702 
01703     tf::Stamped<tf::Pose> highlid = lid;
01704     highlid.setOrigin(lid.getOrigin() + btVector3(0,0,0.2));
01705 
01706     tf::Stamped<tf::Pose> prelid;
01707     prelid.frame_id_ = "/map";
01708     prelid.setOrigin(btVector3(0.542697,2.212567,0.871439));
01709     prelid.setRotation(btQuaternion(-0.155615,0.112088,0.756168,0.625644));
01710 
01711     tf::Stamped<tf::Pose> lid_pre_grip;
01712     lid_pre_grip.frame_id_ = "/map";
01713     lid_pre_grip.setOrigin(btVector3(0.543219, 2.230091, 0.836666));
01714     lid_pre_grip.setRotation(btQuaternion(-0.132676, 0.078832, 0.814762, 0.558879));
01715 
01716     tf::Stamped<tf::Pose> lid_final_grasp;
01717     lid_final_grasp.frame_id_ = "/map";
01718     lid_final_grasp.setOrigin(btVector3(0.500972, 2.299158, 0.834633));
01719     lid_final_grasp.setRotation(btQuaternion(-0.108869, 0.051036, 0.840173, 0.528821));
01720 
01721     Gripper::getInstance(0)->openThreaded();
01722 
01723     OperateHandleController::plateAttackPoseRight();
01724 
01725     RobotArm::getInstance(0)->raise_elbow = true;
01726 
01727     RobotArm::getInstance(0)->preset_angle = 2.7;
01728 
01729     double time = RobotArm::getInstance(0)->time_to_target;
01730     RobotArm::getInstance(0)->time_to_target = 0.5;
01731     RobotArm::getInstance(1)->time_to_target = 0.5;
01732 
01733 
01734     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(highlid);
01735 
01736     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(lid_pre_grip);
01737 
01738     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(lid_final_grasp);
01739 
01740 
01741     Gripper::getInstance(0)->close();
01742 
01743 
01745 
01746     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(highlid);
01747 
01748 
01749     tf::Stamped<tf::Pose> lidcarry;
01750     lidcarry.frame_id_ = "base_link";
01751     lidcarry.setOrigin(btVector3(0.465009,-0.200021,1.1));
01752     lidcarry.setRotation(btQuaternion(-0.151885,0.117935,0.735543,0.649614));
01753 
01754     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(lidcarry);
01755 
01756     RobotArm::getInstance(0)->time_to_target = time;
01757     RobotArm::getInstance(1)->time_to_target = time;
01758 
01759 
01760     RobotArm::getInstance(0)->raise_elbow = false;
01761 
01762     {
01763         tf::Stamped<tf::Pose> lidcarry;
01764         lidcarry.frame_id_ = "base_link";
01765         lidcarry.setOrigin(btVector3(0.088186, -0.539420, 1.110697));
01766         lidcarry.setRotation(btQuaternion(-0.061714, 0.110614, 0.436300, 0.890841));
01767 
01768         boost::thread t(&RobotArm::universal_move_toolframe_ik_pose,RobotArm::getInstance(0),lidcarry);
01769     }
01770     return 0;
01771 }
01772 
01773 int Current::getBowlOut(int jump)
01774 {
01775     RobotHead::getInstance()->lookAtThreaded("/map",0.614, 1.945, 0.84);
01776 
01777     //RobotDriver::moveBase4(-0.031 - 0.1, 2.211,   0.034, 0.999); // bowl out
01778 
01779     Gripper::getInstance(1)->openThreaded();
01780 
01781     if (jump < 100)
01782     {
01783 
01784         tf::Stamped<tf::Pose> act = RobotArm::getInstance(1)->getToolPose("/map");
01785         //act.getOrigin() += btVector3(-.1,0,0);
01786         act.getOrigin() += btVector3(0,+.25,0);
01787 
01788         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(act);
01789     }
01790 
01791 
01792     OperateHandleController::plateAttackPoseLeft();
01793 
01794     RobotArm::getInstance(1)->raise_elbow = true;
01795 
01796     btVector3 min(0.614, 1.945, 0.84);
01797     btVector3 max(0.796, 2.245, 0.92);
01798     btVector3 center;
01799     double radius;
01800     getCircle(min, max, center, radius);
01801 
01802     ROS_INFO("center %f %f %f", center.x(), center.y(), center.z());
01803 
01804     {
01805         tf::Stamped<tf::Pose> bowl;
01806         bowl.setOrigin(center);
01807         bowl.setRotation(btQuaternion(0,0,0,1));
01808         bowl.frame_id_ = "/map";
01809 
01810         announce("Bowl", bowl);
01811     }
01812 
01813     tf::Stamped<tf::Pose> grasp;
01814     grasp.setOrigin(center + btVector3(0,0.08,-0.035));
01815     grasp.setRotation(btQuaternion(-0.128, 0.591, -0.167, 0.778));
01816     grasp.frame_id_ = "/map";
01817 
01818     tf::Stamped<tf::Pose> grasphigh = grasp;
01819     grasphigh.getOrigin() += btVector3(0,0,0.1);
01820 
01821     tf::Stamped<tf::Pose> grasphighpre = grasp;
01822     grasphighpre.getOrigin() += btVector3(-.2,.1,0.1);
01823 
01824     ROS_INFO("grasphigh %f %f %f", grasphigh.getOrigin().x(), grasphigh.getOrigin().y(), grasphigh.getOrigin().z());
01825 
01826     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(grasphighpre);
01827     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(grasphigh);
01828     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(grasp);
01829 
01830     Gripper::getInstance(1)->close();
01831 
01832     ros::Duration(0.5).sleep();
01833 
01834     ROS_ERROR("getBowlOut LID AMOUNT OPEN: %f", Gripper::getInstance(1)->getAmountOpen());
01835 
01836     if (Gripper::getInstance(1)->getAmountOpen() < 0.005)
01837     {
01838         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(grasphigh);
01839         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(grasphighpre);
01840         return Current::getBowlOut(101);
01841     }
01842 
01843     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(grasphigh);
01844 
01845     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(grasphighpre);
01846 
01847     RobotArm::getInstance(1)->raise_elbow = false;
01848 
01849     //RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(grasp);
01850 
01851     //Gripper::getInstance(1)->open();
01852     //RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(grasphigh);
01853 
01854     //OperateHandleController::plateAttackPose();
01855 
01856     tf::Stamped<tf::Pose> bowl_carry;
01857     bowl_carry.frame_id_ = "base_link";
01858     bowl_carry.setOrigin(btVector3(0.209078, 0.4470, 1.053729));
01859     bowl_carry.setRotation(btQuaternion(-0.153977, 0.579149, -0.128882, 0.790106));
01860 
01861     //RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(bowl_carry);
01862 
01863     boost::thread t(&RobotArm::universal_move_toolframe_ik_pose,RobotArm::getInstance(1),bowl_carry);
01864 
01865 
01866 
01867     return 0;
01868 }
01869 
01870 int Current::closeDrawerUnderOven(int jump)
01871 {
01872     /*RobotArm::getInstance(1)->universal_move_toolframe_ik(0.759, 2.367, 0.846, -0.719, -0.023, 0.032, 0.693, "/map");
01873     Gripper::getInstance(1)->open();
01874     RobotArm::getInstance(1)->universal_move_toolframe_ik(0.7, 2.367, 0.846, -0.719, -0.023, 0.032, 0.693, "/map");
01875 
01876     OperateHandleController::plateTuckPoseLeft();*/
01877 
01878     {
01879         RobotDriver::moveBase4(-0.031 - 0.1, 2.211,   0.034, 0.999); // bowl out
01880 
01881         tf::Stamped<tf::Pose> bowl_carry;
01882         bowl_carry.frame_id_ = "base_link";
01883         bowl_carry.setOrigin(btVector3(0.209078, 0.4470, 1.053729));
01884         bowl_carry.setRotation(btQuaternion(-0.153977, 0.579149, -0.128882, 0.790106));
01885 
01886         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(bowl_carry);
01887 
01888         //boost::thread t(&RobotArm::universal_move_toolframe_ik_pose,RobotArm::getInstance(1),bowl_carry);
01889         RobotHead::getInstance()->lookAtThreaded("/l_gripper_tool_frame",0, 0, .1);
01890 
01891 
01892         tf::Stamped<tf::Pose> pre_slam;
01893         pre_slam.frame_id_ = "/map";
01894         pre_slam.setOrigin(btVector3(0.154007, 2.283781, 0.764292));
01895         pre_slam.setRotation(btQuaternion(0.224605, 0.621592, -0.546879, 0.513906));
01896 
01897         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(pre_slam);
01898 
01899         tf::Stamped<tf::Pose> post_slam;
01900         post_slam.frame_id_ = "/map";
01901         //post_slam.setOrigin(btVector3(0.693888, 2.316993, 0.792044));
01902         post_slam.setOrigin(btVector3(0.643888, 2.316993, 0.792044));
01903         post_slam.setRotation(btQuaternion(0.224605, 0.621592, -0.546879, 0.513906));
01904 
01905         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(post_slam);
01906 
01907         //RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(bowl_carry);
01908 
01909         boost::thread t(&RobotArm::universal_move_toolframe_ik_pose,RobotArm::getInstance(1),bowl_carry);
01910 
01911     }
01912     return 0;
01913 }
01914 
01915 int Current::pourPopCorn(int jump)
01916 {
01917 
01918     double modifier = -.06; // small pot vs high pot
01919 
01920     RobotHead::getInstance()->lookAtThreaded("/map",-1.906074,2.872285,1.171132);
01921 
01922 
01923     //RobotDriver::moveBase4(-1.187991, 2.401353, 0.999250, -0.038681);
01924     RobotDriver::moveBase4(-1.230, 2.782, 0.999, -0.042); // move base to island
01925 
01926 //            t.join();
01927 
01928     tf::Stamped<tf::Pose> potPose, pre_grasp_r, grasp_r, pre_grasp_l, grasp_l;
01929     bool gotPotPose = false;
01930     while (!gotPotPose)
01931     {
01932         //gotPotPose = getPotPose(potPose, pre_grasp_l, grasp_l, pre_grasp_r, grasp_r);
01933         gotPotPose = getPotPose(potPose, pre_grasp_l, grasp_l, pre_grasp_r, grasp_r);
01934     }
01935 
01936     // pour the popcorn
01937     {
01938 
01939         RobotHead::getInstance()->lookAtThreaded("l_gripper_tool_frame",0,0,0);
01940 
01941         {
01942             tf::Stamped<tf::Pose> bowl_carry;
01943             bowl_carry.frame_id_ = "base_link";
01944             bowl_carry.setOrigin(btVector3(0.209078, 0.4470, 1.053729));
01945             bowl_carry.setRotation(btQuaternion(-0.153977, 0.579149, -0.128882, 0.790106));
01946 
01947             RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(bowl_carry);
01948         }
01949 
01950         tf::Stamped<tf::Pose> poura = RobotArm::getInstance(1)->getToolPose("/map");
01951         poura.getOrigin().setX(potPose.getOrigin().x()) ;
01952         poura.getOrigin().setY(potPose.getOrigin().y() - .1);
01953         poura.getOrigin().setZ(poura.getOrigin().getZ() + .1 + modifier);
01954 
01955 
01956         tf::Stamped<tf::Pose> bowl_pour_b;
01957         bowl_pour_b.frame_id_ = "/map";
01958         bowl_pour_b.setOrigin(btVector3(-1.892476 + 1.91 + potPose.getOrigin().x(), 2.942343 - 2.927 + potPose.getOrigin().y(), 1.180554 + .1 + modifier));
01959         bowl_pour_b.setRotation(btQuaternion(-0.167260, 0.680787, 0.601136, -0.383652));
01960 
01961 
01962         RobotArm::getInstance(0)->raise_elbow = true;
01963 
01964         // TODO findbasemove
01965         {
01966             tf::Stamped<tf::Pose> result;
01967             std::vector<int> arm;
01968             std::vector<tf::Stamped<tf::Pose> > goal;
01969             arm.push_back(1);
01970             arm.push_back(1);
01971             goal.push_back(bowl_pour_b);
01972             goal.push_back(poura);
01973             //bool RobotArm::findBaseMovement(tf::Stamped<tf::Pose> &result, std::vector<int> arm, std::vector<tf::Stamped<tf::Pose> > goal, bool drive, bool reach)
01974 
01975             RobotArm::findBaseMovement(result, arm, goal, true, false);
01976         }
01977 
01978         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(poura);
01979 
01980         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(bowl_pour_b);
01981 
01982         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(poura);
01983 
01984         RobotArm::getInstance(0)->raise_elbow = false;
01985 
01986         tf::Stamped<tf::Pose> put_down;
01987         put_down.frame_id_ = "/map";
01988         put_down.setOrigin(btVector3(-1.610931, 1.983166 + .12, 0.929598));
01989         put_down.setRotation(btQuaternion(-0.353326, -0.545381, 0.643021, -0.405270));
01990 
01991 
01992         tf::Stamped<tf::Pose> put_down_inter;
01993         put_down_inter.frame_id_ = "/map";
01994         put_down_inter.setOrigin(0.5 * (put_down.getOrigin() + bowl_pour_b.getOrigin()));
01995         put_down_inter.setRotation(btQuaternion(-0.353326, -0.545381, 0.643021, -0.405270));
01996 
01997         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(put_down_inter);
01998 
01999         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(put_down);
02000 
02001         Gripper::getInstance(1)->openThreaded();
02002 
02003         ros::Duration(1).sleep();
02004 
02005         tf::Stamped<tf::Pose> put_down_high;
02006         put_down_high.frame_id_ = "/map";
02007         put_down_high.setOrigin(btVector3(-1.612672, 1.985903 + .12, 0.931158 + .1));
02008         put_down_high.setRotation(btQuaternion(-0.353345, -0.544065, 0.643510, -0.406245));
02009 
02010         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(put_down_high);
02011     }
02012     return 0;
02013 
02014 }
02015 
02016 /*
02017 int Current::putLidOn(int jump)
02018 {
02019 
02020     tf::Stamped<tf::Pose> lidcarry;
02021     lidcarry.frame_id_ = "base_link";
02022     lidcarry.setOrigin(btVector3(0.088186, -0.539420, 1.110697));
02023     lidcarry.setRotation(btQuaternion(-0.061714, 0.110614, 0.436300, 0.890841));
02024 
02025     double modifier = -.06; // small pot vs high pot
02026 
02027     //pr2_controllers_msgs::JointTrajectoryGoal goalAT = RobotArm::getInstance(0)->goalTraj(-2.13282, -0.3, -1.209236618713309, -1.8705012213796224, -0.2192995424769604, -1.2529613898971834, 8.2846172962242548,1);
02028     //boost::thread t2T(&RobotArm::startTrajectory, RobotArm::getInstance(0), goalAT,true);
02029     //boost::thread t1(&RobotArm::startTrajectory,RobotArm::getInstance(0),tg);
02030 
02031     RobotHead::getInstance()->lookAtThreaded("/map",-1.906074,2.872285,1.171132);
02032 
02033     RobotDriver::moveBase4(-1.230, 2.782, 0.999, -0.042); // move base to island
02034 
02035 
02036     //tf::Stamped<tf::Pose> curr = RobotArm::getInstance(0)->getToolPose("base_link");
02037     //curr.getOrigin() += btVector3(0,0,.2);
02038     //boost::thread t9(&RobotArm::universal_move_toolframe_ik_pose, RobotArm::getInstance(0), curr);
02039     RobotArm::getInstance(0)->raise_elbow = true;
02040 
02041     tf::Stamped<tf::Pose> potPose, pre_grasp_r, grasp_r, pre_grasp_l, grasp_l;
02042     bool gotPotPose = false;
02043 
02044     discardKinectFrame();
02045     discardKinectFrame();
02046 
02047     while (!gotPotPose)
02048     {
02049         gotPotPose = getPotPose(potPose, pre_grasp_l, grasp_l, pre_grasp_r, grasp_r);
02050     }
02051 
02052     RobotHead::getInstance()->lookAtThreaded("r_gripper_tool_frame",0,0,0);
02053 
02054     //t9.join();
02055 
02056     double adj_x = 0.005;
02057     double adj_y = -.015;
02058 
02059     tf::Stamped<tf::Pose> goal;
02060     goal = RobotArm::getInstance(0)->getToolPose("/map");
02061     //goal.setOrigin(btVector3(potPose.getOrigin().x() + adj_x ,potPose.getOrigin().y() + adj_y,goal.getOrigin().z()));
02062 
02063     //RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(goal);
02064 
02065     //goal.setOrigin(btVector3(potPose.getOrigin().x() - .0,potPose.getOrigin().y() -.02 ,1.06 + modifier));
02066     goal.setOrigin(btVector3(potPose.getOrigin().x() + adj_x,potPose.getOrigin().y() + adj_y,1.06 + modifier - 0.01));
02067 
02068     tf::Stamped<tf::Pose> goal_base_rot = Geometry::getPoseIn("base_link", goal);
02069     goal_base_rot.setRotation(btQuaternion(0,0,0,1));
02070     goal_base_rot = Geometry::getPoseIn("/map", goal_base_rot);
02071 
02072     tf::Stamped<tf::Pose> goalappr_rot = goal;
02073     tf::Stamped<tf::Pose> goalappr_shift= goal;
02074 
02075     goalappr_rot =  Geometry::rotateAroundPose(goal,goal_base_rot,0,0.15,0);
02076     goalappr_rot.getOrigin() += btVector3(0.02,0,0.04);
02077 
02078     tf::Stamped<tf::Pose> goalappr_rot_high = goalappr_rot;
02079 
02080     //tf::Stamped<tf::Pose> goalappr_rot_shift = goalappr_rot;
02081 
02082     goalappr_rot_high.getOrigin() += btVector3(0,0,0.1);
02083 
02084     goalappr_shift.getOrigin() += btVector3(-0.05,0,0);
02085 
02086     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(goalappr_rot_high);
02087     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(goalappr_rot);
02088     //RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(goalappr_rot_shift);
02089     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(goalappr_shift);
02090     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(goal);
02091 
02092     tf::Stamped<tf::Pose> pot_pushed_to_cooker;
02093     pot_pushed_to_cooker.frame_id_ = "/map";
02094     pot_pushed_to_cooker.setOrigin(btVector3(-1.901817, 2.941642, 1.015725));
02095     pot_pushed_to_cooker.setRotation(btQuaternion(-0.117109, -0.076221, 0.877845, -0.458110));
02096 
02097     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(pot_pushed_to_cooker);
02098 
02099     //pot_pushed_to_cooker.getOrigin() += btVector3(0,0,0.1);
02100 
02101     //RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(pot_pushed_to_cooker);
02102 
02103     //goal.setOrigin(btVector3(potPose.getOrigin().x() + adj_x,potPose.getOrigin().y() + adj_y,1.06 + modifier - 0.02 + 0.1));
02104 
02105     //RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(goal);
02106 
02107     //RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(lidcarry);
02108 
02109     RobotArm::getInstance(0)->raise_elbow = false;
02110 
02111     //exit(0);
02112 
02113     Gripper::getInstance(0)->open();
02114 
02115     goal.setOrigin(btVector3(potPose.getOrigin().x() - .0,potPose.getOrigin().y() -.02 ,goal.getOrigin().z() + .1));
02116 
02117     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(goal);
02118 
02119     RobotArm::getInstance((0))->universal_move_toolframe_ik_pose(lidcarry);
02120     return 0;
02121 }
02122 */
02123 
02124 int Current::putLidOn(int jump)
02125 {
02126 
02127     tf::Stamped<tf::Pose> lidcarry;
02128     lidcarry.frame_id_ = "base_link";
02129     lidcarry.setOrigin(btVector3(0.088186, -0.539420, 1.110697));
02130     lidcarry.setRotation(btQuaternion(-0.061714, 0.110614, 0.436300, 0.890841));
02131 
02132     double modifier = -.06; // small pot vs high pot
02133 
02134     //pr2_controllers_msgs::JointTrajectoryGoal goalAT = RobotArm::getInstance(0)->goalTraj(-2.13282, -0.3, -1.209236618713309, -1.8705012213796224, -0.2192995424769604, -1.2529613898971834, 8.2846172962242548,1);
02135     //boost::thread t2T(&RobotArm::startTrajectory, RobotArm::getInstance(0), goalAT,true);
02136     //boost::thread t1(&RobotArm::startTrajectory,RobotArm::getInstance(0),tg);
02137 
02138     RobotHead::getInstance()->lookAtThreaded("/map",-1.906074,2.872285,1.171132);
02139 
02140     RobotDriver::moveBase4(-1.230, 2.782, 0.999, -0.042); // move base to island
02141 
02142     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(lidcarry);
02143 
02144     Gripper::getInstance(0)->closeThreaded();
02145 
02146 
02147     //tf::Stamped<tf::Pose> curr = RobotArm::getInstance(0)->getToolPose("base_link");
02148     //curr.getOrigin() += btVector3(0,0,.2);
02149     //boost::thread t9(&RobotArm::universal_move_toolframe_ik_pose, RobotArm::getInstance(0), curr);
02150     RobotArm::getInstance(0)->raise_elbow = true;
02151 
02152     tf::Stamped<tf::Pose> potPose, pre_grasp_r, grasp_r, pre_grasp_l, grasp_l;
02153     bool gotPotPose = false;
02154 
02155     discardKinectFrame();
02156     discardKinectFrame();
02157 
02158     while (!gotPotPose)
02159     {
02160         gotPotPose = getPotPose(potPose, pre_grasp_l, grasp_l, pre_grasp_r, grasp_r);
02161     }
02162 
02163     //RobotHead::getInstance()->lookAtThreaded("r_gripper_tool_frame",0,0,0);
02164 
02165     //t9.join();
02166 
02167     double adj_x = 0.005;
02168     double adj_y = -.015;
02169 
02170     tf::Stamped<tf::Pose> goal;
02171     goal = RobotArm::getInstance(0)->getToolPose("/map");
02172     //goal.setOrigin(btVector3(potPose.getOrigin().x() + adj_x ,potPose.getOrigin().y() + adj_y,goal.getOrigin().z()));
02173 
02174     //RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(goal);
02175 
02176     //goal.setOrigin(btVector3(potPose.getOrigin().x() - .0,potPose.getOrigin().y() -.02 ,1.06 + modifier));
02177     goal.setOrigin(btVector3(potPose.getOrigin().x() + adj_x,potPose.getOrigin().y() + adj_y,1.06 + modifier - 0.01));
02178     //goal.getOrigin() += btVector3(0,0,0.04);
02179 
02180     tf::Stamped<tf::Pose> goal_base_rot = Geometry::getPoseIn("base_link", goal);
02181     goal_base_rot.setRotation(btQuaternion(0,0,0,1));
02182     goal_base_rot = Geometry::getPoseIn("/map", goal_base_rot);
02183 
02184     tf::Stamped<tf::Pose> goalappr_rot = goal;
02185     tf::Stamped<tf::Pose> goalappr_rot_backwards = goal;
02186     tf::Stamped<tf::Pose> goalappr_shift= goal;
02187 
02188     goalappr_rot =  Geometry::rotateAroundPose(goal,goal_base_rot,0,0.25,0);
02189     goalappr_rot.getOrigin() += btVector3(0.02,0,0.04);
02190 
02191     goalappr_rot_backwards =  Geometry::rotateAroundPose(goal,goal_base_rot,0,-0.15,0);
02192     goalappr_rot_backwards.getOrigin() -= btVector3(0.02,0,0.00);
02193 
02194     tf::Stamped<tf::Pose> goalappr_rot_high = goalappr_rot;
02195 
02196     //tf::Stamped<tf::Pose> goalappr_rot_shift = goalappr_rot;
02197 
02198     goalappr_rot_high.getOrigin() += btVector3(0,0,0.1);
02199 
02200     goalappr_shift.getOrigin() += btVector3(-0.05,0,0);
02201 
02202     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(goalappr_rot_high);
02203     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(goalappr_rot);
02204     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(goalappr_rot_backwards);
02205     //RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(goalappr_rot_shift);
02206     //RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(goalappr_shift);
02207     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(goal);
02208 
02209     tf::Stamped<tf::Pose> pot_pushed_to_cooker;
02210     pot_pushed_to_cooker.frame_id_ = "/map";
02211     pot_pushed_to_cooker.setOrigin(btVector3(-1.901817, 2.941642, 1.015725));
02212     pot_pushed_to_cooker.setRotation(btQuaternion(-0.117109, -0.076221, 0.877845, -0.458110));
02213 
02214     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(pot_pushed_to_cooker);
02215 
02216     if (jump == 17)
02217     {
02218         pot_pushed_to_cooker.getOrigin() += btVector3(0,0,0.1);
02219 
02220         RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(pot_pushed_to_cooker);
02221 
02222         //goal.setOrigin(btVector3(potPose.getOrigin().x() + adj_x,potPose.getOrigin().y() + adj_y,1.06 + modifier - 0.02 + 0.1));
02223 
02224         //RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(goal);
02225 
02226         RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(lidcarry);
02227 
02228         RobotArm::getInstance(0)->raise_elbow = false;
02229 
02230         exit(0);
02231     }
02232 
02233     Gripper::getInstance(0)->open();
02234 
02235     goal.setOrigin(btVector3(potPose.getOrigin().x() - .0,potPose.getOrigin().y() -.02 ,goal.getOrigin().z() + .1));
02236 
02237     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(goal);
02238 
02239     RobotArm::getInstance((0))->universal_move_toolframe_ik_pose(lidcarry);
02240     return 0;
02241 }
02242 
02243 
02244 void getLidAndBowl(int jump)
02245 {
02246     switch (jump)
02247 
02248     {
02249 
02250         //get around cable
02251     case -2:
02252     {
02253         RobotDriver::moveBase4(-0.346670, 2.307890, 0.757347, 0.653002);
02254     }
02255 
02256     case -1:
02257     {
02258         Current::manipulateKnob(1);
02259         //Current::manipulateKnob(-1);
02260     }
02261 
02262     case 0:
02263     {
02264         Current::openDrawerUnderOven(jump);
02265     }
02266 
02267 // get the lid out
02268     case 1:
02269     {
02270         Current::getLidOut(jump);
02271     }
02272 
02273     // get the bowl out
02274     case 2:
02275     {
02276         Current::getBowlOut(jump);
02277     }
02278 
02279     //close drawer
02280     case 3:
02281     {
02282 
02283         Current::closeDrawerUnderOven(jump);
02284 
02285     }
02286 
02287     //get around cable
02288     case 25:
02289     {
02290         RobotDriver::moveBase4(-0.346670, 2.307890, 0.757347, 0.653002);
02291     }
02292 
02293     // pour popcorn and put lid on pot
02294     case 4:
02295     {
02296 
02297         Current::pourPopCorn(jump);
02298     }
02299 
02300     case 5:
02301     {
02302         Current::putLidOn(jump);
02303     }
02304 
02305     }
02306 }
02307 
02308 int Current::removeLid(int jump)
02309 {
02310 
02311     RobotHead::getInstance()->lookAtThreaded("/map", -1.86 , 2.76, 0.956078350673);
02312 
02313 //    RobotHead::getInstance()->lookAtThreaded("/map", -1.89231883149, 2.93624266139, 0.956078350673);
02314 
02315     OperateHandleController::plateAttackPose();
02316     Gripper::getInstance(1)->openThreaded();
02317 
02318     RobotDriver::moveBase4(-1.243378, 2.790111, 0.998631, -0.052253);
02319 
02320 
02321     tf::Stamped<tf::Pose> lid;
02322     getLidPose(lid);
02323 
02324     btTransform lidRel;
02325     //lidRel.setOrigin(btVector3(0.026, 0.013, 0.042));
02326     lidRel.setOrigin(btVector3(0.026, 0.033, 0.042));
02327     lidRel.setRotation(btQuaternion(0.655, 0.742, -0.102, 0.097));
02328 
02329     //btTransform lidBt;
02330     //lidBt.setOrigin(lid.getOrigin());
02331     //lidBt.setRotation(lid.getRotation());
02332 
02333     btTransform lidAbs = lid * lidRel;
02334 
02335     btTransform lidPreRel;
02336     lidPreRel.setOrigin(btVector3(0.034, -0.043, 0.083));
02337     lidPreRel.setRotation(btQuaternion(0.649, 0.733, -0.142, 0.142));
02338 
02339     btTransform lidPreAbs = lid * lidPreRel;
02340 
02341     announce("Lid_grasp", Geometry::make_pose(lidAbs,"/map"));
02342 
02343     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(Geometry::make_pose(lidPreAbs,"/map"));
02344 
02345     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(Geometry::make_pose(lidAbs,"/map"));
02346 
02347     Gripper::getInstance(1)->close();
02348 
02349     ros::Duration(0.5).sleep();
02350 
02351     ROS_ERROR("REMOVE LID AMOUNT OPEN: %f", Gripper::getInstance(1)->getAmountOpen());
02352 
02353     if (Gripper::getInstance(1)->getAmountOpen() < 0.03)
02354     {
02355         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(Geometry::make_pose(lidPreAbs,"/map"));
02356         OperateHandleController::plateAttackPoseLeft();
02357         return Current::removeLid(jump);
02358     }
02359 
02360     tf::Stamped<tf::Pose> lidhigh = Geometry::make_pose(lidAbs,"/map");
02361     lidhigh.getOrigin() += btVector3(0,0,.1);
02362 
02363     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(lidhigh);
02364 
02365     tf::Stamped<tf::Pose> lidside;
02366     lidside.frame_id_ = "/map";
02367     lidside.setOrigin(btVector3(-1.719, 2.383, 0.918));
02368     lidside.setRotation(btQuaternion(0.287, 0.951, -0.031, 0.113));
02369 
02370 
02371     tf::Stamped<tf::Pose> lidsidehigh;
02372     lidsidehigh.frame_id_ = "/map";
02373     lidsidehigh.setOrigin(btVector3(-1.719, 2.383, 0.918 + 0.15));
02374     lidsidehigh.setRotation(btQuaternion(0.287, 0.951, -0.031, 0.113));
02375 
02376     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(lidsidehigh);
02377 
02378     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(lidside);
02379 
02380 
02381     Gripper::getInstance(1)->open();
02382 
02383 
02384     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(lidsidehigh);
02385 
02386 
02387 
02388 
02389 
02390     //RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(Geometry::make_pose(lidAbs,"/map"));
02391 
02392     //Gripper::getInstance(1)->open();
02393 
02394 
02395     //RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(Geometry::make_pose(lidPreAbs,"/map"));
02396 
02397     //exit(0);
02398 
02399 
02400 
02401 
02402 
02403     /*tf::Stamped<tf::Pose> potPose, pre_grasp_r, grasp_r, pre_grasp_l, grasp_l;
02404     bool gotPotPose = false;
02405     while (!gotPotPose)
02406         gotPotPose = getPotPose(potPose, pre_grasp_l, grasp_l, pre_grasp_r, grasp_r);
02407 
02408     potPose.getOrigin().setZ(1.084787);
02409 
02410     tf::Stamped<tf::Pose> prelid;
02411     prelid.frame_id_ = "/map";
02412     prelid.setOrigin(potPose.getOrigin() - btVector3(0,.1,-.015));
02413     prelid.setRotation(btQuaternion(-0.006873, 0.028028, 0.721502, 0.691810));
02414 
02415     tf::Stamped<tf::Pose> lid;
02416     lid.frame_id_ = "/map";
02417     lid.setOrigin(potPose.getOrigin() - btVector3(-0.02,-.03,0)) ;
02418     lid.getOrigin().setZ(1.075707);
02419     lid.setRotation(btQuaternion(-0.054255, 0.079032, 0.718619, 0.688765));
02420 
02421     announce("Lid", lid);
02422 
02423     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(prelid);
02424 
02425     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(lid);
02426 
02427     Gripper::getInstance(1)->close();
02428 
02429     tf::Stamped<tf::Pose> lidhigh;
02430     lidhigh.frame_id_ = "/map";
02431     lidhigh.setOrigin(potPose.getOrigin() + btVector3(0,0,0.07));
02432     lidhigh.setRotation(btQuaternion(-0.053563, 0.077269, 0.718727, 0.688906));
02433 
02434     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(lidhigh);
02435 
02436 
02437     tf::Stamped<tf::Pose> lidside;
02438     lidside.frame_id_ = "/map";
02439     lidside.setOrigin(btVector3(-1.799645, 2.544812, 1.148517));
02440     lidside.setRotation(btQuaternion(-0.066076, 0.066084, 0.707530, 0.700477));
02441 
02442     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(lidside);
02443     tf::Stamped<tf::Pose> liddown;
02444     liddown.frame_id_ = "/map";
02445     liddown.setOrigin(btVector3(-1.751367, 2.353818, 0.922780));
02446     liddown.setRotation(btQuaternion(-0.073592, 0.048190, 0.918971, 0.384387));
02447 
02448     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(liddown);
02449 
02450     Gripper::getInstance(1)->open();
02451 
02452     tf::Stamped<tf::Pose> lidexit;
02453     lidexit.frame_id_ = "/map";
02454     lidexit.setOrigin(btVector3(-1.686919, 2.283597, 0.933412));
02455     lidexit.setRotation(btQuaternion(-0.088774, 0.080089, 0.911498, 0.393544));
02456 
02457     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(lidexit);*/
02458 
02459     return 0;
02460 }
02461 
02462 
02463 tf::Stamped<tf::Pose> hack_r, hack_l;
02464 
02465 
02466 int Current::takePotFromIsland(int jump)
02467 {
02468     //RobotHead::getInstance()->lookAtThreaded("/map", -1.89231883149, 2.93624266139, 0.956078350673);
02469     RobotHead::getInstance()->lookAtThreaded("/map", -1.86 , 2.76, 0.956078350673);
02470 
02471     // -1.77 2.76 .95
02472 
02473     boost::thread ty(&OperateHandleController::plateAttackPose);
02474 
02475     Gripper::getInstance(0)->openThreaded(0.04);
02476     Gripper::getInstance(1)->openThreaded(0.04);
02477 
02478 
02479     RobotArm::getInstance(0)->raise_elbow = true;
02480     RobotArm::getInstance(1)->raise_elbow = true;
02481 
02482 
02483     //RobotDriver::moveBase4(-1.318529, 2.688299, 0.998893, -0.047046);
02484     RobotDriver::moveBase4(-1.243378, 2.790111, 0.998631, -0.052253); // takepotfrom
02485 
02486     //RobotDriver::moveBase4(-1.243155, 2.693516, 0.999085, -0.042705);
02487 
02488     tf::Stamped<tf::Pose> potPose, pre_grasp_r, grasp_r, pre_grasp_l, grasp_l;
02489     bool gotPotPose = false;
02490     while (!gotPotPose)
02491     {
02492         gotPotPose = getPotPose(potPose, pre_grasp_l, grasp_l, pre_grasp_r, grasp_r,true);
02493         gotPotPose = getPotPose(potPose, pre_grasp_l, grasp_l, pre_grasp_r, grasp_r,true);
02494     }
02495 
02496     RobotArm::moveBothArms(pre_grasp_l,pre_grasp_r);
02497 
02498     grasp_l.getOrigin() += btVector3(0,0,-0.01);
02499     grasp_r.getOrigin() += btVector3(0,0,-0.01);
02500 
02501     hack_l = grasp_l;
02502     hack_r = grasp_r;
02503 
02504     RobotArm::moveBothArms(grasp_l,grasp_r);
02505     OperateHandleController::closeGrippers();
02506 
02507     ros::Duration(0.5).sleep();
02508 
02509     ROS_ERROR("takePotFromIsland  AMOUNT OPEN: %f", Gripper::getInstance(0)->getAmountOpen());
02510     ROS_ERROR("takePotFromIsland  AMOUNT OPEN: %f", Gripper::getInstance(1)->getAmountOpen());
02511 
02512     if ((Gripper::getInstance(1)->getAmountOpen() < 0.007) || (Gripper::getInstance(0)->getAmountOpen() < 0.007) ||
02513             (Gripper::getInstance(1)->getAmountOpen() > 0.03) || (Gripper::getInstance(0)->getAmountOpen() > 0.03))
02514     {
02515 
02516         Gripper::getInstance(0)->openThreaded(0.04);
02517         Gripper::getInstance(1)->open(0.04);
02518 
02519         RobotArm::moveBothArms(pre_grasp_l,pre_grasp_r);
02520 
02521         OperateHandleController::plateAttackPose();
02522         return Current::takePotFromIsland(jump);
02523     }
02524 
02525 
02526 
02527     //OperateHandleController::openGrippers();
02528     //RobotArm::moveBothArms(pre_grasp_l,pre_grasp_r);
02529     //OperateHandleController::plateAttackPose();
02530     //big pot
02531     //tf::Stamped<tf::Pose> rcarry0;
02532     //rcarry0.frame_id_ = "base_link";
02533     //rcarry0.setOrigin(btVector3(0.553647, -0.319272, 1.162564));
02534     //rcarry0.setRotation(btQuaternion(0.547706, 0.480213, -0.537497, -0.424865));
02535 
02536     tf::Stamped<tf::Pose> rcarry0;
02537     rcarry0.frame_id_ = "base_link";
02538     rcarry0.setOrigin(btVector3(0.564247, -0.253055, 1.162559));
02539     rcarry0.setRotation(btQuaternion(-0.520823, -0.509241, 0.559830, 0.394977));
02540     //big pot
02541     //tf::Stamped<tf::Pose> lcarry0;
02542     //lcarry0.frame_id_ = "base_link";
02543     //lcarry0.setOrigin(btVector3(0.528605, 0.034132, 1.164678));
02544     //lcarry0.setRotation(btQuaternion(0.527690, -0.480534, -0.526408, 0.462088));
02545     tf::Stamped<tf::Pose> lcarry0;
02546     lcarry0.frame_id_ = "base_link";
02547     lcarry0.setOrigin(btVector3(0.524614, 0.019969, 1.164658));
02548     lcarry0.setRotation(btQuaternion(-0.414234, 0.548428, -0.363319, 0.628997));
02549 
02550     grasp_l= Geometry::getPoseIn("base_link", grasp_l);
02551 
02552     lcarry0.setRotation(grasp_l.getRotation());
02553 
02554 
02555     RobotArm::moveBothArms(lcarry0,rcarry0);
02556 
02557     RobotArm::getInstance(0)->raise_elbow = false;
02558     RobotArm::getInstance(1)->raise_elbow = false;
02559 
02560     return 0;
02561 }
02562 
02563 void printPose(const char title[], tf::Pose pose)
02564 {
02565     ROS_INFO("%s %f %f %f %f %f %f %f", title, pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z()
02566              , pose.getRotation().x(), pose.getRotation().y(), pose.getRotation().z(), pose.getRotation().w());
02567 }
02568 
02569 
02570 int Current::pushPot(int jump)
02571 {
02572     RobotHead::getInstance()->lookAtThreaded("/map", -1.89231883149, 2.93624266139, 0.956078350673);
02573 
02574     //OperateHandleController::plateAttackPose();
02575     boost::thread ty(&OperateHandleController::plateAttackPose);
02576 
02577     Gripper::getInstance(0)->openThreaded(0.04);
02578     Gripper::getInstance(1)->openThreaded(0.04);
02579 
02580     //RobotDriver::moveBase4(-1.318529, 2.688299, 0.998893, -0.047046);
02581     RobotDriver::moveBase4(-1.243378 - 0.075, 2.790111, 0.998631, -0.052253);
02582     //RobotDriver::moveBase4(-1.243155, 2.693516, 0.999085, -0.042705);
02583     RobotArm::getInstance(0);
02584 
02585     tf::Stamped<tf::Pose> potPose, pre_grasp_r, grasp_r, pre_grasp_l, grasp_l;
02586     //to make sure we dont move, discard one frame
02587     discardKinectFrame();
02588 
02589     bool hadtodrive = true;
02590 
02591     bool gotPotPose = false;
02592 
02593     while (hadtodrive)
02594     {
02595         while (!gotPotPose)
02596             gotPotPose = getPotPose(potPose, pre_grasp_l, grasp_l, pre_grasp_r, grasp_r,true);
02597 
02598         printPose("potPose",potPose);
02599 
02600         tf::Stamped<tf::Pose> result;
02601         std::vector<int> arm;
02602         std::vector<tf::Stamped<tf::Pose> > goal;
02603         arm.push_back(0);
02604         arm.push_back(0);
02605         arm.push_back(1);
02606         arm.push_back(1);
02607         goal.push_back(pre_grasp_r);
02608         goal.push_back(grasp_r);
02609         goal.push_back(pre_grasp_l);
02610         goal.push_back(grasp_l);
02611         //bool RobotArm::findBaseMovement(tf::Stamped<tf::Pose> &result, std::vector<int> arm, std::vector<tf::Stamped<tf::Pose> > goal, bool drive, bool reach)
02612 
02613         RobotArm::findBaseMovement(result, arm, goal, true, false);
02614 
02615         if (result.getOrigin().length() < 0.01)
02616             hadtodrive = false;
02617     }
02618 
02619 
02620     RobotArm::getInstance(0)->raise_elbow = true;
02621     RobotArm::getInstance(1)->raise_elbow = true;
02622 
02623     RobotArm::moveBothArms(pre_grasp_l,pre_grasp_r);
02624 
02625     grasp_l.getOrigin() += btVector3(0,0,-0.01);
02626     grasp_r.getOrigin() += btVector3(0,0,-0.01);
02627 
02628     RobotArm::moveBothArms(grasp_l,grasp_r);
02629     OperateHandleController::closeGrippers();
02630 
02631     ros::Duration(0.5).sleep();
02632 
02633     {
02634         btTransform btri = grasp_r;
02635         btTransform btli = grasp_l;
02636         btTransform relIntended = btri.inverseTimes(btli);
02637         tf::Stamped<tf::Pose> act_r, act_l;
02638         act_r = RobotArm::getInstance(0)->getToolPose("map");
02639         act_l = RobotArm::getInstance(1)->getToolPose("map");
02640         btTransform btr = act_r;
02641         btTransform btl = act_l;
02642         btTransform relAct = btr.inverseTimes(btl);
02643 
02644         ROS_ERROR("XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX");
02645         printPose("intended", relIntended);
02646         printPose("relAct", relAct);
02647         ROS_ERROR("XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX");
02648 
02649         if (1)
02650         {
02651             gotPotPose = false;
02652             while (!gotPotPose)
02653                 gotPotPose = getPotPose(potPose, pre_grasp_l, grasp_l, pre_grasp_r, grasp_r,true);
02654             printPose("potPose after grasp",potPose);
02655         }
02656     }
02657 
02658     ROS_ERROR("pushPot AMOUNT OPEN: %f", Gripper::getInstance(0)->getAmountOpen());
02659     ROS_ERROR("pushPot AMOUNT OPEN: %f", Gripper::getInstance(1)->getAmountOpen());
02660 
02661     if ((Gripper::getInstance(1)->getAmountOpen() < 0.007) || (Gripper::getInstance(0)->getAmountOpen() < 0.007))
02662     {
02663 
02664         Gripper::getInstance(0)->openThreaded(0.04);
02665         Gripper::getInstance(1)->open(0.04);
02666 
02667         RobotArm::moveBothArms(pre_grasp_l,pre_grasp_r);
02668 
02669         OperateHandleController::plateAttackPose();
02670         return Current::pushPot(jump);
02671     }
02672 
02673 
02674     // push to side
02675     tf::Stamped<tf::Pose> rpot_down;
02676     rpot_down.frame_id_ = "/map";
02677     rpot_down.setOrigin(btVector3(-1.867984, 2.791455, 0.941677));
02678     rpot_down.setRotation(btQuaternion(-0.435962, 0.552046, -0.310802, 0.639206));
02679 
02680 
02681     tf::Stamped<tf::Pose> lpot_down;
02682     lpot_down.frame_id_ = "/map";
02683     lpot_down.setOrigin(btVector3(-1.837314, 2.528506, 0.945535));
02684     lpot_down.setRotation(btQuaternion(-0.608696, -0.378505, 0.631323, 0.296063));
02685 
02686     RobotHead::getInstance()->lookAtThreaded("/map",-1.837314, 2.528506, 0.945535);
02687 
02688 
02689     //RobotDriver::moveBase4(-1.173220, 2.577223, 0.998607, -0.052696);
02690 
02691     RobotArm::moveBothArms(lpot_down,rpot_down);
02692 
02693     //OperateHandleController::openGrippers();
02694     Gripper::getInstance(0)->openThreaded(0.04);
02695     Gripper::getInstance(1)->open(0.04);
02696 
02697     tf::Stamped<tf::Pose> rpot_down_exit;
02698     rpot_down_exit.frame_id_ = "/map";
02699     rpot_down_exit.setOrigin(btVector3(-1.867984, 2.791455 + .1, 0.941677));
02700     rpot_down_exit.setRotation(btQuaternion(-0.435962, 0.552046, -0.310802, 0.639206));
02701 
02702 
02703     tf::Stamped<tf::Pose> lpot_down_exit;
02704     lpot_down_exit.frame_id_ = "/map";
02705     lpot_down_exit.setOrigin(btVector3(-1.837314, 2.528506  - .1, 0.945535));
02706     lpot_down_exit.setRotation(btQuaternion(-0.608696, -0.378505, 0.631323, 0.296063));
02707 
02708 
02709     RobotArm::moveBothArms(lpot_down_exit,rpot_down_exit);
02710 
02711     OperateHandleController::plateAttackPose();
02712 
02713     RobotArm::getInstance(0)->raise_elbow = false;
02714     RobotArm::getInstance(1)->raise_elbow = false;
02715 
02716 
02717     return 0;
02718 }
02719 
02720 
02721 int pourReadyPopcorn_(double base_x, double base_y, double base_oz, double base_ow, double height_ = 0.0)
02722 {
02723 
02724     RobotArm::getInstance(0)->raise_elbow = true;
02725     RobotArm::getInstance(1)->raise_elbow = true;
02726 
02728 
02729     //RobotDriver::moveBase4(-0.941867, 1.919927, 0.999686, -0.025022);
02730 
02731     //RobotDriver::moveBase4(-1.094190, 1.929376, 0.997743, -0.067087);
02732     //RobotDriver::moveBase4(-1.094190, 1.6,  0.997743, -0.067087);
02733     RobotDriver::moveBase4(base_x, base_y, base_oz, base_ow);
02734 
02735     tf::Stamped<tf::Pose> r,l,rrot,lrot,mid;
02736 
02737     r = RobotArm::getInstance(0)->getToolPose("/base_link");
02738     l = RobotArm::getInstance(1)->getToolPose("/base_link");
02739 
02740     r.getOrigin() += btVector3(0.02, 0,0);
02741     l.getOrigin() += btVector3(0.02, 0,0);
02742 
02743     mid = r;
02744     mid.setRotation(btQuaternion(0,0,0,1));
02745     mid.setOrigin(0.5 * (r.getOrigin() + l.getOrigin()));
02746 
02747     printPose("r" , r);
02748     printPose("l" , l);
02749 
02750     rrot =  Geometry::rotateAroundPose(r, mid, 0, 2.4, 0);
02751     lrot =  Geometry::rotateAroundPose(l, mid, 0, 2.4, 0);
02752 
02753     printPose("r" , r);
02754     printPose("l" , l);
02755 
02756     OperateHandleController::spinnerL(0,0,-.1 - height_);
02757 
02758     double time = RobotArm::getInstance(0)->time_to_target;
02759 
02760     RobotArm::getInstance(0)->time_to_target = 5;
02761     RobotArm::getInstance(1)->time_to_target = 5;
02762 
02763     double down = -.12;
02764 
02765     down = -.2 - height_;
02766 
02767     lrot.setOrigin(l.getOrigin() + btVector3(0,0,down));
02768     rrot.setOrigin(r.getOrigin() + btVector3(0,0,down));
02769 
02770     RobotArm::moveBothArms(lrot,rrot);
02771 
02772     RobotArm::getInstance(0)->time_to_target = time;
02773     RobotArm::getInstance(1)->time_to_target = time;
02774 
02775     lrot.setOrigin(l.getOrigin() + btVector3(0,0,0));
02776     rrot.setOrigin(r.getOrigin() + btVector3(0,0,0));
02777 
02778     RobotArm::moveBothArms(lrot,rrot);
02779 
02780     RobotArm::moveBothArms(l,r);
02781 
02782     RobotArm::getInstance(0)->raise_elbow = false;
02783     RobotArm::getInstance(1)->raise_elbow = false;
02784 
02785 
02786     return 0;
02787 }
02788 
02789 
02790 int fotopourReadyPopcorn_(double base_x, double base_y, double base_oz, double base_ow, double height_ , double angle, bool do_exit = false)
02791 {
02792 
02793     RobotArm::getInstance(0)->raise_elbow = true;
02794     RobotArm::getInstance(1)->raise_elbow = true;
02795 
02797 
02798     //RobotDriver::moveBase4(-0.941867, 1.919927, 0.999686, -0.025022);
02799 
02800     //RobotDriver::moveBase4(-1.094190, 1.929376, 0.997743, -0.067087);
02801     //RobotDriver::moveBase4(-1.094190, 1.6,  0.997743, -0.067087);
02802     RobotDriver::moveBase4(base_x, base_y, base_oz, base_ow);
02803 
02804 
02805     tf::Stamped<tf::Pose> r,l,rrot,lrot,mid;
02806 
02807     r = RobotArm::getInstance(0)->getToolPose("/base_link");
02808     l = RobotArm::getInstance(1)->getToolPose("/base_link");
02809 
02810     r.getOrigin() += btVector3(0.02, 0,0);
02811     l.getOrigin() += btVector3(0.02, 0,0);
02812 
02813     mid = r;
02814     mid.setRotation(btQuaternion(0,0,0,1));
02815     mid.setOrigin(0.5 * (r.getOrigin() + l.getOrigin()));
02816 
02817     printPose("r" , r);
02818     printPose("l" , l);
02819 
02820     rrot =  Geometry::rotateAroundPose(r, mid, 0, angle, 0);
02821     lrot =  Geometry::rotateAroundPose(l, mid, 0, angle, 0);
02822 
02823     printPose("r" , r);
02824     printPose("l" , l);
02825 
02826     OperateHandleController::spinnerL(0,0,-.1 - height_);
02827 
02828     double time = RobotArm::getInstance(0)->time_to_target;
02829 
02830     RobotArm::getInstance(0)->time_to_target = 5;
02831     RobotArm::getInstance(1)->time_to_target = 5;
02832 
02833     double down = -.12;
02834 
02835     down = -.2 - height_;
02836 
02837     lrot.setOrigin(l.getOrigin() + btVector3(0,0,down));
02838     rrot.setOrigin(r.getOrigin() + btVector3(0,0,down));
02839     RobotArm::moveBothArms(lrot,rrot);
02840 
02841 
02842     if (do_exit)
02843         exit(0);
02844 
02845     RobotArm::getInstance(0)->time_to_target = time;
02846     RobotArm::getInstance(1)->time_to_target = time;
02847 
02848     lrot.setOrigin(l.getOrigin() + btVector3(0,0,0));
02849     rrot.setOrigin(r.getOrigin() + btVector3(0,0,0));
02850 
02851     RobotArm::moveBothArms(lrot,rrot);
02852 
02853     RobotArm::moveBothArms(l,r);
02854 
02855     RobotArm::getInstance(0)->raise_elbow = false;
02856     RobotArm::getInstance(1)->raise_elbow = false;
02857 
02858 
02859     return 0;
02860 }
02861 
02862 
02863 
02864 //return pot to heater
02865 
02866 void returnPotToHeater()
02867 {
02868     tf::Stamped<tf::Pose> rpot_down;
02869     rpot_down.frame_id_ = "/map";
02870     rpot_down.setOrigin(btVector3(-1.867984, 2.791455, 0.941677));
02871     rpot_down.setRotation(btQuaternion(-0.435962, 0.552046, -0.310802, 0.639206));
02872 
02873 
02874     tf::Stamped<tf::Pose> lpot_down;
02875     lpot_down.frame_id_ = "/map";
02876     lpot_down.setOrigin(btVector3(-1.837314, 2.528506, 0.945535));
02877     lpot_down.setRotation(btQuaternion(-0.608696, -0.378505, 0.631323, 0.296063));
02878 
02879     RobotHead::getInstance()->lookAtThreaded("/map",-1.837314, 2.528506, 0.945535);
02880 
02881     RobotDriver::moveBase4(-1.173220, 2.577223, 0.998607, -0.052696);
02882 
02883     RobotArm::moveBothArms(lpot_down,rpot_down);
02884 
02885     //OperateHandleController::openGrippers();
02886     Gripper::getInstance(0)->openThreaded(0.04);
02887     Gripper::getInstance(1)->open(0.04);
02888 
02889     tf::Stamped<tf::Pose> rpot_down_exit;
02890     rpot_down_exit.frame_id_ = "/map";
02891     rpot_down_exit.setOrigin(btVector3(-1.867984, 2.791455 + .1, 0.941677));
02892     rpot_down_exit.setRotation(btQuaternion(-0.435962, 0.552046, -0.310802, 0.639206));
02893 
02894 
02895     tf::Stamped<tf::Pose> lpot_down_exit;
02896     lpot_down_exit.frame_id_ = "/map";
02897     lpot_down_exit.setOrigin(btVector3(-1.837314, 2.528506  - .1, 0.945535));
02898     lpot_down_exit.setRotation(btQuaternion(-0.608696, -0.378505, 0.631323, 0.296063));
02899 
02900     RobotArm::moveBothArms(lpot_down_exit,rpot_down_exit);
02901 
02902     OperateHandleController::plateAttackPose();
02903 }
02904 
02905 
02906 void returnPotToSink()
02907 {
02908 
02909     RobotHead::getInstance()->lookAtThreaded("/map",1.08, .31, 0.945535);
02910 
02911 
02912     RobotDriver::moveBase4(0.030936, 0.289066, 0.022239, 0.999752);
02913     RobotArm::getInstance(0)->raise_elbow = true;
02914     RobotArm::getInstance(1)->raise_elbow = true;
02915 
02916 
02917     //RobotArm::moveBothArms(lpot_down,rpot_down);
02918     tf::Stamped<tf::Pose> rputdown;
02919     rputdown.frame_id_ = "/map";
02920     rputdown.setOrigin(btVector3(1.060072, 0.144450, 0.957838));
02921     rputdown.setRotation(btQuaternion(0.545655, 0.541955, -0.543372, -0.336590));
02922 
02923 
02924     tf::Stamped<tf::Pose> lputdown;
02925     lputdown.frame_id_ = "/map";
02926     lputdown.setOrigin(btVector3(1.001025, 0.414402, 0.966610));
02927     lputdown.setRotation(btQuaternion(-0.382869, 0.495838, -0.391748, 0.673862));
02928 
02929     tf::Stamped<tf::Pose> rputdownexit;
02930     rputdownexit.frame_id_ = "/map";
02931     rputdownexit.setOrigin(btVector3(1.058829, 0.145553 - .07, 0.958970));
02932     rputdownexit.setRotation(btQuaternion(0.546275, 0.541866, -0.543263, -0.335902));
02933 
02934 
02935     tf::Stamped<tf::Pose> lputdownexit;
02936     lputdownexit.frame_id_ = "/map";
02937     lputdownexit.setOrigin(btVector3(0.999645, 0.415450 + .07, 0.967175));
02938     lputdownexit.setRotation(btQuaternion(-0.383121, 0.495288, -0.392308, 0.673797));
02939 
02940     {
02941         tf::Stamped<tf::Pose> result;
02942         std::vector<int> arm;
02943         std::vector<tf::Stamped<tf::Pose> > goal;
02944         arm.push_back(0);
02945         arm.push_back(0);
02946         arm.push_back(1);
02947         arm.push_back(1);
02948         goal.push_back(rputdown);
02949         goal.push_back(rputdownexit);
02950         goal.push_back(lputdown);
02951         goal.push_back(lputdownexit);
02952         //bool RobotArm::findBaseMovement(tf::Stamped<tf::Pose> &result, std::vector<int> arm, std::vector<tf::Stamped<tf::Pose> > goal, bool drive, bool reach)
02953 
02954         RobotArm::findBaseMovement(result, arm, goal, true, false);
02955     }
02956 
02957     RobotArm::moveBothArms(lputdown,rputdown);
02958 
02959     //OperateHandleController::openGrippers();
02960     Gripper::getInstance(0)->openThreaded(0.04);
02961     Gripper::getInstance(1)->open(0.04);
02962 
02963     RobotArm::moveBothArms(lputdownexit,rputdownexit);
02964 
02965     Gripper::getInstance(0)->openThreaded();
02966     Gripper::getInstance(1)->openThreaded();
02967 
02968     //RobotArm::moveBothArms(lpot_down_exit,rpot_down_exit);
02969 
02970     lputdownexit.getOrigin() += btVector3(0,0,0.1);
02971     rputdownexit.getOrigin() += btVector3(0,0,0.1);
02972 
02973     RobotArm::moveBothArms(lputdownexit,rputdownexit);
02974 
02975     RobotArm::getInstance(0)->raise_elbow = false;
02976     RobotArm::getInstance(1)->raise_elbow = false;
02977 
02978     OperateHandleController::plateAttackPose();
02979 }
02980 
02981 // this is a dirty hack, we keep record of how the bowl pose deviates from the exepected pose and simply add this to the position we salt above
02982 // would probably be cleaner to keep the bowl pose instead of the delta..
02983 btVector3 diff(-.12,0,0);
02984 
02985 tf::Stamped<tf::Pose> getBigBowlPose(const btVector3 &search);
02986 
02987 int Current::pourReadyPopcorn(int jump)
02988 {
02989     RobotHead::getInstance()->lookAtThreaded("/map",-1.692, 1.621, 0.899);
02990     //pourReadyPopcorn_(-1.094190, 1.6,  0.997743, -0.067087);
02991     //pourReadyPopcorn_(-1.044190, 1.6,  0.997743, -0.067087);
02992     //pourReadyPopcorn_(-1, 1.6,  0.997743, -0.067087);
02993 
02994     RobotDriver::moveBase4(-1.154662, 1.573712, 0.998081, -0.061899);
02995 
02996     btVector3 search(-1.72, 1.6, .88);
02997     tf::Stamped<tf::Pose> bowlPose = getBigBowlPose(search);
02998 
02999     diff = bowlPose.getOrigin() - btVector3(-1.583374,1.634822,1.087549);
03000 
03001     {
03002         tf::Stamped<tf::Pose> inBase = Geometry::getPoseIn("base_link", bowlPose);
03003         printPose("BowlPoseinbase", bowlPose);
03004         // define where the bowl should be in base coords to make the trajectory work for it
03005         tf::Stamped<tf::Pose> goalPose = Geometry::make_pose(0.600358,-0.125386,0.736068,-0.001765,-0.002289,-0.004847,0.999984,"base_link");
03006 
03007         // calc how much we have to move to bring the bowl into the desired position relative to the robot
03008         btVector3 rel = inBase.getOrigin() - goalPose.getOrigin();
03009         ROS_ERROR("rel %f %f", rel.x(), rel.y());
03010 
03011         tf::Stamped<tf::Pose> nav_goal;
03012         nav_goal.frame_id_ = "/base_link";
03013         nav_goal.setRotation(btQuaternion(0,0,0,1));
03014         nav_goal.setOrigin(btVector3(rel.x(), rel.y(), 0));
03015         nav_goal = Geometry::getPoseIn("map", nav_goal);
03016 
03017         //RobotDriver::getInstance()->driveInMap(nav_goal, false); //
03018 
03019         printPose("BowlPose", bowlPose);
03020 
03021         pourReadyPopcorn_(nav_goal.getOrigin().x(), nav_goal.getOrigin().y(), nav_goal.getRotation().z(), nav_goal.getRotation().w(), 0);
03022 
03023     }
03024 
03025     return 0;
03026 }
03027 
03028 
03029 
03030 
03031 //RobotDriver::moveBase4(-1.087042, 1.707556 - .025, 0.998003, -0.063155);
03032 void salt_it(btVector3 saltloc, double base_x =-1.087042 + .1, double base_y = 1.707556 - .025, double base_oz = 0.998003, double base_ow = -0.063155)
03033 {
03034     btVector3 expected(-.9, 3.8, 0.88);
03035 
03036     //expected = btVector3(atof(argv[2]),atof(argv[3]),atof(argv[4]));
03037     expected = saltloc;
03038 
03039     double time = RobotArm::getInstance(0)->time_to_target;
03040     RobotArm::getInstance(0)->time_to_target = 0.5;
03041     RobotArm::getInstance(1)->time_to_target = 0.5;
03042 
03043 
03044     RobotHead::getInstance()->lookAtThreaded("/map", expected);
03045 
03046     boost::thread t0(&OperateHandleController::plateTuckPose);
03047 
03048     RobotDriver::moveBase4(base_x - .15, base_y, base_oz, base_ow);
03049 
03050     t0.join();
03051 
03052     RobotHead::getInstance()->lookAtThreaded("/map", expected);
03053 
03054 
03055     //RobotDriver::moveBase4(-1.087042, 1.707556, 0.998003, -0.063155);
03056     //
03057 
03058     RobotArm *rarm = RobotArm::getInstance(0);
03059     RobotArm *larm = RobotArm::getInstance(1);
03060 
03061     RobotHead::getInstance()->lookAtThreaded("/map", expected);
03062 
03063     boost::thread t1(&OperateHandleController::plateAttackPose);
03064 
03065     Gripper::getInstance(1)->openThreaded();
03066     Gripper::getInstance(0)->openThreaded();
03067 
03068     bool found = false;
03069     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
03070     pcl::PointCloud<pcl::PointXYZRGB>::Ptr inBox (new pcl::PointCloud<pcl::PointXYZRGB>);
03071 
03072     std::vector<btVector3> center;
03073     std::vector<double> radius;
03074     std::vector<int> numinliers;
03075 
03076     while (!found && ros::ok())
03077     {
03078 
03079         Kinect::getInstance()->getCloud(cloud);
03080 
03081         double search_radius = 0.15;
03082         double height = 0.10;
03083 
03084         getPointsInBox(cloud,expected - btVector3(search_radius,search_radius,-0.05),expected + btVector3(search_radius,search_radius,height),inBox);
03085 
03086         getCirclesFromCloud(*inBox, .02, .01,center,radius,numinliers,1);
03087 
03088         //if (numinliers[0] < 300)
03089         //return false;
03090 
03091         tf::Stamped<tf::Pose> lid;
03092 
03093         lid.setOrigin(center[0]);
03094         lid.setRotation(btQuaternion(0,0,0,1));
03095         lid.frame_id_ = "/map";
03096 
03097         announce("Salt", lid);
03098 
03099         if ((radius[0] < 0.05) && (radius[0] > 0.01) && (numinliers[0] > 50))
03100             found = true;
03101     }
03102 
03103     /*
03104         tf::Stamped<tf::Pose> lid;
03105     getLidPose(lid);
03106 
03107     btTransform lidRel;
03108     //lidRel.setOrigin(btVector3(0.026, 0.013, 0.042));
03109     lidRel.setOrigin(btVector3(0.026, 0.033, 0.042));
03110     lidRel.setRotation(btQuaternion(0.655, 0.742, -0.102, 0.097));
03111 
03112     btTransform lidBt;
03113     lidBt.setOrigin(lid.getOrigin());
03114     lidBt.setRotation(lid.getRotation());
03115 
03116     btTransform lidAbs = lid * lidRel;
03117 
03118     btTransform lidPreRel;
03119     lidPreRel.setOrigin(btVector3(0.034, -0.043, 0.083));
03120     lidPreRel.setRotation(btQuaternion(0.649, 0.733, -0.142, 0.142));
03121 
03122     btTransform lidPreAbs = lid * lidPreRel;
03123 
03124     announce("Lid_grasp", Geometry::make_pose(lidAbs,"/map"));
03125 
03126     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(Geometry::make_pose(lidPreAbs,"/map"));
03127 
03128     */
03129 
03130     tf::Stamped<tf::Pose> cen;
03131     cen.setOrigin(center[0]);
03132     cen.setRotation(btQuaternion(0,0,0,1));
03133     cen.frame_id_ = "map";
03134     cen = Geometry::getPoseIn("/base_link", cen);
03135     cen.setRotation(btQuaternion(0.000, -0.001, 0.657, 0.754));
03136 
03137 
03138     btTransform lidRel;
03139     //lidRel.setOrigin(btVector3(-0.039, 0.012, -0.062));
03140     //lidRel.setRotation(btQuaternion(-0.040, 0.999, 0.008, 0.014));
03141     lidRel.setOrigin(btVector3(-0.030, -0.033, -0.068));
03142     lidRel.setRotation(btQuaternion(-0.389, 0.921, -0.014, 0.000));
03143 
03144     btTransform lidAbs = cen * lidRel;
03145 
03146     cen = Geometry::getPoseIn("/map",Geometry::make_pose(lidAbs,"base_link"));
03147 
03148     pubCloud(inBox);
03149 
03150     cen.getOrigin() += btVector3(0,0,0.054);
03151 
03152     tf::Stamped<tf::Pose> inMap,inMapAppr,inMap_high;
03153     inMap = cen;
03154     //inMap = Geometry::approach(inMap, 0.015);
03155     inMap = Geometry::approach(inMap, 0.02);
03156     inMapAppr = Geometry::approach(inMap, 0.1);
03157 
03158     inMap_high = inMapAppr;
03159     inMap_high.getOrigin() += btVector3(0,0,.1);
03160 
03161     t1.join();
03162     {
03163         tf::Stamped<tf::Pose> result;
03164         std::vector<int> arm;
03165         std::vector<tf::Stamped<tf::Pose> > goal;
03166         arm.push_back(1);
03167         arm.push_back(1);
03168         arm.push_back(1);
03169         goal.push_back(inMap_high);
03170         goal.push_back(inMapAppr);
03171         goal.push_back(inMap);
03172         //bool RobotArm::findBaseMovement(tf::Stamped<tf::Pose> &result, std::vector<int> arm, std::vector<tf::Stamped<tf::Pose> > goal, bool drive, bool reach)
03173 
03174         RobotArm::findBaseMovement(result, arm, goal, true, false);
03175     }
03176 
03177     RobotHead::getInstance()->lookAtThreaded("/l_gripper_tool_frame", 0,0,0);
03178 
03179     //RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(inMap_high);
03180     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(inMapAppr);
03181     //Gripper::getInstance(1)->closeThreaded();
03182     //boost::thread t1(&Gripper::close,Gripper::getInstance(1),0);
03183     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(inMap);
03184     Gripper::getInstance(1)->close();
03185 
03186     ros::Duration(0.5).sleep();
03187 
03188     ROS_ERROR("salt_it  AMOUNT OPEN: %f", Gripper::getInstance(0)->getAmountOpen());
03189     ROS_ERROR("salt_it  AMOUNT OPEN: %f", Gripper::getInstance(1)->getAmountOpen());
03190 
03191     if (Gripper::getInstance(1)->getAmountOpen() < 0.015)
03192     {
03193 
03194         Gripper::getInstance(1)->open();
03195 
03196         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(inMap_high);
03197 
03198         OperateHandleController::plateAttackPose();
03199 
03200         salt_it(saltloc, base_x, base_y, base_oz, base_ow);
03201 
03202         return;
03203     }
03204 
03205     //t1.join();
03206 
03207     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(inMap_high);
03208 
03209     if (0)
03210     {
03211         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(inMap);
03212         Gripper::getInstance(1)->openThreaded();
03213         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(inMap_high);
03214     }
03215 
03216     tf::Stamped<tf::Pose> curr = larm->getToolPose("/base_link");
03217     curr.setOrigin(btVector3(0.6,-0.03,curr.getOrigin().z()));
03218     curr.setRotation(btQuaternion(-1,1,0,0));
03219     larm->universal_move_toolframe_ik_pose(curr);
03220 
03221     double state[7];
03222     RobotArm::getInstance(1)->getJointState(state);
03223     state[6] += 3.142;
03224     boost::thread tturn(&RobotArm::startTrajectory, larm,larm->goalTraj(state[0],state[1],state[2],state[3],state[4],state[5],state[6]),true);
03225     //RobotArm::getInstance(1)->startTrajectory(larm->goalTraj(state[0],state[1],state[2],state[3],state[4],state[5],state[6]));
03226     tturn.join();
03227 
03228     tf::Stamped<tf::Pose> right_appr_rel;
03229     right_appr_rel.frame_id_ = "l_gripper_tool_frame";
03230     right_appr_rel.setOrigin(btVector3(0.000251, -0.000434, 0.085977));
03231     right_appr_rel.setRotation(btQuaternion(0.001757, -0.000095, 0.999998, -0.000168));
03232 
03233     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(right_appr_rel);
03234 
03235     tf::Stamped<tf::Pose> right_final_rel;
03236     right_final_rel.frame_id_ = "l_gripper_tool_frame";
03237     right_final_rel.setOrigin(btVector3(-0.068345, -0.000794, 0.056093));
03238     right_final_rel.setRotation(btQuaternion(0.001791, -0.000075, 0.999998, 0.001085));
03239 
03240     //Gripper::getInstance(0)->closeThreaded();
03241     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(right_final_rel);
03242     Gripper::getInstance(0)->close();
03243 
03244     //fake it
03245     if (0)
03246     {
03247         double open = Gripper::getInstance(0)->getAmountOpen();
03248         Gripper::getInstance(0)->openThreaded(open + 0.005);
03249     }
03250 
03251     tf::Stamped<tf::Pose> right_mid = rarm->getToolPose("base_link"); // mid as in no additional rotation
03252     tf::Stamped<tf::Pose>  left_mid = larm->getToolPose("base_link");
03253     tf::Stamped<tf::Pose>  left_minus,left_plus,right_minus,right_plus, left_act, right_act; // positive and negative rotation
03254 
03255     double amount = 0.2;
03256 
03257     left_minus =  Geometry::rotateAroundPose(left_mid, left_mid, 0,0,-amount);
03258     left_plus =  Geometry::rotateAroundPose(left_mid, left_mid, 0,0,amount);
03259     right_minus =  Geometry::rotateAroundPose(right_mid, right_mid, 0,0,-amount);
03260     right_plus =  Geometry::rotateAroundPose(right_mid, right_mid, 0,0,amount);
03261 
03262     double center_x = 0.6 - (0.5 * (right_mid.getOrigin().x() + left_mid.getOrigin().x()));
03263     double center_y = -0.03 - (0.5 * (right_mid.getOrigin().y() + left_mid.getOrigin().y()));
03264 
03265     //RobotDriver::moveBase4(-1.087042, 1.707556 - .025, 0.998003, -0.063155);
03266     RobotDriver::moveBase4(base_x + diff.x(), base_y + diff.y(), base_oz, base_ow);
03267 
03268     RobotArm::getInstance(0)->time_to_target = 0;
03269     RobotArm::getInstance(1)->time_to_target = 0;
03270 
03271     {
03272         tf::Stamped<tf::Pose> centerPose;
03273         centerPose.frame_id_ = "/base_link";
03274         centerPose.setOrigin(btVector3(center_x, center_y,0) + ((left_mid.getOrigin() + right_mid.getOrigin()) * 0.5));
03275         centerPose.setRotation(btQuaternion(0,0,0,1));
03276         centerPose = Geometry::getPoseIn("/map", centerPose);
03277         ROS_ERROR("no error");
03278         printPose("salting center", centerPose);
03279         announce("above bowl", centerPose);
03280     }
03281 
03282 
03283     bool bit = false;
03284     double rad = 0.07;
03285     double max = 4*M_PI;
03286     double step = M_PI / 2.5;
03287     double steps =  max / step;
03288     double radstep = rad / steps;
03289     for (double angle = 0; angle < max + M_PI; angle += step)
03290     {
03291 
03292         if (angle >= max)
03293             rad = 0;
03294 
03295         double x_add = cos(angle) * rad + center_x;
03296         double y_add = sin(angle) * rad + center_y;
03297         if (bit)
03298         {
03299             left_act = left_plus;
03300             right_act = right_minus;
03301         }
03302         else
03303         {
03304             left_act = left_minus;
03305             right_act = right_plus;
03306         }
03307         left_act.getOrigin() += btVector3(x_add,y_add,0);
03308         right_act.getOrigin() += btVector3(x_add,y_add,0);
03309 
03310         RobotArm::moveBothArms(left_act, right_act);
03311 
03312         bit = !bit;
03313         rad -= radstep * 0.7;
03314         //RobotArm::moveBothArms(left_plus, right_minus);
03315     }
03316 
03317     RobotArm::moveBothArms(left_mid, right_mid);
03318 
03319     RobotArm::getInstance(0)->time_to_target = time;
03320     RobotArm::getInstance(1)->time_to_target = time;
03321 
03322 
03323     if (1)
03324     {
03325         Gripper::getInstance(0)->openThreaded();
03326         RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(right_appr_rel);
03327         boost::thread t0(&OperateHandleController::plateAttackPoseRight);
03328         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(inMap);
03329         Gripper::getInstance(1)->openThreaded();
03330         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(inMapAppr);
03331         OperateHandleController::plateAttackPose();
03332     }
03333 }
03334 
03335 int Current::salt(int jump)
03336 {
03337     salt_it(btVector3( -1.65,1.23 ,.894)); // 6:27 pour 6:50 pot down
03338     return 0;
03339 }
03340 
03341 
03342 
03343 void takepot(int jump)
03344 {
03345 
03346     switch (jump)
03347     {
03348 
03349     case 0:
03350     {
03351         Current::removeLid(jump);
03352     }
03353 
03354 
03355     case 1:
03356     {
03357         Current::takePotFromIsland(jump);
03358 
03359     }
03360 
03361     case 2:
03362     {
03363         Current::pourReadyPopcorn(jump);
03364     }
03365 
03366 
03367 
03368     }
03369 }
03370 
03371 tf::Stamped<tf::Pose> getBigBowlPose(const btVector3 &search)
03372 {
03373 
03374     RobotHead::getInstance()->lookAt("/map",search.x(),search.y(),search.z(),true);
03375     RobotHead::getInstance()->lookAtThreaded("/map",search);
03376 
03377     tf::Stamped<tf::Pose> bowl;
03378 
03379     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
03380 
03381     Kinect::getInstance()->getCloud(cloud);
03382 
03383     pcl::PointCloud<pcl::PointXYZRGB>::Ptr inBox (new pcl::PointCloud<pcl::PointXYZRGB>);
03384 
03385     getPointsInBox(cloud, search - btVector3(.2,.2,-.03), search + btVector3(.2,.2,.05),inBox);
03386 
03387     pubCloud(inBox);
03388 
03389     btVector3 center;
03390     double radius;
03391 
03392     getCircleFromCloud(*inBox, center, radius);
03393 
03394     bool found = true;
03395     double roi_size = btVector3(.4,.4,0).length();
03396     double circle_center_distance = (center - search).length();
03397     if (circle_center_distance > roi_size / 2)
03398         found = false;
03399     if (radius > 0.15)
03400         found = false;
03401 
03402     if (!found)
03403     {
03404         return getBigBowlPose(search);
03405     }
03406     else
03407     {
03408         bowl.frame_id_ = "/map";
03409         bowl.setOrigin(center);
03410         bowl.setRotation(btQuaternion(0,0,0,1));
03411         bowl = Geometry::getPoseIn("/base_link",bowl);
03412         bowl.setRotation(btQuaternion(0,0,0,1));
03413         bowl = Geometry::getPoseIn("/map",bowl);
03414 
03415         announce("bowl", bowl);
03416 
03417         printPose("bowl", bowl);
03418     }
03419 
03420     return bowl;
03421 }
03422 
03423 void calcBigBowlGraspPoses(const tf::Stamped<tf::Pose> &bowl, tf::Stamped<tf::Pose> &l_pregrasp, tf::Stamped<tf::Pose> &l_grasp, tf::Stamped<tf::Pose> &r_pregrasp, tf::Stamped<tf::Pose> &r_grasp)
03424 {
03425     tf::Stamped<tf::Pose> pregrasp;
03426     pregrasp.setRotation(btQuaternion(btVector3(0,0,1), 0.3));
03427     pregrasp.setOrigin(btVector3(-.05,0,0));
03428 
03429     tf::Stamped<tf::Pose> l_grip_bowl;
03430     l_grip_bowl.frame_id_ = "bowl";
03431     l_grip_bowl.setOrigin(btVector3(0.009556, 0.118237, -0.009350));
03432     l_grip_bowl.setRotation(btQuaternion(-0.205230, 0.662406, -0.227324, 0.683683));
03433 
03434     tf::Stamped<tf::Pose> r_grip_bowl;
03435     r_grip_bowl.frame_id_ = "bowl";
03436     r_grip_bowl.setOrigin(btVector3(-0.009556, -0.118237, -0.009350));
03437     r_grip_bowl.setRotation(btQuaternion(-0.662406,-0.205230, 0.683683, 0.227324));
03438 
03439     l_grasp = Geometry::getRel(bowl, l_grip_bowl);
03440     r_grasp = Geometry::getRel(bowl, r_grip_bowl);
03441 
03442     l_pregrasp = Geometry::getRel(bowl, Geometry::getRel(l_grip_bowl, pregrasp));
03443     r_pregrasp = Geometry::getRel(bowl, Geometry::getRel(r_grip_bowl, pregrasp));
03444 }
03445 
03446 bool graspBigBowl(const tf::Stamped<tf::Pose> &bowl, bool grasp = true)
03447 {
03448     tf::Stamped<tf::Pose> l_grip_bowl_,r_grip_bowl_,l_pre_grip_bowl_,r_pre_grip_bowl_;
03449 
03450     calcBigBowlGraspPoses(bowl, l_pre_grip_bowl_, l_grip_bowl_, r_pre_grip_bowl_, r_grip_bowl_);
03451 
03452     //if (grasp) {
03453     announce("r", r_grip_bowl_);
03454     announce("l", l_grip_bowl_);
03455     //}
03456 
03457     RobotArm::getInstance(0)->raise_elbow = true;
03458     RobotArm::getInstance(1)->raise_elbow = true;
03459 
03460     if (grasp)
03461     {
03462         OperateHandleController::openGrippers(true);
03463 
03464         tf::Stamped<tf::Pose> rt = RobotArm::getInstance(0)->getToolPose("/map");
03465         tf::Stamped<tf::Pose> lt = RobotArm::getInstance(1)->getToolPose("/map");
03466 
03467         rt.setRotation(r_pre_grip_bowl_.getRotation());
03468         lt.setRotation(l_pre_grip_bowl_.getRotation());
03469 
03470         rt.getOrigin() += r_pre_grip_bowl_.getOrigin() + btVector3(0,0,0.2);
03471         rt.getOrigin() *= 0.5;
03472 
03473         lt.getOrigin() += l_pre_grip_bowl_.getOrigin() + btVector3(0,0,0.2);
03474         lt.getOrigin() *= 0.5;
03475 
03476         {
03477             tf::Stamped<tf::Pose> result;
03478             std::vector<int> arm;
03479             std::vector<tf::Stamped<tf::Pose> > goal;
03480             arm.push_back(1);
03481             arm.push_back(1);
03482             arm.push_back(1);
03483             arm.push_back(0);
03484             arm.push_back(0);
03485             arm.push_back(0);
03486             goal.push_back(lt);
03487             goal.push_back(l_pre_grip_bowl_);
03488             goal.push_back(l_grip_bowl_);
03489             goal.push_back(rt);
03490             goal.push_back(r_pre_grip_bowl_);
03491             goal.push_back(r_grip_bowl_);
03492             //bool RobotArm::findBaseMovement(tf::Stamped<tf::Pose> &result, std::vector<int> arm, std::vector<tf::Stamped<tf::Pose> > goal, bool drive, bool reach)
03493 
03494             RobotArm::findBaseMovement(result, arm, goal, true, false);
03495         }
03496 
03497 
03498         RobotArm::moveBothArms(lt,rt);
03499 
03500         RobotArm::moveBothArms(l_pre_grip_bowl_,r_pre_grip_bowl_);
03501         Gripper::getInstance(0)->closeThreaded(0.04);
03502         Gripper::getInstance(1)->closeThreaded(0.04);
03503         RobotArm::moveBothArms(l_grip_bowl_,r_grip_bowl_);
03504         OperateHandleController::closeGrippers();
03505 
03506         ros::Duration(0.5).sleep();
03507 
03508         ROS_ERROR("graspBigBowl  AMOUNT OPEN: %f", Gripper::getInstance(0)->getAmountOpen());
03509         ROS_ERROR("graspBigBowl  AMOUNT OPEN: %f", Gripper::getInstance(1)->getAmountOpen());
03510 
03511         if ((Gripper::getInstance(1)->getAmountOpen() < 0.004) || (Gripper::getInstance(0)->getAmountOpen() < 0.004))
03512         {
03513 
03514             Gripper::getInstance(0)->openThreaded(0.04);
03515             Gripper::getInstance(1)->open(0.04);
03516 
03517             RobotArm::moveBothArms(l_pre_grip_bowl_,r_pre_grip_bowl_);
03518 
03519             OperateHandleController::plateAttackPose();
03520 
03521 
03522             RobotArm::getInstance(0)->raise_elbow = false;
03523             RobotArm::getInstance(1)->raise_elbow = false;
03524 
03525 
03526             return false;
03527         }
03528 
03529 
03530         RobotArm::moveBothArms(Geometry::getRelInBase(l_grip_bowl_,btVector3(0,0,.1)),Geometry::getRelInBase(r_grip_bowl_,btVector3(0,0,.1)));
03531     }
03532     else
03533     {
03534         tf::Stamped<tf::Pose> rapp, lapp;
03535         rapp = r_grip_bowl_;
03536         lapp = l_grip_bowl_;
03537         //rapp.getOrigin() += btVector3(0,0,0.05);
03538         //lapp.getOrigin() += btVector3(0,0,0.05);
03539         //r_grip_bowl_.getOrigin() += btVector3(0,0,0.01);
03540         //l_grip_bowl_.getOrigin() += btVector3(0,0,0.01);
03541         ROS_ERROR("EXT");
03542         //RobotArm::moveBothArms(lapp,rapp);
03543         RobotArm::moveBothArms(l_grip_bowl_,r_grip_bowl_);
03544         OperateHandleController::openGrippers(false);
03545         RobotArm::moveBothArms(l_pre_grip_bowl_,r_pre_grip_bowl_);
03546         l_pre_grip_bowl_.getOrigin() += btVector3(0,0,0.07);
03547         r_pre_grip_bowl_.getOrigin() += btVector3(0,0,0.07);
03548         RobotArm::moveBothArms(l_pre_grip_bowl_,r_pre_grip_bowl_);
03549         OperateHandleController::plateAttackPose();
03550     }
03551 
03552     RobotArm::getInstance(0)->raise_elbow = false;
03553     RobotArm::getInstance(1)->raise_elbow = false;
03554 
03555     return true;
03556 }
03557 
03558 
03559 void getBigBowlOut()
03560 {
03561     btVector3 search(.51, 1.12, .75);
03562 
03563     bool got_it = false;
03564 
03565     while (!got_it)
03566     {
03567         tf::Stamped<tf::Pose> bowlPose = getBigBowlPose(search);
03568         got_it = graspBigBowl(bowlPose, true);
03569     }
03570 
03571     OperateHandleController::plateCarryPose();
03572 }
03573 
03574 
03575 int openFavouriteDrawer(int jump)
03576 {
03577     btVector3 search(.51, 1.12, .75);
03578 
03579     RobotHead::getInstance()->lookAtThreaded("/map",search);
03580 
03581     Gripper::getInstance(0)->openThreaded();
03582 
03583 
03584     RobotDriver::moveBase4(-0.187269, 1.122678, 0.023650, 0.999720);
03585 
03586     tf::Stamped<tf::Pose> drawer_pre_closed_grasp;
03587     drawer_pre_closed_grasp.frame_id_ = "/map";
03588     drawer_pre_closed_grasp.setOrigin(btVector3(0.6, 0.868231, 0.789463));
03589     drawer_pre_closed_grasp.setRotation(btQuaternion(0.694022, 0.007679, 0.072881, 0.716214));
03590 
03591     // open drawer
03592 
03593     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(drawer_pre_closed_grasp);
03594 
03595     tf::Stamped<tf::Pose> drawer_closed_grasp;
03596     drawer_closed_grasp.frame_id_ = "/map";
03597     drawer_closed_grasp.setOrigin(btVector3(0.691291, 0.868231, 0.789463));
03598     drawer_closed_grasp.setOrigin(btVector3(0.671291, 0.868231, 0.789463));
03599     drawer_closed_grasp.setRotation(btQuaternion(0.694022, 0.007679, 0.072881, 0.716214));
03600 
03601     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(drawer_closed_grasp);
03602 
03603     Gripper::getInstance(0)->close();
03604 
03605     ros::Duration(0.5).sleep();
03606 
03607     ROS_ERROR("openFavouriteDrawer OPEN: %f", Gripper::getInstance(0)->getAmountOpen());
03608 
03609     if (Gripper::getInstance(0)->getAmountOpen() < 0.01)
03610     {
03611         Gripper::getInstance(0)->close();
03612         RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(drawer_pre_closed_grasp);
03613         OperateHandleController::plateTuckPoseRight();
03614         return openFavouriteDrawer(jump);
03615     }
03616 
03617     tf::Stamped<tf::Pose> drawer_open_grasp;
03618     drawer_open_grasp.frame_id_ = "/map";
03619     drawer_open_grasp.setOrigin(btVector3(0.217701, 0.836088, 0.783115));
03620     drawer_open_grasp.setRotation(btQuaternion(0.690426, -0.012174, 0.045241, 0.721884));
03621 
03622     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(drawer_open_grasp);
03623 
03624     Gripper::getInstance(0)->openThreaded();
03625 
03626     tf::Stamped<tf::Pose> drawer_avoid_open_grasp;
03627     drawer_avoid_open_grasp.frame_id_ = "/map";
03628     drawer_avoid_open_grasp.setOrigin(btVector3(0.217025, 0.635642, 0.783132));
03629     drawer_avoid_open_grasp.setRotation(btQuaternion(0.690618, -0.012459, 0.045518, 0.721678));
03630 
03631     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(drawer_avoid_open_grasp);
03632 
03633     OperateHandleController::plateAttackPose();
03634 
03635     //get bowl out
03636 
03637     //getBigBowlOut();
03638 
03639     return 0;
03640 }
03641 
03642 int getBowlFromDrawer(int jump)
03643 {
03644     getBigBowlOut();
03645     return 0;
03646 }
03647 
03648 
03649 
03650 int closeDrawer(int jump)
03651 {
03652 
03653     btVector3 search(.51, 1.12, .75);
03654 
03655     RobotHead::getInstance()->lookAtThreaded("/map",search);
03656 
03657     Gripper::getInstance(0)->openThreaded();
03658 
03659     OperateHandleController::plateTuckPose();
03660 
03661     RobotDriver::moveBase4(-0.187269, 1.122678, 0.023650, 0.999720);
03662 
03663     tf::Stamped<tf::Pose> drawer_pre_closed_grasp;
03664     drawer_pre_closed_grasp.frame_id_ = "/map";
03665     drawer_pre_closed_grasp.setOrigin(btVector3(0.6, 0.868231, 0.789463));
03666     drawer_pre_closed_grasp.setRotation(btQuaternion(0.694022, 0.007679, 0.072881, 0.716214));
03667 
03668     tf::Stamped<tf::Pose> drawer_closed_grasp;
03669     drawer_closed_grasp.frame_id_ = "/map";
03670     drawer_closed_grasp.setOrigin(btVector3(0.671291, 0.868231, 0.789463));
03671     drawer_closed_grasp.setRotation(btQuaternion(0.694022, 0.007679, 0.072881, 0.716214));
03672 
03673     tf::Stamped<tf::Pose> drawer_open_grasp;
03674     drawer_open_grasp.frame_id_ = "/map";
03675     drawer_open_grasp.setOrigin(btVector3(0.217701, 0.836088, 0.783115));
03676     drawer_open_grasp.setRotation(btQuaternion(0.690426, -0.012174, 0.045241, 0.721884));
03677 
03678     tf::Stamped<tf::Pose> drawer_avoid_open_grasp;
03679     drawer_avoid_open_grasp.frame_id_ = "/map";
03680     drawer_avoid_open_grasp.setOrigin(btVector3(0.217025, 0.635642, 0.783132));
03681     drawer_avoid_open_grasp.setRotation(btQuaternion(0.690618, -0.012459, 0.045518, 0.721678));
03682 
03683     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(drawer_avoid_open_grasp);
03684 
03685     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(drawer_open_grasp);
03686 
03687     //Gripper::getInstance(0)->close();
03688 
03689     //Gripper::getInstance(0)->openThreaded();
03690 
03691     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(drawer_closed_grasp);
03692 
03693     Gripper::getInstance(0)->open();
03694 
03695     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(drawer_pre_closed_grasp);
03696 
03697 
03698     OperateHandleController::plateAttackPose();
03699 
03700     return 0;
03701 
03702 }
03703 
03704 void putBigBowlOntoIsland()
03705 {
03706     btVector3 search(-1.699046, 1.632452, 0.88);
03707 
03708     RobotHead::getInstance()->lookAtThreaded("/map",search);
03709 
03710     RobotDriver::moveBase4(-0.977623, 1.694076, 0.998947, -0.045763);
03711 
03712     tf::Stamped<tf::Pose> bowlPose;
03713     bowlPose.setOrigin(search);
03714     bowlPose.setRotation(Geometry::getRelativeTransform("/map", "base_link").getRotation());
03715     bowlPose.frame_id_ = "/map";
03716 
03717     graspBigBowl(bowlPose, false);
03718 }
03719 
03720 
03721 void putBigBowlOntoTable(btVector3 search, double base_x, double base_y, double base_oz, double base_ow)
03722 {
03723     //btVector3 search(.45, -1.35, .88 - .13);
03724 
03725     RobotHead::getInstance()->lookAtThreaded("/map",search);
03726 
03727     //RobotDriver::moveBase4(-0.977623, 1.694076, 0.998947, -0.045763);
03728     //RobotDriver::moveBase4(0.073525, -0.679463, -0.682670, 0.730720); // putBigBowlOntoTable
03729     RobotDriver::moveBase4(base_x, base_y, base_oz, base_ow);
03730 
03731     tf::Stamped<tf::Pose> bowlPose;
03732     bowlPose.setOrigin(search);
03733     bowlPose.setRotation(Geometry::getRelativeTransform("/map", "base_link").getRotation());
03734     bowlPose.frame_id_ = "/map";
03735 
03736     graspBigBowl(bowlPose, false);
03737 }
03738 
03739 void putBigBowlOntoIsland_heater()
03740 {
03741     btVector3 search(-1.91044139862 + 0.03, 2.9423532486, 0.88);
03742 
03743     RobotHead::getInstance()->lookAtThreaded("/map",search);
03744 
03745     //RobotDriver::moveBase4(-0.977623, 1.694076, 0.998947, -0.045763);
03746 
03747     tf::Stamped<tf::Pose> bowlPose;
03748     bowlPose.setOrigin(search);
03749     bowlPose.setRotation(Geometry::getRelativeTransform("/map", "base_link").getRotation());
03750     bowlPose.frame_id_ = "/map";
03751 
03752     ROS_ERROR("putting it down now");
03753 
03754     graspBigBowl(bowlPose, false);
03755 }
03756 
03757 
03758 
03759 void shake_it()
03760 {
03761 
03762     RobotArm *rarm = RobotArm::getInstance(0);
03763     RobotArm *larm = RobotArm::getInstance(1);
03764     tf::Stamped<tf::Pose> r_tp = rarm->getToolPose("/base_link");
03765     tf::Stamped<tf::Pose> l_tp = larm->getToolPose("/base_link");
03766 
03767     tf::Stamped<tf::Pose> r_tp_map = Geometry::getPoseIn("/map",r_tp);
03768     tf::Stamped<tf::Pose> l_tp_map = Geometry::getPoseIn("/map",l_tp);
03769 
03770     tf::Stamped<tf::Pose> mid,mid_tilted;
03771     mid.frame_id_ = "/base_link";
03772     mid.setOrigin(0.5 * (r_tp.getOrigin() + l_tp.getOrigin()));
03773     mid.setRotation(btQuaternion(0,0,0,1));
03774 
03775     btTransform midbt;
03776     midbt.setOrigin(mid.getOrigin());
03777     midbt.setRotation(mid.getRotation());
03778 
03779     double angle = 0;
03780     double amplitude = 0.35;
03781     double angle_increment = M_PI / 5.0;
03782     double amplitude_diminisher = amplitude / ((2 * M_PI) / angle_increment);
03783 
03784     ROS_ERROR("angle_increment %f amplitude_diminisher %f", angle_increment, amplitude_diminisher);
03785 
03786     ros::Rate r(2);
03787     while (angle < 2 * M_PI)
03788     {
03789         mid_tilted =  Geometry::rotateAroundPose(mid,mid,cos(angle) * amplitude,sin(angle) * amplitude,0);
03790         btTransform mid_tiltedbt;
03791         mid_tiltedbt.setOrigin(mid_tilted.getOrigin());
03792         mid_tiltedbt.setRotation(mid_tilted.getRotation());
03793 
03794         //announce("mid", mid);
03795 
03796         //announce("mid", mid_tilted);
03797 
03798         r.sleep();
03799 
03800         btTransform r,l;
03801 
03802         r = r_tp;
03803         l = l_tp;
03804 
03805         l = mid_tiltedbt * mid.inverseTimes(l);
03806         r = mid_tiltedbt * mid.inverseTimes(r);
03807 
03808         tf::Stamped<tf::Pose> act_l_tp = l_tp;
03809         act_l_tp.setOrigin(l.getOrigin());
03810         act_l_tp.setRotation(l.getRotation());
03811         tf::Stamped<tf::Pose> act_r_tp = r_tp;
03812         act_r_tp.setOrigin(r.getOrigin());
03813         act_r_tp.setRotation(r.getRotation());
03814 
03815         //announce("r",act_r_tp);
03816         //announce("l",act_l_tp);
03817 
03818         RobotArm::moveBothArms(act_l_tp, act_r_tp, 0, false);
03819 
03820         ros::Duration(0.5).sleep();
03821 
03822         angle += angle_increment;
03823         amplitude -= amplitude_diminisher;
03824     }
03825 
03826 
03827     RobotArm::moveBothArms(l_tp_map,r_tp_map);
03828     /*
03829     r
03830         x: -1.8773600001
03831         y: 3.0805883138
03832         z: 0.946
03833       orientation:
03834         x: -0.320103064305
03835         y: 0.630521636559
03836         z: -0.39629712381
03837         w: 0.585598056446
03838 
03839       l
03840 
03841         x: -1.91300054796
03842         y: 2.81695646567
03843         z: 0.947
03844       orientation:
03845         x: -0.583435311095
03846         y: -0.362567567153
03847         z: 0.594931073057
03848         w: 0.417378743261
03849     */
03850 
03851     tf::Stamped<tf::Pose> r_putdown;
03852     r_putdown.frame_id_ = "/map";
03853     r_putdown.setOrigin(btVector3( -1.8773600001, 3.0805883138, 0.946));
03854     r_putdown.setRotation(btQuaternion(-0.320103064305,0.630521636559,   -0.39629712381,  0.585598056446));
03855 
03856     tf::Stamped<tf::Pose> l_putdown;
03857     l_putdown.frame_id_ = "/map";
03858     l_putdown.setOrigin(btVector3(-1.91300054796, 2.81695646567, 0.947));
03859     l_putdown.setRotation(btQuaternion(-0.583435311095, -0.362567567153, 0.594931073057, 0.417378743261));
03860 
03861     //r_putdown = hack_r;
03862     //l_putdown = hack_l;
03863 
03864     tf::Stamped<tf::Pose> r_putdown_exit = r_putdown;
03865     r_putdown_exit.getOrigin() += btVector3(0,0.1,0.0);
03866     tf::Stamped<tf::Pose> l_putdown_exit = l_putdown;
03867     l_putdown_exit.getOrigin() -= btVector3(0,0.1,0.0);
03868 
03869 
03870     RobotArm::getInstance(0)->raise_elbow = true;
03871     RobotArm::getInstance(1)->raise_elbow = true;
03872 
03873     RobotArm::moveBothArms(l_putdown,r_putdown);
03874 
03875     if (0) // re-align to get pot to the right spot
03876     {
03877         // rosrun tf static_transform_publisher -1.899720 2.966096 0.960000 0.000000 0.000000 0.769873 0.638197 /map pot 100
03878         btVector3 potTarget = btVector3(-1.899720, 2.966096, 0.960000);
03879 
03880         tf::Stamped<tf::Pose> potPose, pre_grasp_r, grasp_r, pre_grasp_l, grasp_l;
03881 
03882         ros::Duration(0.5).sleep();
03883 
03884         bool gotPotPose = false;
03885         while (!gotPotPose)
03886             gotPotPose = getPotPose(potPose, pre_grasp_l, grasp_l, pre_grasp_r, grasp_r, true);
03887 
03888         btVector3 diff = potTarget - potPose.getOrigin();
03889 
03890         ROS_ERROR("POT DIFF %f %f", diff.x(), diff.y());
03891 
03892         diff.setZ(0);
03893 
03894         tf::Stamped<tf::Pose> radj, ladj;
03895         radj = r_putdown;
03896         ladj = l_putdown;
03897         radj.getOrigin() += diff;
03898         ladj.getOrigin() += diff;
03899 
03900         l_putdown_exit.getOrigin() += diff;
03901         r_putdown_exit.getOrigin() += diff;
03902 
03903         tf::Stamped<tf::Pose> l_putdown_exit_high, r_putdown_exit_high;
03904         l_putdown_exit_high = l_putdown_exit;
03905         r_putdown_exit_high = r_putdown_exit;
03906 
03907         l_putdown_exit_high.getOrigin() += btVector3(0,0,.1);
03908         r_putdown_exit_high.getOrigin() += btVector3(0,0,.1);
03909 
03910         {
03911             tf::Stamped<tf::Pose> result;
03912             std::vector<int> arm;
03913             std::vector<tf::Stamped<tf::Pose> > goal;
03914             arm.push_back(1);
03915             arm.push_back(1);
03916             arm.push_back(1);
03917             arm.push_back(0);
03918             arm.push_back(0);
03919             arm.push_back(0);
03920             goal.push_back(ladj);
03921             goal.push_back(l_putdown_exit);
03922             goal.push_back(l_putdown_exit_high);
03923             goal.push_back(radj);
03924             goal.push_back(r_putdown_exit);
03925             goal.push_back(r_putdown_exit_high);
03926             //bool RobotArm::findBaseMovement(tf::Stamped<tf::Pose> &result, std::vector<int> arm, std::vector<tf::Stamped<tf::Pose> > goal, bool drive, bool reach)
03927 
03928             RobotArm::findBaseMovement(result, arm, goal, true, false);
03929         }
03930 
03931         RobotArm::moveBothArms(ladj,radj);
03932 
03933         Gripper::getInstance(0)->openThreaded(0.055);
03934         Gripper::getInstance(1)->openThreaded(0.055);
03935 
03936         RobotArm::moveBothArms(l_putdown_exit,r_putdown_exit);
03937 
03938         OperateHandleController::openGrippers(false);
03939 
03940         RobotArm::moveBothArms(l_putdown_exit_high,r_putdown_exit_high);
03941 
03942         // 9x9x9x
03943     }
03944     else
03945     {
03946         Gripper::getInstance(0)->openThreaded(0.055);
03947         Gripper::getInstance(1)->openThreaded(0.055);
03948 
03949         RobotArm::moveBothArms(l_putdown_exit,r_putdown_exit);
03950 
03951         Gripper::getInstance(0)->openThreaded(0.055);
03952         Gripper::getInstance(1)->openThreaded(0.055);
03953 
03954         l_putdown_exit.getOrigin() += btVector3(0,0,.1);
03955         r_putdown_exit.getOrigin() += btVector3(0,0,.1);
03956 
03957         RobotArm::moveBothArms(l_putdown_exit,r_putdown_exit);
03958     }
03959 
03960     RobotArm::getInstance(0)->raise_elbow = false;
03961     RobotArm::getInstance(1)->raise_elbow = false;
03962 
03963     OperateHandleController::plateAttackPose();
03964 
03965 
03966 }
03967 
03968 
03969 int pourReadyPopcornTable(int jump)
03970 {
03971 
03972     RobotHead::getInstance()->lookAtThreaded("/map",.27, -1.33, 0.899);
03973 
03974     RobotDriver::moveBase4(.3098 + diff.x() , -0.716 + diff.y(), -0.659599, 0.751613);
03975 
03976     //pourReadyPopcorn_(0.279824 + 0.025, -0.691109 -.03, -0.659599, 0.751613, 0.12);
03977 
03978     //pourReadyPopcorn_(0.279824 + xshift, -0.691109 + yshift, -0.659599, 0.751613, 0.12);
03979     tf::Stamped<tf::Pose> bowlPose = getBigBowlPose(btVector3(.27,-1.35, .75));
03980     bowlPose = getBigBowlPose(btVector3(.27,-1.35, .75));
03981 
03982     diff = bowlPose.getOrigin() - btVector3(.271, -1.33, 1.00);
03983 
03984     //ROS_ERROR("BOWL DIFF %f %f", diff.x(), diff.y());
03985     //for bowl .271 -1.33 1.00
03986 
03987 
03988     if (0)
03989     {
03990 
03991         bowlPose = Geometry::getPoseIn("map", bowlPose);
03992         RobotDriver::moveBase4(.3098 + diff.x() , -0.716 + diff.y(), -0.659599, 0.751613);
03993         bowlPose = Geometry::getPoseIn("base_link", bowlPose);
03994         printPose("Bowl Pose", bowlPose);
03995         exit(0);
03996         //0.600358 -0.125386 0.736068 -0.001765 -0.002289 -0.004847 0.999984
03997         //0.602894 -0.119965 0.736622 0.000888 -0.000972 0.004101 0.999991
03998     }
03999 
04000     pourReadyPopcorn_(.3098 + diff.x() , -0.716 + diff.y(), -0.659599, 0.751613, 0.12);
04001 
04002     //pourReadyPopcorn_(-1.094190, 1.6,  0.997743, -0.067087);
04003     return 0;
04004 }
04005 
04006 
04007 
04008 int current(int argc, char** argv)
04009 {
04010 
04011     ros::NodeHandle node_handle;
04012 
04013     development_tools(argc, argv);
04014 
04015     //run the whole damn thing
04016     if (atoi(argv[1]) == -700)
04017     {
04018         getPotOut(0);
04019         getLidAndBowl(-2);
04020         Current::removeLid(0);
04021         takepot(1);
04022     }
04023 
04024     if (atoi(argv[1]) == -701)
04025     {
04026 
04027         int jump = 0;
04028 
04029         switch (atoi(argv[2]))
04030         {
04031 
04032         case 0 : //0:15
04033             Current::getPotOut_openDrawer(jump);
04034 
04035         case 1 : //0:54 pot on // 1:30
04036             Current::getPotOut_pickPlacePot(jump);
04037 
04038         case 2 :
04039             if ((argc > 3) && (atoi(argv[3])==1))
04040                 RobotDriver::moveBase4(-0.346670, 2.307890, 0.757347, 0.653002);
04041 
04042         case 3 :
04043             Current::manipulateKnob(1); // 1:54
04044             //Current::manipulateKnob(-1);
04045 
04046         case 4 :
04047             Current::openDrawerUnderOven(jump); // 2:23
04048 
04049         case 5 :
04050             Current::getLidOut(jump); // 2:45
04051 
04052         case 6 :
04053             Current::getBowlOut(jump); // 3:15
04054 
04055         case 7 :
04056             Current::closeDrawerUnderOven(jump); // 3:30
04057 
04058         case 8 :
04059             if ((argc > 3) && (atoi(argv[3])==1))
04060                 RobotDriver::moveBase4(-0.346670, 2.307890, 0.757347, 0.653002);
04061         case 9 :
04062             Current::pourPopCorn(jump); //3:54
04063 
04064         case 10 :
04065             Current::putLidOn(jump); //4:10
04066             RobotDriver::moveBase4(-0.904410, 2.493494, 0.998334, -0.057692); // for waiting
04067 
04068         case 11 :
04069             Current::pushPot(jump); // 4:31
04070 
04071         case 12 :
04072             if ((argc > 3) && (atoi(argv[3])==1))
04073                 RobotDriver::moveBase4(-0.346670, 2.307890, 0.757347, 0.653002);
04074             Current::manipulateKnob(-1); // 5:13
04075 
04076         case 13 :
04077             Current::removeLid(jump); //5:51
04078 
04079         case 14 :
04080             Current::takePotFromIsland(jump); // 6:06
04081 
04082         case 15 :
04083             //Current::pourReadyPopcorn(jump); // 6:27 pour 6:50 pot down
04084             fotopourReadyPopcorn_(-1, 1.6,  0.997743, -0.067087, .0 ,  2.4,false);
04085         }
04086     }
04087 
04088 
04089     if (atoi(argv[1]) == -616)
04090     {
04091         tf::Stamped<tf::Pose> r,l,mid;
04092 
04093 
04094         r = RobotArm::getInstance(0)->getToolPose("/base_link");
04095         l = RobotArm::getInstance(1)->getToolPose("/base_link");
04096 
04097         mid = r;
04098         mid.setRotation(btQuaternion(0,0,0,1));
04099         mid.setOrigin(0.5 * (r.getOrigin() + l.getOrigin()));
04100 
04101         printPose("r" , r);
04102         printPose("l" , l);
04103 
04104         r =  Geometry::rotateAroundPose(r, mid, atof(argv[2]),atof(argv[3]), atof(argv[4]));
04105         l =  Geometry::rotateAroundPose(l, mid, atof(argv[2]),atof(argv[3]), atof(argv[4]));
04106 
04107         printPose("r" , r);
04108         printPose("l" , l);
04109 
04110         RobotArm::moveBothArms(l,r);
04111     }
04112 
04113     /*
04114     //test cop based pot detection
04115     if (atoi(argv[1]) == -617)
04116     {
04117         ros::Publisher pose_pub = node_handle.advertise<geometry_msgs::PoseStamped>( "/cop_detection", 0 , true);
04118         ros::Publisher best_pose_pub = node_handle.advertise<geometry_msgs::PoseStamped>( "/cop_detection_best", 0, true );
04119 
04120         std::vector<tf::Stamped<tf::Pose> > poses;
04121         std::vector<double > votes;
04122 
04123         while (ros::ok())
04124         {
04125             tf::Stamped<tf::Pose> pose;
04126             pose = OperateHandleController::getCopPose("PotWW", "/openni_rgb_optical_frame");
04127             ros::Duration(0.05).sleep();
04128             if (pose.frame_id_ == "NO_ID_STAMPED_DEFAULT_CONSTRUCTION")
04129             {
04130                 ROS_ERROR("dropped %s", pose.frame_id_.c_str());
04131                 continue;
04132             }
04133 
04134             if (pose.getOrigin().length() < 0.1)
04135                 continue;
04136             pose = Geometry::getPoseIn("/map", pose);
04137 
04138             geometry_msgs::PoseStamped pose_stamped, best_pose_stamped;
04139 
04140             tf::poseStampedTFToMsg(pose,pose_stamped);
04141             pose_pub.publish(pose_stamped);
04142             ros::Duration(0.05).sleep();
04143 
04144             if ((btVector3(-1.836059, 2.612557, 1.016513) - pose.getOrigin()).length() > 0.5)
04145                 continue;
04146 
04147             {
04148                 poses.push_back(pose);
04149                 votes.push_back(.0);
04150 
04151                 ROS_INFO("tick");
04152 
04153                 if (poses.size() > 1)
04154                 {
04155                     for (size_t k = 0; k < poses.size(); ++k)
04156                     {
04157                         votes[k] = 0;
04158                     }
04159                     double max_score = 0;
04160                     for (size_t k = 0; k < poses.size(); ++k)
04161                     {
04162                         for (size_t j = 0; j < poses.size(); ++j)
04163                         {
04164                             btTransform a,b,c;
04165                             a.setOrigin(poses[k].getOrigin());
04166                             a.setRotation(poses[k].getRotation());
04167                             b.setOrigin(poses[j].getOrigin());
04168                             b.setRotation(poses[j].getRotation());
04169                             c = b.inverseTimes(a);
04170                             double score = c.getOrigin().length() + c.getRotation().getAngle() * .001;
04171                             votes[j] += score;
04172                             votes[k] += score;
04173                             if (votes[k] > max_score)
04174                                 max_score = votes[k];
04175                             if (votes[j] > max_score)
04176                                 max_score = votes[j];
04177                         }
04178                     }
04179                     double minscore = 1000000;
04180                     tf::Stamped<tf::Pose> best;
04181                     for (size_t k = 0; k < poses.size(); ++k)
04182                     {
04183                         double act = votes[k] / max_score;
04184                         ROS_INFO("%zu score %f: %f", k, votes[k], act);
04185                         if (act < minscore)
04186                         {
04187                             minscore = act;
04188                             best = poses[k];
04189                         }
04190                     }
04191                     tf::poseStampedTFToMsg(best,best_pose_stamped);
04192                     ROS_INFO("min score = %f", minscore);
04193                     best_pose_pub.publish(best_pose_stamped);
04194                     ros::Duration(0.05).sleep();
04195                 }
04196             }
04197         }
04198     }
04199     */
04200 
04201 
04203     if (atoi(argv[1]) == -619)
04204     {
04205 
04206         RobotHead::getInstance()->lookAtThreaded("/map",0.527460,2.304037,0.832586);
04207 
04208         switch (atoi(argv[2]))
04209         {
04210 
04211         case 0 :
04212         {
04213 
04214             Gripper::getInstance(1)->openThreaded();
04215 
04216             btVector3 min(0.614, 1.945, 0.84);
04217             btVector3 max(0.796, 2.245, 0.92);
04218             btVector3 center;
04219             double radius;
04220             getCircle(min, max, center, radius);
04221 
04222             ROS_INFO("center %f %f %f", center.x(), center.y(), center.z());
04223 
04224             tf::Stamped<tf::Pose> grasp;
04225             grasp.setOrigin(center + btVector3(0,0.08,-0.02));
04226             grasp.setRotation(btQuaternion(-0.128, 0.591, -0.167, 0.778));
04227             grasp.frame_id_ = "/map";
04228 
04229             tf::Stamped<tf::Pose> grasphigh = grasp;
04230             grasphigh.getOrigin() += btVector3(0,0,0.1);
04231 
04232             ROS_INFO("grasphigh %f %f %f", grasphigh.getOrigin().x(), grasphigh.getOrigin().y(), grasphigh.getOrigin().z());
04233 
04234             RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(grasphigh);
04235             RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(grasp);
04236 
04237             Gripper::getInstance(1)->close();
04238             RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(grasphigh);
04239             RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(grasp);
04240 
04241             Gripper::getInstance(1)->open();
04242             RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(grasphigh);
04243 
04244 
04245             OperateHandleController::plateAttackPose();
04246 
04247         }
04248 
04249         }
04250 
04251         /*
04252         - Translation: [0.614, 1.945, 0.807]
04253         - Rotation: in Quaternion [-0.514, 0.481, 0.476, 0.527]
04254                     in RPY [-1.470, 1.487, 0.084]
04255         ruehr@pr2b:~/sshsandbox/ias_drawer_executive$ rosrun tf tf_echo map l_gripper_tool_frame
04256         At time 1319040599.730
04257         - Translation: [0.796, 2.245, 0.810]
04258         - Rotation: in Quaternion [0.719, 0.008, -0.695, -0.008]
04259                     in RPY [-2.569, 1.529, 0.573]
04260                     */
04261 
04262     }
04263 
04264 
04265     if (atoi(argv[1]) == -620)
04266     {
04267 
04268         if (atoi(argv[2]) == 0) {
04269 
04270             tf::Stamped<tf::Pose> potPose, pre_grasp_r, grasp_r, pre_grasp_l, grasp_l;
04271 
04272             bool via_lid = false;
04273             if (argc > 3)
04274                 via_lid = true;
04275 
04276             while (ros::ok())
04277             {
04278                 bool gotPotPose = false;
04279                 while (!gotPotPose)
04280                     gotPotPose = getPotPose(potPose, pre_grasp_l, grasp_l, pre_grasp_r, grasp_r, via_lid);
04281             }
04282         } else {
04283             //9x9x9x
04284             ROS_INFO("Dip");
04285             PotLocalizer *pot = new PotLocalizer();
04286 
04287             double table_height = 0.865;
04288             tf::Vector3 roi_min(-2.06, 2.41, table_height);
04289             tf::Vector3 roi_max(-1.7, 3.09, table_height + .4);
04290 
04291             tf::Stamped<tf::Pose> pose;
04292 
04293             ros::Rate rt(5);
04294 
04295             while (ros::ok())
04296             {
04297                 ObjectLocalizer::localize("Pot",&pose, 1, Keywords("min",roi_min)("max",roi_max));
04298                 rt.sleep();
04299             }
04300         }
04301     }
04302 
04303 
04304     if (atoi(argv[1]) == -621)
04305     {
04306 
04307         ros::Publisher cloud_pub = node_handle.advertise<sensor_msgs::PointCloud2>("/debug_cloud",0,true);
04308 
04309 
04310         while (ros::ok())
04311         {
04312 
04313 
04314             sensor_msgs::PointCloud2 pc = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/kinect/cloud_throttled"));
04315 
04316             pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
04317 
04318             tf::Stamped<tf::Pose> net_stamped = Geometry::getPose("/map","/openni_rgb_optical_frame");
04319             tf::Transform net_transform;
04320             net_transform.setOrigin(net_stamped.getOrigin());
04321             net_transform.setRotation(net_stamped.getRotation());
04322 
04323             sensor_msgs::PointCloud2 pct; //in map frame
04324 
04325             pcl_ros::transformPointCloud("/map",net_transform,pc,pct);
04326             pct.header.frame_id = "/map";
04327 
04328             pcl::fromROSMsg(pct, *cloud);
04329 
04330             pcl::PointCloud<pcl::PointXYZRGB>::Ptr boundary (new pcl::PointCloud<pcl::PointXYZRGB>);
04331             pcl::PointCloud<pcl::PointXYZRGB>::Ptr not_boundary (new pcl::PointCloud<pcl::PointXYZRGB>);
04332             std::vector<int> edge_indices;
04333 
04334             pcl::PointCloud<pcl::PointXYZRGB>::Ptr inBox (new pcl::PointCloud<pcl::PointXYZRGB>);
04335             {
04336 
04337                 Eigen::Vector4f min_pt, max_pt;
04338 
04339 
04340 
04341                 min_pt = Eigen::Vector4f(0.33,.82,0.75, 1);
04342                 max_pt = Eigen::Vector4f(.72,1.4,  .8, 1);
04343                 /*
04344                 {
04345                     tf::Stamped<tf::Pose> res;
04346                     res.setOrigin(min);
04347                     res.setRotation(btQuaternion(0,0,0,1));
04348                     res.frame_id_ = "/map";
04349 
04350                     geometry_msgs::PoseStamped res_msg;
04351                     tf::poseStampedTFToMsg(res,res_msg);
04352 
04353                     pose_pubr.publish(res_msg);
04354                 }
04355 
04356                 {
04357                     tf::Stamped<tf::Pose> res;
04358                     res.setOrigin(max);
04359                     res.setRotation(btQuaternion(0,0,0,1));
04360                     res.frame_id_ = "/map";
04361 
04362                     geometry_msgs::PoseStamped res_msg;
04363                     tf::poseStampedTFToMsg(res,res_msg);
04364 
04365                     pose_publ.publish(res_msg);
04366                 }*/
04367 
04368 
04369                 ROS_INFO("min %f %f %f" ,min_pt[0],min_pt[1],min_pt[2]);
04370                 ROS_INFO("max %f %f %f" ,max_pt[0],max_pt[1],max_pt[2]);
04371 
04372                 ROS_INFO("cloud size : %zu", cloud->points.size());
04373 
04374                 boost::shared_ptr<std::vector<int> > indices( new std::vector<int> );
04375                 pcl::getPointsInBox(*cloud,min_pt,max_pt,*indices);
04376 
04377                 pcl::ExtractIndices<pcl::PointXYZRGB> ei;
04378                 ei.setInputCloud(cloud);
04379                 ei.setIndices(indices);
04380                 ei.filter(*inBox);
04381             }
04382 
04383 
04384             findBoundary2(*inBox,*boundary,*not_boundary, edge_indices );
04385 
04386             sensor_msgs::PointCloud2 out; //in map frame
04387 
04388             pcl::toROSMsg(*boundary,out);
04389             out.header.frame_id = "/map";
04390 
04391             btVector3 center;
04392             double radius_goal = .1356;
04393             double radius = radius_goal;
04394             getCircleFromCloud( *boundary, center, radius);
04395 
04396             //if (fabs(radius - radius_goal) > 0.01)
04397             //   continue
04398 
04399             cloud_pub.publish(out);
04400 
04401             ROS_INFO("published");
04402 
04403         }
04404 
04405     }
04406 
04407 
04408     if (atoi(argv[1]) == -622)
04409     {
04410 
04411         //ros::Publisher cloud_pub = node_handle.advertise<sensor_msgs::PointCloud2>("/debug_cloud",0,true);
04412         ros::Publisher pose_pub = node_handle.advertise<geometry_msgs::PoseStamped>( "/cop_detection", 0 , true);
04413         ros::Publisher pose_pubr = node_handle.advertise<geometry_msgs::PoseStamped>( "/cop_detectionr", 0 , true);
04414         ros::Publisher pose_publ = node_handle.advertise<geometry_msgs::PoseStamped>( "/cop_detectionl", 0 , true);
04415 
04416         while (ros::ok())
04417         {
04418             sensor_msgs::PointCloud2 pc = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/kinect/cloud_throttled"));
04419 
04420             pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
04421 
04422             tf::Stamped<tf::Pose> net_stamped = Geometry::getPose("/map","/openni_rgb_optical_frame");
04423             tf::Transform net_transform;
04424             net_transform.setOrigin(net_stamped.getOrigin());
04425             net_transform.setRotation(net_stamped.getRotation());
04426 
04427             sensor_msgs::PointCloud2 pct; //in map frame
04428 
04429             pcl_ros::transformPointCloud("/map",net_transform,pc,pct);
04430             pct.header.frame_id = "/map";
04431 
04432             pcl::fromROSMsg(pct, *cloud);
04433 
04434             pcl::PointCloud<pcl::PointXYZRGB>::Ptr boundary (new pcl::PointCloud<pcl::PointXYZRGB>);
04435             pcl::PointCloud<pcl::PointXYZRGB>::Ptr not_boundary (new pcl::PointCloud<pcl::PointXYZRGB>);
04436             std::vector<int> edge_indices;
04437 
04438             pcl::PointCloud<pcl::PointXYZRGB>::Ptr inBox (new pcl::PointCloud<pcl::PointXYZRGB>);
04439             {
04440 
04441                 Eigen::Vector4f min_pt, max_pt;
04442 
04443                 //min_pt = Eigen::Vector4f(0.33,.82,0.75, 1);
04444                 //max_pt = Eigen::Vector4f(.72,1.4,  .8, 1);
04445 
04446                 btVector3 min(0.33,.82,.75);
04447                 btVector3 max(1.2, 1.4,.9);
04448 
04449 
04450                 min_pt = Eigen::Vector4f(min.x(),min.y(),min.z(), 1);
04451                 max_pt = Eigen::Vector4f(max.x(),max.y(),max.z(), 1);
04452 
04453                 min_pt = Eigen::Vector4f(0.33,.82,0.75, 1);
04454                 max_pt = Eigen::Vector4f(.72,1.4,  .8, 1);
04455 
04456 
04457 
04458                 {
04459                     tf::Stamped<tf::Pose> res;
04460                     res.setOrigin(min);
04461                     res.setRotation(btQuaternion(0,0,0,1));
04462                     res.frame_id_ = "/map";
04463 
04464                     geometry_msgs::PoseStamped res_msg;
04465                     tf::poseStampedTFToMsg(res,res_msg);
04466 
04467                     pose_pubr.publish(res_msg);
04468                 }
04469 
04470                 {
04471                     tf::Stamped<tf::Pose> res;
04472                     res.setOrigin(max);
04473                     res.setRotation(btQuaternion(0,0,0,1));
04474                     res.frame_id_ = "/map";
04475 
04476                     geometry_msgs::PoseStamped res_msg;
04477                     tf::poseStampedTFToMsg(res,res_msg);
04478 
04479                     pose_publ.publish(res_msg);
04480                 }
04481 
04482 
04483                 ROS_INFO("min %f %f %f" ,min_pt[0],min_pt[1],min_pt[2]);
04484                 ROS_INFO("max %f %f %f" ,max_pt[0],max_pt[1],max_pt[2]);
04485 
04486                 ROS_INFO("cloud size : %zu", cloud->points.size());
04487 
04488                 boost::shared_ptr<std::vector<int> > indices( new std::vector<int> );
04489                 pcl::getPointsInBox(*cloud,min_pt,max_pt,*indices);
04490 
04491                 pcl::ExtractIndices<pcl::PointXYZRGB> ei;
04492                 ei.setInputCloud(cloud);
04493                 ei.setIndices(indices);
04494                 ei.filter(*inBox);
04495             }
04496 
04497 
04498             findBoundary2(*inBox,*boundary,*not_boundary, edge_indices );
04499 
04500             //btVector3 center;
04501             //double radius_goal = .1356;
04502             //double radius = radius_goal;
04503             std::vector<btVector3> center;
04504             std::vector<double> radius;
04505             std::vector<int> numinliers;;
04506 
04507             //getCircleFromCloud( *boundary, center, radius);
04508             getCirclesFromCloud(*boundary, .136, .02,
04509                                 center,
04510                                 radius,
04511                                 numinliers,
04512                                 2);
04513 
04514             //if (fabs(radius - radius_goal) > 0.01)
04515             //   continue
04516 
04517 
04518 
04519             ROS_INFO("published");
04520 
04521         }
04522 
04523     }
04524 
04525 
04526     if (atoi(argv[1]) == -623)
04527     {
04528 
04529         ros::Publisher cloud_pub = node_handle.advertise<sensor_msgs::PointCloud2>("/debug_cloud",0,true);
04530 
04531         rosbag::Bag bag;
04532         bag.open("clouds.bag", rosbag::bagmode::Write);
04533 
04534 
04535         while (ros::ok())
04536         {
04537 
04538             sensor_msgs::PointCloud2 pc = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/kinect/cloud_throttled"));
04539 
04540             pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
04541 
04542             tf::Stamped<tf::Pose> net_stamped = Geometry::getPose("/map","/openni_rgb_optical_frame");
04543             tf::Transform net_transform;
04544             net_transform.setOrigin(net_stamped.getOrigin());
04545             net_transform.setRotation(net_stamped.getRotation());
04546 
04547             sensor_msgs::PointCloud2 pct; //in map frame
04548 
04549             pcl_ros::transformPointCloud("/map",net_transform,pc,pct);
04550             pct.header.frame_id = "/map";
04551 
04552             pcl::fromROSMsg(pct, *cloud);
04553 
04554             pcl::PointCloud<pcl::PointXYZRGB>::Ptr boundary (new pcl::PointCloud<pcl::PointXYZRGB>);
04555             pcl::PointCloud<pcl::PointXYZRGB>::Ptr not_boundary (new pcl::PointCloud<pcl::PointXYZRGB>);
04556             std::vector<int> edge_indices;
04557 
04558 
04559             findBoundary2(*cloud,*boundary,*not_boundary, edge_indices );
04560 
04561             sensor_msgs::PointCloud2 out; //in map frame
04562 
04563             pcl::toROSMsg(*boundary,out);
04564             out.header.frame_id = "/map";
04565 
04566             //btVector3 center;
04567             //double radius_goal = .1356;
04568             //double radius = radius_goal;
04569             //getCircleFromCloud( *boundary, center, radius);
04570 
04571             //if (fabs(radius - radius_goal) > 0.01)
04572             //   continue
04573 
04574             cloud_pub.publish(out);
04575 
04576             ROS_INFO("published");
04577 
04578 
04579             bag.write("/kinect/cloud_throttled", ros::Time::now(), pct);
04580             bag.write("/boundaries", ros::Time::now(), out);
04581 
04582 
04583         }
04584         bag.close();
04585 
04586     }
04587 
04588 
04589     if (atoi(argv[1]) == -624)
04590     {
04591         while (ros::ok())
04592         {
04593 
04594             tf::Stamped<tf::Pose> lid;
04595             getLidPose(lid);
04596             printPose("lid", lid);
04597 
04598         }
04599     }
04600 
04601 
04602     if (atoi(argv[1]) == -625)
04603     {
04604 
04605         while (ros::ok())
04606         {
04607             tf::Stamped<tf::Pose> a;
04608             a.frame_id_ = "base_link";
04609             a.setOrigin(btVector3(0.388559, -0.559736, 1.131493));
04610             a.setRotation(btQuaternion(-0.623655, -0.207955, -0.531256, 0.534393));
04611 
04612             RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(a);
04613 
04614             RobotArm::getInstance(0)->evil_switch = true;
04615 
04616 
04617             tf::Stamped<tf::Pose> b;
04618             b.frame_id_ = "base_link";
04619             b.setOrigin(btVector3(0.389019, 0.000308, 1.131259));
04620             b.setRotation(btQuaternion(-0.624204, -0.207787, -0.530962, 0.534110));
04621 
04622             RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(b);
04623 
04624             bool reached = false;
04625             while (!reached)
04626             {
04627                 reached = (RobotArm::getInstance(0)->getToolPose(b.frame_id_.c_str()).getOrigin() - b.getOrigin()).length() < 0.05;
04628                 ros::Duration(0.001).sleep();
04629             }
04630 
04631             ROS_INFO("DIST : %f ", (RobotArm::getInstance(0)->getToolPose(b.frame_id_.c_str()).getOrigin() - b.getOrigin()).length());
04632 
04633             RobotArm::getInstance(0)->evil_switch = false;
04634         }
04635 
04636     }
04637 
04638     if (atoi(argv[1]) == -626)
04639     {
04640         tf::Stamped<tf::Pose> a;
04641         a.frame_id_ = "base_link";
04642         a.setOrigin(btVector3(0.388559, -0.559736, 1.131493));
04643         a.setRotation(btQuaternion(-0.623655, -0.207955, -0.531256, 0.534393));
04644 
04645         tf::Stamped<tf::Pose> b;
04646         b.frame_id_ = "base_link";
04647         b.setOrigin(btVector3(0.389019, 0.000308, 1.131259));
04648         b.setRotation(btQuaternion(-0.624204, -0.207787, -0.530962, 0.534110));
04649 
04650         while (ros::ok())
04651         {
04652             RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(a);
04653 
04654             RobotArm::getInstance(0)->universal_move_toolframe_ik_pose_tolerance(b,0.25);
04655         }
04656     }
04657 
04658 
04659     if (atoi(argv[1]) == -627)
04660     {
04661         Current::putLidOn(17);
04662     }
04663 
04664 
04665 
04666     if (atoi(argv[1]) == -628)
04667     {
04668         tf::Stamped<tf::Pose> r,l,mid;
04669 
04670         r = RobotArm::getInstance(0)->getToolPose("/base_link");
04671 
04672         mid = r;
04673         mid.setRotation(btQuaternion(0,0,0,1));
04674 
04675         printPose("r" , r);
04676 
04677         r =  Geometry::rotateAroundPose(r, mid, atof(argv[2]),atof(argv[3]), atof(argv[4]));
04678 
04679         printPose("r" , r);
04680 
04681         //RobotArm::moveBothArms(l,r);
04682         RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(r);
04683     }
04684 
04685 
04686     if (atoi(argv[1]) == -629)
04687     {
04688         tf::Stamped<tf::Pose> r,l,mid;
04689 
04690 
04691         r = RobotArm::getInstance(0)->getToolPose("/base_link");
04692         l = RobotArm::getInstance(1)->getToolPose("/base_link");
04693 
04694         mid = r;
04695         mid.setRotation(btQuaternion(0,0,0,1));
04696         mid.setOrigin(0.5 * (r.getOrigin() + l.getOrigin()));
04697 
04698         printPose("r" , r);
04699         printPose("l" , l);
04700 
04701         r =  Geometry::rotateAroundPose(r, mid, atof(argv[2]),atof(argv[3]), atof(argv[4]));
04702         l =  Geometry::rotateAroundPose(l, mid, -atof(argv[2]),-atof(argv[3]), -atof(argv[4]));
04703 
04704         printPose("r" , r);
04705         printPose("l" , l);
04706 
04707         RobotArm::moveBothArms(l,r);
04708     }
04709 
04710 
04711 
04712     if (atoi(argv[1]) == -630)
04713     {
04714         Current::salt(0);
04715     }
04716 
04717 
04718     if (atoi(argv[1]) == -631) // -1.7 1.66 .88 just detect
04719     {
04720         OperateHandleController::plateAttackPose();
04721 
04722         btVector3 search(atof(argv[2]),atof(argv[3]), atof(argv[4]));
04723         tf::Stamped<tf::Pose> bowlPose = getBigBowlPose(search);
04724         //graspBigBowl(bowlPose, true);
04725 
04726         //OperateHandleController::plateCarryPose();
04727 
04728         //graspBigBowl(bowlPose, false);
04729     }
04730 
04731     if (atoi(argv[1]) == -632) // -1.7 1.66 .88
04732     {
04733         OperateHandleController::plateAttackPose();
04734 
04735         btVector3 search(atof(argv[2]),atof(argv[3]), atof(argv[4]));
04736         tf::Stamped<tf::Pose> bowlPose = getBigBowlPose(search);
04737 
04738         graspBigBowl(bowlPose, true);
04739 
04740         OperateHandleController::plateCarryPose();
04741     }
04742 
04743     if (atoi(argv[1]) == -633) // put down
04744     {
04745         btVector3 search(atof(argv[2]),atof(argv[3]), atof(argv[4]));
04746         tf::Stamped<tf::Pose> bowlPose;
04747         bowlPose.setOrigin(search);
04748         bowlPose.setRotation(Geometry::getRelativeTransform("/map", "base_link").getRotation());
04749         bowlPose.frame_id_ = "/map";
04750 
04751         graspBigBowl(bowlPose, false);
04752     }
04753 
04754     // pour ready popcorn
04755     if (atoi(argv[1]) == -635) // -1.7 1.66 .88
04756     {
04757 
04758         RobotDriver::moveBase4(-1.154662, 1.573712, 0.998081, -0.061899);
04759 
04760         btVector3 search(atof(argv[2]),atof(argv[3]), atof(argv[4]));
04761         tf::Stamped<tf::Pose> bowlPose = getBigBowlPose(search);
04762 
04763         {
04764             tf::Stamped<tf::Pose> inBase = Geometry::getPoseIn("base_link", bowlPose);
04765             printPose("BowlPoseinbase", bowlPose);
04766             // define where the bowl should be in base coords to make the trajectory work for it
04767             tf::Stamped<tf::Pose> goalPose = Geometry::make_pose(0.600358,-0.125386,0.736068,-0.001765,-0.002289,-0.004847,0.999984,"base_link");
04768 
04769             // calc how much we have to move to bring the bowl into the desired position relative to the robot
04770             btVector3 rel = inBase.getOrigin() - goalPose.getOrigin();
04771             ROS_ERROR("rel %f %f", rel.x(), rel.y());
04772 
04773             tf::Stamped<tf::Pose> nav_goal;
04774             nav_goal.frame_id_ = "/base_link";
04775             nav_goal.setRotation(btQuaternion(0,0,0,1));
04776             nav_goal.setOrigin(btVector3(rel.x(), rel.y(), 0));
04777             nav_goal = Geometry::getPoseIn("map", nav_goal);
04778 
04779             //RobotDriver::getInstance()->driveInMap(nav_goal, false); //
04780 
04781             printPose("BowlPose", bowlPose);
04782 
04783             pourReadyPopcorn_(nav_goal.getOrigin().x(), nav_goal.getOrigin().y(), nav_goal.getRotation().z(), nav_goal.getRotation().w(), 0);
04784 
04785         }
04786 
04787         if (0)
04788         {
04789             tf::Stamped<tf::Pose> bowlPose = getBigBowlPose(search);
04790             bowlPose = Geometry::getPoseIn("/base_link", bowlPose);
04791             printPose("bowlpose_in_base_link", bowlPose);
04792             exit(0);
04793         }
04794 
04795         //graspBigBowl(bowlPose, true);
04796 
04797         //OperateHandleController::plateCarryPose();
04798 
04799         //graspBigBowl(bowlPose, false);
04800     }
04801 
04802     /*if (atoi(argv[1]) == -637)
04803     {
04804         // try rotating pot to distribute popcorn more evenly
04805 
04806         RobotArm *rarm = RobotArm::getInstance(0);
04807         RobotArm *larm = RobotArm::getInstance(1);
04808         tf::Stamped<tf::Pose> r_tp = rarm->getToolPose("/base_link");
04809         tf::Stamped<tf::Pose> l_tp = larm->getToolPose("/base_link");
04810         tf::Stamped<tf::Pose> mid,mid_tilted;
04811         mid.frame_id_ = "/base_link";
04812         mid.setOrigin(0.5 * (r_tp.getOrigin() + l_tp.getOrigin()));
04813         mid.setRotation(btQuaternion(0,0,0,1));
04814 
04815         btTransform midbt;
04816         midbt.setOrigin(mid.getOrigin());
04817         midbt.setRotation(mid.getRotation());
04818 
04819         double angle = 0;
04820 
04821         ros::Rate r(2);
04822         while (ros::ok())
04823         {
04824             mid_tilted =  RobotArm::rotateAroundPose(mid,mid,cos(angle) * 0.25,sin(angle) * 0.25,0);
04825             btTransform mid_tiltedbt;
04826             mid_tiltedbt.setOrigin(mid_tilted.getOrigin());
04827             mid_tiltedbt.setRotation(mid_tilted.getRotation());
04828 
04829             announce("mid", mid);
04830 
04831             announce("mid", mid_tilted);
04832 
04833             r.sleep();
04834 
04835             btTransform r,l;
04836 
04837             r = r_tp;
04838             l = l_tp;
04839 
04840             l = mid_tiltedbt * mid.inverseTimes(l);
04841             r = mid_tiltedbt * mid.inverseTimes(r);
04842 
04843             tf::Stamped<tf::Pose> act_l_tp = l_tp;
04844             act_l_tp.setOrigin(l.getOrigin());
04845             act_l_tp.setRotation(l.getRotation());
04846             tf::Stamped<tf::Pose> act_r_tp = r_tp;
04847             act_r_tp.setOrigin(r.getOrigin());
04848             act_r_tp.setRotation(r.getRotation());
04849 
04850             announce("r",act_r_tp);
04851             announce("l",act_l_tp);
04852 
04853             RobotArm::moveBothArms(act_l_tp, act_r_tp, 0, false);
04854 
04855 
04856             ros::Duration(0.5).sleep();
04857 
04858 
04859             angle += M_PI / 5.0;
04860         }
04861     }*/
04862 
04863     if (atoi(argv[1]) == -702)
04864     {
04865 
04866         int jump = 0;
04867 
04868         switch (atoi(argv[2]))
04869         {
04870 
04871         case 0 : //0:15
04872             Current::getPotOut_openDrawer(jump);
04873 
04874         case 1 : //0:54 pot on // 1:30
04875             Current::getPotOut_pickPlacePot(jump);
04876 
04877         case 2 :
04878             if ((argc > 3) && (atoi(argv[3])==1))
04879                 RobotDriver::moveBase4(-0.346670, 2.307890, 0.757347, 0.653002);
04880 
04881         case 3 :
04882             Current::manipulateKnob(1); // 1:54
04883             //Current::manipulateKnob(-1);
04884 
04885         case 4 :
04886             Current::openDrawerUnderOven(jump); // 2:23
04887 
04888         case 5 :
04889             Current::getLidOut(jump); // 2:45
04890 
04891         case 6 :
04892             Current::getBowlOut(jump); // 3:15
04893 
04894         case 7 :
04895             Current::closeDrawerUnderOven(jump); // 3:30
04896 
04897         case 8 :
04898             if ((argc > 3) && (atoi(argv[3])==1))
04899                 RobotDriver::moveBase4(-0.346670, 2.307890, 0.757347, 0.653002);
04900         case 9 :
04901             Current::pourPopCorn(jump); //3:54
04902 
04903         case 10 :
04904             Current::putLidOn(jump); //4:10
04905             //RobotDriver::moveBase4(-0.904410, 2.493494, 0.998334, -0.057692); // for waiting
04906         case 11:
04907 
04908             Current::takePotFromIsland(jump); // 6:06
04909             shake_it();
04910             ROS_ERROR(" SHAKE DONE ");
04911             //putBigBowlOntoIsland_heater();
04912 
04913         case 12:
04914             getBowlFromDrawer(0);
04915 
04916         case 13:
04917             putBigBowlOntoIsland();
04918 
04919         case 14:
04920             closeDrawer(0);
04921 
04922         case 15 :
04923             Current::pushPot(jump); // 4:31
04924 
04925         case 16 :
04926             if ((argc > 3) && (atoi(argv[3])==1))
04927                 RobotDriver::moveBase4(-0.346670, 2.307890, 0.757347, 0.653002);
04928             Current::manipulateKnob(-1); // 5:13
04929 
04930         case 17 :
04931             Current::removeLid(jump); //5:51
04932 
04933         case 18 :
04934             Current::takePotFromIsland(jump); // 6:06
04935 
04936         case 19 :
04937             Current::pourReadyPopcorn(jump); // 6:27 pour 6:50 pot down
04938 
04939         case 20 :
04940             Current::salt(jump); // 6:27 pour 6:50 pot down
04941 
04942         }
04943     }
04944 
04945     /*if (atoi(argv[1]) == -638)
04946     {
04947         getBowlFromDrawer(0);
04948         btVector3 search(.27, -1.33, .75);
04949         putBigBowlOntoTable(search, 0.129038, -0.702062, -0.655191, 0.755459);
04950         closeDrawer(0);
04951     }
04952     if (atoi(argv[1]) == -639)
04953     {
04954         //btVector3 search(.45, -1.35, .88 - .13);
04955         btVector3 search(.27, -1.33, .75);
04956         putBigBowlOntoTable(search, 0.129038, -0.702062, -0.655191, 0.755459);
04957     }*/
04958 
04959     if (atoi(argv[1]) == -640)
04960     {
04961 
04962         //btVector3 search(.637, -1.486, .742 + .022); // should be 2.2 cm above table
04963         //RobotDriver::moveBase4(0.168233, -0.847962, -0.512474, 0.85870);
04964         //salt_it(search, 0.168233, -0.847962, -0.512474, 0.85870);
04965         //salt_it(search, 0.073525, -0.679463, -0.682670, 0.730720);
04966         // .18 -1.29
04967         btVector3 search(.43, -1.46, .742 + 0.022);
04968         salt_it(search, 0.129038 + 0.09, -0.702062 - 0.04, -0.655191, 0.75545);
04969     }
04970 
04971     if (atoi(argv[1]) == -703)
04972     {
04973 
04974         int jump = 0;
04975 
04976         switch (atoi(argv[2]))
04977         {
04978 
04979         case 0 : //0:15
04980             Current::getPotOut_openDrawer(jump);
04981 
04982         case 1 : //0:54 pot on // 1:30
04983             Current::getPotOut_pickPlacePot(jump);
04984 
04985         case 2 :
04986             if ((argc > 3) && (atoi(argv[3])==1))
04987                 RobotDriver::moveBase4(-0.346670, 2.307890, 0.757347, 0.653002);
04988 
04989         case 3 :
04990             Current::manipulateKnob(1); // 1:54
04991             //Current::manipulateKnob(-1);
04992 
04993         case 4 :
04994             Current::openDrawerUnderOven(jump); // 2:23
04995 
04996         case 5 :
04997             Current::getLidOut(jump); // 2:45
04998 
04999         case 6 :
05000             Current::getBowlOut(jump); // 3:15
05001 
05002         case 7 :
05003             Current::closeDrawerUnderOven(jump); // 3:30
05004 
05005         case 8 :
05006             if ((argc > 3) && (atoi(argv[3])==1))
05007                 RobotDriver::moveBase4(-0.346670, 2.307890, 0.757347, 0.653002);
05008         case 9 :
05009             Current::pourPopCorn(jump); //3:54
05010 
05011         case 10 :
05012             Current::putLidOn(jump); //4:10
05013             //RobotDriver::moveBase4(-0.904410, 2.493494, 0.998334, -0.057692); // for waiting
05014         case 11:
05015 
05016             Current::takePotFromIsland(jump); // 6:06
05017 
05018             shake_it();
05019 
05020             ROS_ERROR(" SHAKE DONE ");
05021             //putBigBowlOntoIsland_heater();
05022 
05023         case 12:
05024             openFavouriteDrawer(0);
05025         case 13:
05026             getBowlFromDrawer(0);
05027 
05028         case 14:
05029             //putBigBowlOntoIsland();
05030 
05031             putBigBowlOntoTable(btVector3(.27, -1.33, .75), 0.129038, -0.702062, -0.655191, 0.755459);
05032 
05033         case 15:
05034 
05035             closeDrawer(0);
05036 
05037         case 16 :
05038             Current::pushPot(jump); // 4:31
05039 
05040         case 17 :
05041             if ((argc > 3) && (atoi(argv[3])==1))
05042                 RobotDriver::moveBase4(-0.346670, 2.307890, 0.757347, 0.653002);
05043             Current::manipulateKnob(-1); // 5:13
05044 
05045         case 18 :
05046             Current::removeLid(jump); //5:51
05047 
05048         case 19 :
05049             Current::takePotFromIsland(jump); // 6:06
05050 
05051         case 20 :
05052 
05053             pourReadyPopcornTable(jump); // 6:27 pour 6:50 pot down
05054             //pourReadyPopcorn(0);
05055             //RobotHead::getInstance()->lookAtThreaded("/map", -1.7, 1.5, 0.899);
05056             //fotopourReadyPopcorn_(-1, 1.6,  0.997743, -0.067087, 0,2.4,false);
05057 
05058         case 21 :
05059 
05060             //returnPotToHeater();
05061             returnPotToSink();
05062 
05063         case 22 :
05064             //Current::salt(jump); // 6:27 pour 6:50 pot down
05065 
05066             salt_it(btVector3(.43, -1.46, .742 + 0.022 + 0.006), 0.129038 + 0.09, -0.702062 - 0.04, -0.655191, 0.75545);
05067             exit(0);
05068         case 23:
05069             tf::Stamped<tf::Pose> roffer;
05070             roffer.frame_id_ = "/map";
05071             roffer.setOrigin(btVector3(0.128651, -1.243839, 0.835045));
05072             roffer.setRotation(btQuaternion(0.976158, -0.112988, -0.179133, -0.047544));
05073 
05074             tf::Stamped<tf::Pose> loffer;
05075             loffer.frame_id_ = "/map";
05076             loffer.setOrigin(btVector3(0.423095, -1.223293, 0.8270));
05077             loffer.setRotation(btQuaternion(-0.229902, 0.959984, 0.038952, 0.155109));
05078 
05079             RobotHead::getInstance()->lookAtThreaded("/map",.29, -1.38, 0.899);
05080 
05081             //RobotArm::moveBothArms(loffer,roffer);
05082 
05083             boost::thread t0(&OperateHandleController::plateAttackPose);
05084 
05085             RobotDriver::moveBase4(-0.135284, -0.136694, -0.676531, 0.736413);
05086 
05087             t0.join();
05088 
05089             //RobotHead::getInstance()->lookAt("/map", 2.1, -5.1, 1.6, true);
05090             //RobotHead::getInstance()->lookAt("/map", -2.8, -6.3, 1.6, true);
05091 
05092             boost::thread t1(&OperateHandleController::plateTuckPose);
05093 
05094             RobotHead::getInstance()->lookAtThreaded("/map",.29, -1.38, 0.899);
05095 
05096 
05097         }
05098     }
05099 
05100     if (atoi(argv[1]) == -641)
05101     {
05102         double xshift = atof(argv[2]);
05103         double yshift = atof(argv[3]);
05104         //pourReadyPopcorn_(0.279824 + 0.025, -0.691109 -.03, -0.659599, 0.751613, 0.12);
05105         RobotHead::getInstance()->lookAtThreaded("/map",.27, -1.33, 0.899);
05106         //pourReadyPopcorn_(0.279824 + xshift, -0.691109 + yshift, -0.659599, 0.751613, 0.12);
05107         tf::Stamped<tf::Pose> bowlPose = getBigBowlPose(btVector3(.27,-1.35, .75));
05108 
05109         btVector3 diff = bowlPose.getOrigin() - btVector3(.271, -1.33, 1.00);
05110         ROS_ERROR("BOWL DIFF %f %f", diff.x(), diff.y());
05111         //for bowl .271 -1.33 1.00
05112         pourReadyPopcorn_(.3098 + diff.x() + xshift, -0.716 + diff.y() + yshift, -0.659599, 0.751613, 0.12);
05113     }
05114 
05115     if (atoi(argv[1]) == -642)
05116     {
05117         ROS_INFO("RECORD TRAJECTORY WHILE GRIPPER IS CLOSED.");
05118         ROS_INFO("bin/ias_drawer_executive -642 amountOpenThreshold rate side");
05119         double gripperOpeningThreshold = atof(argv[2]);
05120         double rate = atof(argv[3]);
05121         ros::Rate rt(rate);
05122 
05123         while (ros::ok())
05124         {
05125             if (Gripper::getInstance(atoi(argv[4]))->getAmountOpen() < gripperOpeningThreshold)
05126                 printPoseSimple(RobotArm::getInstance(atoi(argv[4]))->getToolPose("/map"));
05127             rt.sleep();
05128         }
05129     }
05130 
05131     if (atoi(argv[1]) == -643)
05132     {
05133         RobotDriver::moveBase4(-1.281009, -3.185903, 0.830314, 0.557294);
05134 
05135 
05136         RobotHead::getInstance()->lookAtThreaded("r_gripper_tool_frame",0,0,0);
05137 
05138         double pts[][7] ={{-1.351540,-2.296082,0.975278,0.659204,0.751421,-0.023565,-0.016137},
05139             {-1.351977,-2.296833,0.975388,0.658666,0.751905,-0.022882,-0.016551},
05140             {-1.351919,-2.297108,0.975577,0.660577,0.750314,-0.020253,-0.016063},
05141             {-1.352188,-2.296892,0.975475,0.664234,0.747228,-0.015198,-0.014589},
05142             {-1.351736,-2.301302,0.975433,0.659887,0.751031,-0.014568,-0.017011},
05143             {-1.343917,-2.335844,0.976201,0.619519,0.784706,-0.018606,-0.009272},
05144             {-1.329244,-2.372110,0.976194,0.574161,0.818253,-0.025994,-0.011172},
05145             {-1.312993,-2.402995,0.975653,0.524666,0.850551,-0.033886,-0.011848},
05146             {-1.289472,-2.436672,0.975557,0.476669,0.878117,-0.039786,-0.010718},
05147             {-1.248192,-2.478786,0.975374,0.407516,0.912462,-0.036654,-0.000978},
05148             {-1.196179,-2.513152,0.975324,0.316567,0.948113,-0.029311,0.002950},
05149             {-1.134346,-2.533039,0.975364,0.167569,0.985736,-0.012399,0.009524},
05150             {-1.083221,-2.539201,0.976012,0.087312,0.996137,-0.007728,-0.005284},
05151             {-1.045371,-2.537415,0.976197,0.028048,0.999574,-0.004110,-0.006976},
05152             {-1.010388,-2.531838,0.976724,-0.027442,0.999588,0.002041,-0.008125},
05153             {-0.990392,-2.526423,0.976715,-0.056290,0.998398,0.001162,-0.005625},
05154             {-0.989306,-2.526726,0.976869,-0.059351,0.998221,0.000320,-0.005661},
05155             {-0.988068,-2.526833,0.981427,-0.063703,0.997895,0.002038,-0.012009},
05156             {-0.987489,-2.526563,0.985804,-0.073449,0.997171,-0.003679,-0.015570},
05157             {-0.984144,-2.524953,0.984858,-0.071556,0.997236,-0.010002,-0.017344},
05158             {-0.986933,-2.526707,0.988158,-0.075933,0.997079,-0.004444,-0.006946},
05159             {-0.982650,-2.524833,0.988676,-0.077677,0.996949,-0.001231,-0.007560},
05160             {-0.981975,-2.524955,0.990235,-0.077920,0.996923,-0.004331,-0.007301},
05161             {-0.980884,-2.521257,0.991953,-0.079351,0.996689,-0.016710,-0.005972},
05162             {-0.979558,-2.521293,0.992222,-0.079630,0.996626,-0.019001,-0.005824}
05163         };
05164 
05165         std::vector<tf::Stamped<tf::Pose> > poses;
05166 
05167         poses.resize(24);
05168 
05169         for (size_t k = 0; k < poses.size(); k ++)
05170         {
05171             tf::Stamped<tf::Pose> act;
05172             act.frame_id_ = "map";
05173             act.setOrigin(btVector3(pts[k][0],pts[k][1],pts[k][2]));
05174             act.setRotation(btQuaternion(pts[k][3],pts[k][4],pts[k][5],pts[k][6]));
05175             poses[k] = act;
05176         }
05177 
05178         //we have our pose vector now:
05179 
05180         //check for a base pose
05181         {
05182             tf::Stamped<tf::Pose> result;
05183             std::vector<int> arm;
05184             std::vector<tf::Stamped<tf::Pose> > goal;
05185 
05186             for (size_t k = 0; k < poses.size(); k ++)
05187             {
05188                 arm.push_back(0);
05189                 goal.push_back(poses[k]);
05190             }
05191 
05192             RobotArm::findBaseMovement(result, arm, goal, true, false);
05193         }
05194 
05195 
05196 
05197         Gripper::getInstance(0)->open();
05198 
05199         for (int k = 10; k > 0 ; k --)
05200         {
05201             ROS_INFO("K %i", k);
05202             RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(poses[k]);
05203         }
05204 
05205         Gripper::getInstance(0)->close();
05206 
05207         for (size_t k = 0; k < poses.size() ; k++)
05208         {
05209             ROS_INFO("K %zu", k);
05210             RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(poses[k]);
05211         }
05212 
05213         Gripper::getInstance(0)->open();
05214 
05215         for (size_t k = poses.size() -1 ; k > 0 ; k --)
05216         {
05217             ROS_INFO("K %zu", k);
05218             RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(poses[k]);
05219         }
05220 
05221     }
05222 
05223 
05224     if (atoi(argv[1]) == -644)
05225     {
05226         ROS_INFO("RECORD TRAJECTORY WHILE GRIPPER IS CLOSED.");
05227         ROS_INFO("bin/ias_drawer_executive -642 amountOpenThreshold rate side");
05228         double gripperOpeningThreshold = atof(argv[2]);
05229         double rate = atof(argv[3]);
05230         ros::Rate rt(rate);
05231 
05232 
05233         ros::Time start = ros::Time::now();
05234         bool hit = false;
05235 
05236         tf::Stamped<tf::Pose> act = RobotArm::getInstance(atoi(argv[4]))->getToolPose("/map");
05237 
05238         while (ros::ok())
05239         {
05240             if (Gripper::getInstance(atoi(argv[4]))->getAmountOpen() < gripperOpeningThreshold)
05241             {
05242                 if (!hit)
05243                 {
05244                     hit = true;
05245                     start = ros::Time::now();
05246                     printf("std::vector<tf::Stamped<tf::Pose> > poses;\n");
05247                     printf("std::vector<double > timestamps;\n");
05248                     printf("tf::Stamped<tf::Pose> curr; curr.frame_id_ = \"/map\";  \n");
05249                 }
05250                 tf::Stamped<tf::Pose> act = RobotArm::getInstance(atoi(argv[4]))->getToolPose("/map");
05251                 printf("curr.setOrigin(btVector3(%f,%f,%f));", act.getOrigin().x(), act.getOrigin().y(), act.getOrigin().z());
05252                 printf("curr.setRotation(btQuaternion(%f,%f,%f,%f));", act.getRotation().x(), act.getRotation().y(), act.getRotation().z(),act.getRotation().w());
05253                 printf("poses.push_back(curr);");
05254                 ros::Duration actTime = ros::Time::now() - start;
05255                 printf("timestamps.push_back(%f);\n",actTime.toSec());
05256                 //printPoseSimple(RobotArm::getInstance(atoi(argv[4]))->getToolPose("/map"));
05257             }
05258             rt.sleep();
05259         }
05260     }
05261 
05262     if (atoi(argv[1]) == -645)
05263     {
05264 
05265         std::vector<tf::Stamped<tf::Pose> > poses;
05266         std::vector<double > timestamps;
05267         tf::Stamped<tf::Pose> curr;
05268         curr.frame_id_ = "/map";
05269         curr.setOrigin(btVector3(-0.858909,2.662146,1.069023));
05270         curr.setRotation(btQuaternion(-0.063184,0.705228,0.700371,-0.090230));
05271         poses.push_back(curr);
05272         timestamps.push_back(0.010172);
05273         curr.setOrigin(btVector3(-0.858132,2.662023,1.068230));
05274         curr.setRotation(btQuaternion(-0.061947,0.706447,0.699131,-0.091168));
05275         poses.push_back(curr);
05276         timestamps.push_back(0.510091);
05277         curr.setOrigin(btVector3(-0.856363,2.663267,1.066434));
05278         curr.setRotation(btQuaternion(-0.059214,0.709004,0.696730,-0.091511));
05279         poses.push_back(curr);
05280         timestamps.push_back(1.010011);
05281         curr.setOrigin(btVector3(-0.830527,2.676649,1.066342));
05282         curr.setRotation(btQuaternion(-0.043343,0.708105,0.701059,-0.072286));
05283         poses.push_back(curr);
05284         timestamps.push_back(1.510092);
05285         curr.setOrigin(btVector3(-0.727655,2.686956,1.064891));
05286         curr.setRotation(btQuaternion(-0.053571,0.706820,0.704510,-0.034657));
05287         poses.push_back(curr);
05288         timestamps.push_back(2.010104);
05289         curr.setOrigin(btVector3(-0.607844,2.665836,1.075091));
05290         curr.setRotation(btQuaternion(-0.053020,0.709189,0.699932,-0.065840));
05291         poses.push_back(curr);
05292         timestamps.push_back(2.510075);
05293         curr.setOrigin(btVector3(-0.568007,2.651925,1.071384));
05294         curr.setRotation(btQuaternion(-0.054405,0.709798,0.699263,-0.065253));
05295         poses.push_back(curr);
05296         timestamps.push_back(3.010017);
05297         curr.setOrigin(btVector3(-0.568066,2.650177,1.071403));
05298         curr.setRotation(btQuaternion(-0.091834,0.724687,0.679900,-0.064276));
05299         poses.push_back(curr);
05300         timestamps.push_back(3.510078);
05301         curr.setOrigin(btVector3(-0.562386,2.652335,1.072278));
05302         curr.setRotation(btQuaternion(-0.096352,0.716489,0.687963,-0.063779));
05303         poses.push_back(curr);
05304         timestamps.push_back(4.010068);
05305         curr.setOrigin(btVector3(-0.550039,2.660620,1.069650));
05306         curr.setRotation(btQuaternion(-0.087717,0.715498,0.691267,-0.050182));
05307         poses.push_back(curr);
05308         timestamps.push_back(4.510390);
05309 
05310         std::vector<int> arms;
05311         for (int k =0; k < poses.size(); k++)
05312             arms.push_back(0);
05313 
05314         tf::Stamped<tf::Pose> result;
05315         RobotArm::findBaseMovement(result, arms, poses, true, false);
05316 
05317         RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(poses[0]);
05318         RobotArm::getInstance(0)->startTrajectory(RobotArm::getInstance(0)->multiPointTrajectory(poses,timestamps));
05319 
05320     }
05321 
05322 
05323     if (atoi(argv[1]) == -677)
05324     {
05325         fotopourReadyPopcorn_(atof(argv[2]), 1.6,  0.997743, -0.067087, atof(argv[3]), atof(argv[4]),atof(argv[5]) > 0);
05326     }
05327 
05328     if (atoi(argv[1]) == -677)
05329     {
05330         salt_it(btVector3(.43, -1.46, .742 + 0.022 + 0.006), 0.129038 + 0.09, -0.702062 - 0.04, -0.655191, 0.75545);
05331     }
05332 
05333 
05334 
05335     if (atoi(argv[1]) == -704)
05336     {
05337 
05338         int jump = 0;
05339 
05340         switch (atoi(argv[2]))
05341         {
05342 
05343         case 0 : //0:15
05344             Current::getPotOut_openDrawer(jump);
05345 
05346         case 1 : //0:54 pot on // 1:30
05347             Current::getPotOut_pickPlacePot(jump);
05348 
05349         case 2 :
05350             if ((argc > 3) && (atoi(argv[3])==1))
05351                 RobotDriver::moveBase4(-0.346670, 2.307890, 0.757347, 0.653002);
05352 
05353         case 3 :
05354             Current::manipulateKnob(1); // 1:54
05355             //Current::manipulateKnob(-1);
05356 
05357         case 4 :
05358             Current::openDrawerUnderOven(jump); // 2:23
05359 
05360         case 5 :
05361             Current::getLidOut(jump); // 2:45
05362 
05363         case 6 :
05364             Current::getBowlOut(jump); // 3:15
05365 
05366         case 7 :
05367             Current::closeDrawerUnderOven(jump); // 3:30
05368 
05369         case 8 :
05370             if ((argc > 3) && (atoi(argv[3])==1))
05371                 RobotDriver::moveBase4(-0.346670, 2.307890, 0.757347, 0.653002);
05372         case 9 :
05373             Current::pourPopCorn(jump); //3:54
05374 
05375         case 10 :
05376             Current::putLidOn(jump); //4:10
05377             //RobotDriver::moveBase4(-0.904410, 2.493494, 0.998334, -0.057692); // for waiting
05378         case 11:
05379 
05380             Current::takePotFromIsland(jump); // 6:06
05381 
05382             shake_it();
05383 
05384             ROS_ERROR(" SHAKE DONE ");
05385             //putBigBowlOntoIsland_heater();
05386 
05387         case 12:
05388             openFavouriteDrawer(0);
05389         case 13:
05390             getBowlFromDrawer(0);
05391 
05392         case 14:
05393             putBigBowlOntoIsland();
05394 
05395             //putBigBowlOntoTable(btVector3(.27, -1.33, .75), 0.129038, -0.702062, -0.655191, 0.755459);
05396 
05397         case 15:
05398 
05399             closeDrawer(0);
05400 
05401         case 16 :
05402             Current::pushPot(jump); // 4:31
05403 
05404         case 17 :
05405             if ((argc > 3) && (atoi(argv[3])==1))
05406                 RobotDriver::moveBase4(-0.346670, 2.307890, 0.757347, 0.653002);
05407             Current::manipulateKnob(-1); // 5:13
05408 
05409         case 18 :
05410             Current::removeLid(jump); //5:51
05411 
05412         case 19 :
05413             Current::takePotFromIsland(jump); // 6:06
05414 
05415         case 20 :
05416             //pourReadyPopcornTable(jump); // 6:27 pour 6:50 pot down
05417             Current::pourReadyPopcorn(0);
05418             //
05419             //RobotHead::getInstance()->lookAtThreaded("/map", -1.7, 1.5, 0.899);
05420 
05421             //fotopourReadyPopcorn_(-1, 1.6,  0.997743, -0.067087, 0,2.4,false);
05422 
05423         case 21 :
05424 
05425             returnPotToHeater();
05426             //returnPotToSink();
05427 
05428         case 22 :
05429             Current::salt(jump); // 6:27 pour 6:50 pot down
05430 
05431             //salt_it(btVector3(.43, -1.46, .742 + 0.022 + 0.006), 0.129038 + 0.09, -0.702062 - 0.04, -0.655191, 0.75545);
05432             exit(0);
05433 
05434         }
05435     }
05436 
05437 
05438 
05439 
05440 
05441     return 0;
05442 }
05443 
05444 
05445 // deviation between odom_combined and map relative to base_link
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


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