ias_drawer_executive.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, Thomas Ruehr <ruehr@cs.tum.edu>
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 // roslaunch arm_ik.launch
00031 #include <ros/ros.h>
00032 
00033 #include <iostream>
00034 #include <fstream>
00035 using namespace std;
00036 
00037 #include <ias_drawer_executive/Approach.h>
00038 #include <ias_drawer_executive/RobotDriver.h>
00039 #include <ias_drawer_executive/Gripper.h>
00040 #include <ias_drawer_executive/Geometry.h>
00041 #include <ias_drawer_executive/Perception3d.h>
00042 #include <ias_drawer_executive/Pressure.h>
00043 #include <ias_drawer_executive/Poses.h>
00044 #include <ias_drawer_executive/RobotArm.h>
00045 #include <ias_drawer_executive/AverageTF.h>
00046 #include <ias_drawer_executive/Torso.h>
00047 #include <ias_drawer_executive/Head.h>
00048 #include <ias_drawer_executive/OperateHandleController.h>
00049 #include <ias_drawer_executive/yamlWriter.h>
00050 
00051 #include <boost/thread.hpp>
00052 
00053 #include <actionlib/client/simple_client_goal_state.h>
00054 #include <visualization_msgs/Marker.h>
00055 
00056 #include <ias_drawer_executive/DemoScripts.h>
00057 
00058 #include <articulation_pr2/ArticulatedObjectAction.h>
00059 #include <actionlib/client/simple_action_client.h>
00060 
00061 #include <pcl/ros/conversions.h>
00062 
00063 #include <pcl_ros/point_cloud.h>
00064 #include <pcl_ros/transforms.h>
00065 
00066 #include <pcl/kdtree/kdtree_flann.h>
00067 
00068 #include "rosbag/bag.h"
00069 #include "rosbag/query.h"
00070 #include "rosbag/view.h"
00071 #include <boost/foreach.hpp>
00072 #include <limits>
00073 
00074 
00075 //void printPose(const char title[], tf::Stamped<tf::Pose> pose)
00076 //{
00077     //ROS_INFO("%s %f %f %f %f %f %f %f %s", title, pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z()
00078              //, pose.getRotation().x(), pose.getRotation().y(), pose.getRotation().z(), pose.getRotation().w(), pose.frame_id_.c_str());
00079 //}
00080 
00081 
00082 void printPose(const char title[], tf::Stamped<tf::Pose> pose)
00083 {
00084     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()
00085              , pose.getRotation().x(), pose.getRotation().y(), pose.getRotation().z(), pose.getRotation().w(), pose.frame_id_.c_str(), title);
00086 }
00087 
00088 void printPose(const char title[], tf::Pose pose)
00089 {
00090     ROS_INFO("%s %f %f %f %f %f %f %f", title, pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z()
00091              , pose.getRotation().x(), pose.getRotation().y(), pose.getRotation().z(), pose.getRotation().w());
00092 }
00093 
00094 pcl::PointCloud<pcl::PointXYZRGB> substractPC(pcl::PointCloud<pcl::PointXYZRGB>  clouda,  pcl::PointCloud<pcl::PointXYZRGB>  cloudb, double distance_threshold = 0.01)
00095 {
00096     pcl::KdTree<pcl::PointXYZRGB>::Ptr tree;
00097     tree.reset (new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
00098 
00099     pcl::PointCloud<pcl::PointXYZRGB> inp;
00100     for (size_t k = 0; k < cloudb.points.size(); ++k )
00101     {
00102         if (!isnan(cloudb.points[k].x))
00103         {
00104             inp.points.push_back(cloudb.points[k]);
00105         }
00106     }
00107 
00108 //    ROS_INFO("                                                CLEANED CLOUD SIZE : %i", inp.points.size());
00109 
00110     tree->setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZRGB> >(inp));
00111 
00112     // Allocate enough space to hold the results
00113     // \note resize is irrelevant for a radiusSearch ().
00114     std::vector<int> nn_indices (1);
00115     std::vector<float> nn_sqr_dists (1);
00116 
00117     pcl::PointCloud<pcl::PointXYZRGB> result;
00118     for (size_t k = 0; k < clouda.points.size(); ++k )
00119     {
00120         if (!isnan(clouda.points[k].x))
00121         {
00122             if (tree->radiusSearch (clouda.points[k],fabs(distance_threshold), nn_indices,nn_sqr_dists, 1) != 0)
00123             {
00124                 // there are neighbors! see nn_indices, nn_sqr_dists
00125 
00126                 //cloudinlier.points.push_back(cloudb.points[k]);
00127                 //ROS_INFO("k %i O",k);
00128                 if (distance_threshold < 0)
00129                     result.push_back(clouda.points[k]);
00130             }
00131             else
00132             {
00133                 if (distance_threshold > 0)
00134                     result.push_back(clouda.points[k]);
00135                 //ROS_INFO("k %i I",k);
00136                 //cloudoutlier.points.push_back(cloudb.points[k]);
00137             }
00138         }
00139     }
00140     return result;
00141 }
00142 
00143 pcl::KdTree<pcl::PointXYZRGB>::Ptr getTree(pcl::PointCloud<pcl::PointXYZRGB> &cloudb)
00144 {
00145     pcl::KdTree<pcl::PointXYZRGB>::Ptr tree;
00146     tree.reset (new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
00147 
00148     tree->setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZRGB> >(cloudb));
00149     return tree;
00150 }
00151 
00152 pcl::PointCloud<pcl::PointXYZRGB> substractPC(pcl::PointCloud<pcl::PointXYZRGB>  clouda,  pcl::KdTree<pcl::PointXYZRGB>::Ptr &tree, double distance_threshold = 0.01)
00153 {
00154     // Allocate enough space to hold the results
00155     // \note resize is irrelevant for a radiusSearch ().
00156     std::vector<int> nn_indices (1);
00157     std::vector<float> nn_sqr_dists (1);
00158 
00159     pcl::PointCloud<pcl::PointXYZRGB> result;
00160     for (size_t k = 0; k < clouda.points.size(); ++k )
00161     {
00162         if (!isnan(clouda.points[k].x))
00163         {
00164             bool found = false;
00165             if ((tree->nearestKSearch (clouda.points[k], 1, nn_indices, nn_sqr_dists) != 0) && (nn_sqr_dists[0] < distance_threshold * distance_threshold))
00166             {
00167                 if (distance_threshold < 0)
00168                     result.push_back(clouda.points[k]);
00169             }
00170             else
00171             {
00172                 if (distance_threshold > 0)
00173                     result.push_back(clouda.points[k]);
00174             }
00175 
00176             /*if (tree->radiusSearch (clouda.points[k],fabs(distance_threshold), nn_indices,nn_sqr_dists, 1) != 0)
00177             {
00178                 ROS_INFO("sqr dist %f", nn_sqr_dists[0]);
00179                 // there are neighbors! see nn_indices, nn_sqr_dists
00180                 if (distance_threshold < 0)
00181                     result.push_back(clouda.points[k]);
00182             }
00183             else
00184             {
00185                 if (distance_threshold > 0)
00186                     result.push_back(clouda.points[k]);
00187             }*/
00188         }
00189     }
00190     return result;
00191 }
00192 
00193 double pointDist(pcl::PointXYZRGB &p,  pcl::KdTree<pcl::PointXYZRGB>::Ptr &tree)
00194 {
00195     std::vector<int> nn_indices (1);
00196     std::vector<float> nn_sqr_dists (1);
00197     if (tree->nearestKSearch (p, 1, nn_indices, nn_sqr_dists) < 1)
00198         return 1000;
00199     else
00200         return nn_sqr_dists[0];
00201 }
00202 
00203 
00204 
00205 /*void mangle()
00206 {
00207     ros::NodeHandle node_handle;
00208     ros::Publisher pcm_pub_in = node_handle.advertise<sensor_msgs::PointCloud2>( "/pc_inlier", 10 , true);
00209     ros::Publisher pcm_pub_out = node_handle.advertise<sensor_msgs::PointCloud2>( "/pc_outlier", 10 , true);
00210 
00211     bool done = false;
00212     int numclouds = 0;
00213 
00214     std::vector<pcl::PointCloud<pcl::PointXYZRGB> > pcs;
00215     std::vector<pcl::PointCloud<pcl::PointXYZRGB> > foreground;
00216     std::vector<pcl::PointCloud<pcl::PointXYZRGB> > filtered_foreground;
00217 
00218     std::vector<tf::Stamped<tf::Pose> > frames;
00219 
00220     ROS_INFO("READY");
00221 
00222     boost::shared_ptr<const sensor_msgs::PointCloud2> act;
00223 
00224     ros::Time lastTime = ros::Time::now();
00225 
00226     while (numclouds < 15)
00227     {
00228         act = ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/pc");
00229         if (act->header.stamp != lastTime)
00230         {
00231 
00232             lastTime = act->header.stamp;
00233 
00234             ROS_INFO("CURRENT CLOUD STAMP : %f", act->header.stamp.toSec());
00235 
00236             tf::Stamped<tf::Pose> net_stamped = RobotArm::getPose("/r_gripper_tool_frame",act->header.frame_id.c_str());
00237 
00238             pcl::PointCloud<pcl::PointXYZRGB> actPC = pcl::PointCloud<pcl::PointXYZRGB>();
00239             pcl::fromROSMsg(*act,actPC);
00240 
00241             pcl::PointCloud<pcl::PointXYZRGB> *actPCclean = new  pcl::PointCloud<pcl::PointXYZRGB>();
00242             for (int k = 0; k < actPC.points.size(); ++k)
00243             {
00244                 if (!isnan(actPC.points[k].x))
00245                     actPCclean->push_back(actPC.points[k]);
00246             }
00247 
00248             pcs.push_back(*actPCclean);
00249             frames.push_back(net_stamped);
00250             numclouds ++;
00251             ROS_INFO("NUM CLOUDS : %i", numclouds);
00252         }
00253     }
00254 
00255     std::vector<pcl::KdTree<pcl::PointXYZRGB>::Ptr > trees_map;
00256     trees_map.resize(pcs.size());
00257     for (int k = 0; k < pcs.size(); ++k)
00258     {
00259         trees_map[k] = getTree(pcs[k]);
00260     }
00261 
00262     for (int k = 1; k < pcs.size() - 1; ++k)
00263     {
00264         pcl::PointCloud<pcl::PointXYZRGB> act_foreground = pcs[k];
00265         act_foreground.is_dense = false;
00266         for (int j = 0; j < pcs.size(); ++j)
00267         {
00268             if (fabs((double)j - k) > 1)
00269             {
00270                 pcl::PointCloud<pcl::PointXYZRGB> &act_subs = pcs[j];
00271                 //for (int z = 0; z < act_subs.points.size() ; z++) {
00272                 //ROS_INFO("pt : %f", act_subs.points[z].x);
00273                 //}
00274                 ROS_INFO("PRE k %i j %i items %i subs items %i",k, j, act_foreground.points.size(), act_subs.points.size());
00275                 //act_foreground = substractPC(act_foreground, act_subs);
00276                 act_foreground = substractPC(act_foreground, trees_map[j]);
00277             }
00278         }
00279 
00280 
00281         if (act_foreground.points.size() > 0)
00282         {
00283             sensor_msgs::PointCloud2 outliermsg;
00284 
00285             pcl::toROSMsg(act_foreground,outliermsg);
00286 
00287             tf::Stamped<tf::Pose> net_stamped = frames[k];
00288             tf::Transform net_transform;
00289             net_transform.setOrigin(net_stamped.getOrigin());
00290             net_transform.setRotation(net_stamped.getRotation());
00291 
00292             sensor_msgs::PointCloud2 pct; //in gripper frame
00293 
00294             pcl_ros::transformPointCloud("/r_gripper_tool_frame",net_transform,outliermsg,pct);
00295 
00296             pct.header = act->header;
00297             pct.header.frame_id = "/map";
00298             pct.header.stamp = ros::Time::now();
00299             pcm_pub_out.publish(pct);
00300 
00301             pcl::PointCloud<pcl::PointXYZRGB> *actPC = new pcl::PointCloud<pcl::PointXYZRGB>();
00302             pcl::fromROSMsg(pct,*actPC);
00303             foreground.push_back(*actPC);
00304 
00305         }
00306     }
00307 
00308 
00309     pcl::PointCloud<pcl::PointXYZRGB> foreground_filtered_sum;
00310     //int k = 0;
00311 
00312     std::vector<pcl::KdTree<pcl::PointXYZRGB>::Ptr > foreground_trees;
00313     foreground_trees.resize(foreground.size());
00314     for (int k = 0; k < foreground.size(); ++k)
00315     {
00316         foreground_trees[k] = getTree(foreground[k]);
00317     }
00318 
00319     //while (ros::ok()) {
00320     for (int k = 0; k < foreground.size(); ++k)
00321     {
00322         pcl::PointCloud<pcl::PointXYZRGB> act_foreground = foreground[k];
00323         for (int j = 0; j < foreground.size(); ++j)
00324         {
00325             pcl::PointCloud<pcl::PointXYZRGB> act_subs = foreground[j];
00326             if (fabs((double)j - k) == 1)
00327             {
00328                 //act_foreground = substractPC(act_foreground, act_subs, -0.02);
00329                 act_foreground = substractPC(act_foreground, foreground_trees[j], -.01);
00330             }
00331         }
00332 
00333         foreground_filtered_sum += act_foreground;
00334 
00335         filtered_foreground.push_back(act_foreground);
00336         if (act_foreground.points.size() > 0)
00337         {
00338             sensor_msgs::PointCloud2 outliermsg;
00339 
00340             pcl::toROSMsg(act_foreground,outliermsg);
00341             outliermsg.header = act->header;
00342             outliermsg.header.frame_id = "/map";
00343             outliermsg.header.stamp = ros::Time::now();
00344 
00345             pcm_pub_in.publish(outliermsg);
00346         }
00347 
00348     }
00349 
00350     while (ros::ok())
00351     {
00352         ROS_INFO("PUB FILTERED %i", foreground_filtered_sum.points.size() );
00353         if (foreground_filtered_sum.points.size() > 0)
00354         {
00355             sensor_msgs::PointCloud2 outliermsg;
00356 
00357             pcl::toROSMsg(foreground_filtered_sum,outliermsg);
00358             outliermsg.header = act->header;
00359             outliermsg.header.frame_id = "/map";
00360             outliermsg.header.stamp = ros::Time::now();
00361 
00362             pcm_pub_in.publish(outliermsg);
00363 
00364             for (int z =0; z < 10; z++)
00365             {
00366                 ros::spinOnce();
00367                 ros::Duration(1).sleep();
00368             }
00369         }
00370     }
00371 
00372     ros::spin();
00373 }*/
00374 #include <stdio.h>
00375 #include <ctype.h>
00376 
00377 std::string removeWhitespace(const std::string in)
00378 {
00379     std::string ret;
00380     for (int k = 0; k < in.size(); ++k)
00381     {
00382         if (in[k] != ' ')
00383             ret += in[k];
00384     }
00385     return ret;
00386 }
00387 
00388 void mangle(const char filename[])
00389 {
00390     ros::NodeHandle node_handle;
00391     ros::Publisher pcm_pub_in = node_handle.advertise<sensor_msgs::PointCloud2>( "/pc_inlier", 10 , true);
00392     ros::Publisher pcm_pub_out = node_handle.advertise<sensor_msgs::PointCloud2>( "/pc_outlier", 10 , true);
00393 
00394     ros::Publisher pcm_pub_amap = node_handle.advertise<sensor_msgs::PointCloud2>( "/pc_map", 10 , true);
00395     ros::Publisher pcm_pub_atool = node_handle.advertise<sensor_msgs::PointCloud2>( "/pc_tool", 10 , true);
00396 
00397     bool done = false;
00398     int numclouds = 0;
00399 
00400     std::vector<pcl::PointCloud<pcl::PointXYZRGB> > pcs;
00401     std::vector<pcl::PointCloud<pcl::PointXYZRGB> > pcs_tool;
00402     std::vector<pcl::PointCloud<pcl::PointXYZRGB> > pcs_map;
00403     std::vector<pcl::PointCloud<pcl::PointXYZRGB> > foreground;
00404     std::vector<pcl::PointCloud<pcl::PointXYZRGB> > filtered_foreground;
00405     std::vector<pcl::PointCloud<pcl::PointXYZRGB> > background;
00406 
00407     pcl::PointCloud<pcl::PointXYZRGB> allInMap;
00408     pcl::PointCloud<pcl::PointXYZRGB> allInTool;
00409 
00410     //std::vector<tf::Stamped<tf::Pose> > frames;
00411     std::vector<tf::Stamped<tf::Pose> > fr_cl_to_base;
00412     std::vector<tf::Stamped<tf::Pose> > fr_cl_to_tool;
00413     std::vector<tf::Stamped<tf::Pose> > fr_cl_to_map;
00414     std::vector<tf::Stamped<tf::Pose> > fr_base_to_map;
00415 
00416     ROS_INFO("READY");
00417 
00418     boost::shared_ptr<const sensor_msgs::PointCloud2> act;
00419 
00420     ros::Time lastTime = ros::Time::now();
00421 
00422     rosbag::Bag bag;
00423     bag.open(filename, rosbag::bagmode::Read);
00424 
00425     std::string base_to_map = "/base_to_map";
00426     std::string cl_to_base = "/cl_to_base";
00427     std::string cl_to_map = "/cl_to_map";
00428     std::string cl_to_tool = "/cl_to_tool";
00429     std::string pc = "/pc";
00430 
00431     // Image topics to load
00432     std::vector<std::string> topics;
00433     topics.push_back(base_to_map);
00434     topics.push_back(cl_to_base);
00435     topics.push_back(cl_to_map);
00436     topics.push_back(cl_to_tool);
00437     topics.push_back(pc);
00438 
00439     //rosbag::View view(bag, rosbag::TopicQuery(topics));
00440     rosbag::View view(bag);
00441 
00442     // Load all messages into our stereo dataset
00443     BOOST_FOREACH(rosbag::MessageInstance const m, view)
00444     {
00445         //ROS_INFO("topic of msg: %s|", m.getTopic().c_str());
00446 
00447         std::string topicname = removeWhitespace(m.getTopic());
00448 
00449         //boost::trim_right_if(topicname, &isspace);
00450         ROS_INFO("topic of msg: %s|", topicname.c_str());
00451 
00452         if (topicname == pc || ("/" + m.getTopic() == pc))
00453         {
00454 
00455             sensor_msgs::PointCloud2::ConstPtr msg = m.instantiate<sensor_msgs::PointCloud2>();
00456             if (msg != NULL)
00457             {
00458                 pcl::PointCloud<pcl::PointXYZRGB> actPC = pcl::PointCloud<pcl::PointXYZRGB>();
00459                 pcl::fromROSMsg(*msg,actPC);
00460                 pcl::PointCloud<pcl::PointXYZRGB> *actPCclean = new  pcl::PointCloud<pcl::PointXYZRGB>();
00461                 for (size_t k = 0; k < actPC.points.size(); ++k)
00462                 {
00463                     if (!isnan(actPC.points[k].x))
00464                         actPCclean->push_back(actPC.points[k]);
00465                 }
00466                 pcs.push_back(*actPCclean);
00467 
00468                 sensor_msgs::PointCloud2 outliermsg;
00469 
00470                 pcl::toROSMsg(*actPCclean,outliermsg);
00471 
00472                 outliermsg.header.frame_id = "/map";
00473                 outliermsg.header.stamp = ros::Time::now();
00474 
00475                 pcm_pub_out.publish(outliermsg);
00476 
00477                 ROS_INFO("%zu PC Cleaned SIZE %zu", pcs.size(), actPCclean->points.size());
00478             }
00479         } // (c) Nico The Gregor
00480 
00481         if (topicname == cl_to_tool || ("/" + m.getTopic() == cl_to_tool))
00482         {
00483             // ROS_INFO("fr_cl_to_tool size %zu", fr_cl_to_tool.size());
00484 
00485             geometry_msgs::PoseStamped::ConstPtr msg = m.instantiate<geometry_msgs::PoseStamped>();
00486             if (msg != NULL)
00487             {
00488                 tf::Stamped<tf::Pose> act;
00489                 tf::poseStampedMsgToTF(*msg, act);
00490                 fr_cl_to_tool.push_back(act);
00491                 ROS_INFO("fr_cl_to_tool size %zu", fr_cl_to_tool.size());
00492             }
00493         }
00494 
00495         if (topicname == cl_to_base || ("/" + m.getTopic() == cl_to_base))
00496         {
00497             // ROS_INFO("cl_to_base size %zu", cl_to_base.size());
00498 
00499             geometry_msgs::PoseStamped::ConstPtr msg = m.instantiate<geometry_msgs::PoseStamped>();
00500             if (msg != NULL)
00501             {
00502                 tf::Stamped<tf::Pose> act;
00503                 tf::poseStampedMsgToTF(*msg, act);
00504                 fr_cl_to_base.push_back(act);
00505                 ROS_INFO("cl_to_base size %zu", cl_to_base.size());
00506             }
00507         }
00508 
00509         if (topicname == cl_to_map || ("/" + m.getTopic() == cl_to_map))
00510         {
00511             //ROS_INFO("cl_to_map size %zu", cl_to_map.size());
00512 
00513             geometry_msgs::PoseStamped::ConstPtr msg = m.instantiate<geometry_msgs::PoseStamped>();
00514             if (msg != NULL)
00515             {
00516                 tf::Stamped<tf::Pose> act;
00517                 tf::poseStampedMsgToTF(*msg, act);
00518                 fr_cl_to_map.push_back(act);
00519                 ROS_INFO("cl_to_map size %zu", cl_to_map.size());
00520             }
00521         }
00522 
00523         if (topicname == base_to_map || ("/" + m.getTopic() == base_to_map))
00524         {
00525             // ROS_INFO("base_to_map size %zu", base_to_map.size());
00526 
00527             geometry_msgs::PoseStamped::ConstPtr msg = m.instantiate<geometry_msgs::PoseStamped>();
00528             if (msg != NULL)
00529             {
00530                 tf::Stamped<tf::Pose> act;
00531                 tf::poseStampedMsgToTF(*msg, act);
00532                 fr_base_to_map.push_back(act);
00533                 ROS_INFO("base_to_map size %zu", base_to_map.size());
00534             }
00535         }
00536     }
00537 
00538     for (size_t k =0; k < pcs.size(); ++k)
00539     {
00540         sensor_msgs::PointCloud2 msg,msg_tool,msg_map;
00541         pcl::toROSMsg(pcs[k],msg);
00542         {
00543             pcl_ros::transformPointCloud("/map",fr_cl_to_base[k],msg,msg_map);
00544             pcl::PointCloud<pcl::PointXYZRGB> *actPC = new  pcl::PointCloud<pcl::PointXYZRGB>();
00545             pcl::fromROSMsg(msg_map,*actPC);
00546             pcs_map.push_back(*actPC);
00547             ROS_INFO("pcs_map%zu", pcs_map.size());
00548         }
00549         //if (0)
00550         {
00551             pcl_ros::transformPointCloud("/r_gripper_tool_frame",fr_cl_to_tool[k],msg,msg_tool);
00552             pcl::PointCloud<pcl::PointXYZRGB> *actPC = new  pcl::PointCloud<pcl::PointXYZRGB>();
00553             pcl::fromROSMsg(msg_tool,*actPC);
00554             pcs_tool.push_back(*actPC);
00555             ROS_INFO("pcs_tool %zu", pcs_tool.size());
00556         }
00557     }
00558 
00559 
00560     std::vector<pcl::KdTree<pcl::PointXYZRGB>::Ptr > trees_tool;
00561     trees_tool.resize(pcs_tool.size());
00562     for (size_t  k = 0; k < pcs_tool.size(); ++k)
00563     {
00564         trees_tool[k] = getTree(pcs_tool[k]);
00565     }
00566 
00567     std::vector<pcl::KdTree<pcl::PointXYZRGB>::Ptr > trees_map;
00568     trees_map.resize(pcs_map.size());
00569     for (size_t  k = 0; k < pcs_map.size(); ++k)
00570     {
00571         trees_map[k] = getTree(pcs_map[k]);
00572     }
00573 
00574     double threshold = 0.02;
00575     double threshold_squared = threshold * threshold;
00576 
00577     // k is the time index
00578     for (int k = 0; k < pcs.size(); ++k)
00579     {
00580         foreground.push_back(*(new pcl::PointCloud<pcl::PointXYZRGB>()));
00581         background.push_back(*(new pcl::PointCloud<pcl::PointXYZRGB>()));
00582 
00583         double avg_map = 0;
00584         double avg_tool = 0;
00585 
00586         for (int i = 0; i < pcs[k].points.size(); i += 1)
00587         {
00588 
00589             int numMapHits = 0;
00590             int numToolHits = 0;
00591             pcl::PointXYZRGB inMap = pcs_map[k].points[i];
00592             pcl::PointXYZRGB inTool = pcs_tool[k].points[i];
00593 
00594             allInMap.points.push_back(inMap);
00595             allInTool.points.push_back(inTool);
00596 
00597 
00598             for (int o=0; o<pcs.size(); ++o)
00599             {
00600                 if (pointDist(inMap,trees_map[o]) <= threshold_squared)
00601                     numMapHits ++;
00602                 if (pointDist(inTool,trees_tool[o]) <= threshold_squared)
00603                     numToolHits ++;
00604             }
00605 
00606             avg_map += numMapHits;
00607             avg_tool += numToolHits;
00608 
00609             if ((numToolHits > numMapHits + 2) && (numToolHits > 3))
00610             {
00611                 foreground[0].points.push_back(inTool);
00612             }
00613 
00614             if ((numToolHits + 2 < numMapHits) && (numMapHits > 3))
00615             {
00616                 background[0].points.push_back(inMap);
00617             }
00618 
00619             //fr_cl_to_tool[o]
00620         }
00621 
00622         ROS_INFO("AVERAGES : map %f tool %f", avg_map / pcs[k].size(), avg_tool / pcs[k].size());
00623 
00624         ROS_INFO("foreground size : %zu", foreground[0].points.size());
00625 
00626         {
00627             sensor_msgs::PointCloud2 outliermsg;
00628 
00629             pcl::toROSMsg(foreground[0],outliermsg);
00630 
00631             outliermsg.header.frame_id = "/map";
00632             outliermsg.header.stamp = ros::Time::now();
00633 
00634             btTransform to_map = fr_cl_to_map[0] * fr_cl_to_tool[0].inverse();
00635             sensor_msgs::PointCloud2 pctm;
00636             outliermsg.header.frame_id = "/tmp";
00637             pcl_ros::transformPointCloud("/map",to_map ,outliermsg,pctm);
00638             pctm.header.stamp = ros::Time::now();
00639             pcm_pub_in.publish(pctm);
00640         }
00641         {
00642             sensor_msgs::PointCloud2 outliermsg;
00643 
00644             pcl::toROSMsg(background[0],outliermsg);
00645 
00646             outliermsg.header.frame_id = "/map";
00647             outliermsg.header.stamp = ros::Time::now();
00648 
00649             pcm_pub_out.publish(outliermsg);
00650         }
00651 
00652 
00653         std::ostringstream filename;
00654         filename << k;
00655 
00656         pcl::io::savePCDFile(std::string("allTool" + filename.str() + ".PCD"), allInTool, true);
00657         pcl::io::savePCDFile(std::string("allMap" + filename.str() + ".PCD"), allInMap, true);
00658 
00659         pcl::io::savePCDFile(std::string("background" + filename.str() + ".PCD"), background[0], true);
00660         pcl::io::savePCDFile(std::string("foreground" + filename.str() + ".PCD"), foreground[0], true);
00661 
00662         ROS_INFO("saved");
00663 
00664     }
00665 
00666     ROS_INFO("DONE DONE DONE DONE DONE DONE DONE DONE DONE DONE DONE DONE DONE DONE DONE DONE DONE DONE ");
00667 
00668     while (ros::ok())
00669     {
00670         ros::Duration(1.0).sleep();
00671         {
00672             sensor_msgs::PointCloud2 outliermsg;
00673 
00674             pcl::toROSMsg(foreground[0],outliermsg);
00675 
00676             outliermsg.header.frame_id = "/map";
00677             outliermsg.header.stamp = ros::Time::now();
00678 
00679             btTransform to_map = fr_cl_to_map[0] * fr_cl_to_tool[0].inverse();
00680             sensor_msgs::PointCloud2 pctm;
00681             outliermsg.header.frame_id = "/tmp";
00682             pcl_ros::transformPointCloud("/map",to_map ,outliermsg,pctm);
00683             pctm.header.stamp = ros::Time::now();
00684             pcm_pub_in.publish(pctm);
00685         }
00686         {
00687             sensor_msgs::PointCloud2 outliermsg;
00688 
00689             pcl::toROSMsg(background[0],outliermsg);
00690 
00691             outliermsg.header.frame_id = "/map";
00692             outliermsg.header.stamp = ros::Time::now();
00693 
00694             pcm_pub_out.publish(outliermsg);
00695         }
00696 
00697         if (0)
00698         {
00699             sensor_msgs::PointCloud2 outliermsg;
00700 
00701             pcl::toROSMsg(allInMap,outliermsg);
00702 
00703             outliermsg.header.frame_id = "/map";
00704             outliermsg.header.stamp = ros::Time::now();
00705 
00706             pcm_pub_amap.publish(outliermsg);
00707         }
00708 
00709         if (0)
00710         {
00711             sensor_msgs::PointCloud2 outliermsg;
00712 
00713             pcl::toROSMsg(allInTool,outliermsg);
00714 
00715             outliermsg.header.frame_id = "/map";
00716             outliermsg.header.stamp = ros::Time::now();
00717 
00718             btTransform to_map = fr_cl_to_map[0] * fr_cl_to_tool[0].inverse();
00719             sensor_msgs::PointCloud2 pctm;
00720             outliermsg.header.frame_id = "/tmp";
00721             pcl_ros::transformPointCloud("/map",to_map ,outliermsg,pctm);
00722             pctm.header.stamp = ros::Time::now();
00723             pcm_pub_atool.publish(pctm);
00724         }
00725     }
00726 
00727     //exit(0);
00728 
00729 
00730     /*
00731       for (size_t  k = 1; k < pcs.size() - 1; ++k)
00732       {
00733           pcl::PointCloud<pcl::PointXYZRGB> act_foreground = pcs_map[k];
00734           act_foreground.is_dense = false;
00735           for (size_t j = 0; j < pcs.size(); ++j)
00736           {
00737               //  if ((fabs((double)j - k) > 1))
00738               if (fabs((double)j - k) == 1)
00739               {
00740                   ROS_INFO("PRE k %i j %i items %zu ",k, j, act_foreground.points.size());
00741                   act_foreground = substractPC(act_foreground, trees_map[j]);
00742               }
00743           }
00744 
00745 
00746           if (act_foreground.points.size() > 0)
00747           {
00748               sensor_msgs::PointCloud2 outliermsg;
00749 
00750               pcl::toROSMsg(act_foreground,outliermsg);
00751 
00752               //tf::Stamped<tf::Pose>
00753               //tf::Transform net_transform;
00754               //net_transform.setOrigin(net_stamped.getOrigin());
00755               //net_transform.setRotation(net_stamped.getRotation());
00756 
00757               sensor_msgs::PointCloud2 pct; //in gripper frame
00758 
00759               //btTransform net_stamped = fr_cl_to_map[k].inverse();
00760               btTransform inverted = fr_cl_to_tool[k] * fr_cl_to_base[k].inverse();
00761 
00762               pcl_ros::transformPointCloud("/map",inverted,outliermsg,pct);
00763               //outliermsg = pct;
00764               //pcl_ros::transformPointCloud("/r_gripper_tool_frame",fr_cl_to_tool[k],outliermsg,pct);
00765 
00766               //pct.header = act->header;
00767               pct.header.frame_id = "/map";
00768               pct.header.stamp = ros::Time::now();
00769 
00770               pcl::PointCloud<pcl::PointXYZRGB> *actPC = new pcl::PointCloud<pcl::PointXYZRGB>();
00771               pcl::fromROSMsg(pct,*actPC);
00772               foreground.push_back(*actPC);
00773 
00774               btTransform to_map = fr_cl_to_map[k] * fr_cl_to_tool[k].inverse();
00775               sensor_msgs::PointCloud2 pctm;
00776               pct.header.frame_id = "/tmp";
00777               pcl_ros::transformPointCloud("/map",to_map ,pct,pctm);
00778               pcm_pub_out.publish(pctm);
00779           }
00780       }
00781 
00782       ROS_ERROR("pcs.size %i foregroud_size %i", pcs.size(), foreground.size());
00783 
00784 
00785       pcl::PointCloud<pcl::PointXYZRGB> foreground_filtered_sum;
00786 
00787       std::vector<pcl::KdTree<pcl::PointXYZRGB>::Ptr > foreground_trees;
00788       foreground_trees.resize(foreground.size());
00789       for (size_t  k = 0; k < foreground.size(); ++k)
00790       {
00791           foreground_trees[k] = getTree(foreground[k]);
00792       }
00793 
00794       for (size_t k = 1; k < foreground.size(); ++k)
00795       {
00796           pcl::PointCloud<pcl::PointXYZRGB> act_foreground = foreground[k];
00797           for (int j = 0; j < foreground.size(); ++j)
00798           {
00799               if (fabs((double(j + 1) - k)) == 1)
00800               {
00801                   //act_foreground = substractPC(act_foreground, act_subs, -0.02);
00802                   act_foreground = substractPC(act_foreground, foreground_trees[j], -.01);
00803               }
00804           }
00805 
00806           foreground_filtered_sum += act_foreground;
00807 
00808           filtered_foreground.push_back(act_foreground);
00809           if (act_foreground.points.size() > 0)
00810           {
00811               sensor_msgs::PointCloud2 outliermsg;
00812 
00813               pcl::toROSMsg(act_foreground,outliermsg);
00814               //outliermsg.header = act->header;
00815               outliermsg.header.frame_id = "/map";
00816               outliermsg.header.stamp = ros::Time::now();
00817               btTransform to_map = fr_cl_to_map[k] * fr_cl_to_tool[k].inverse();
00818               sensor_msgs::PointCloud2 pctm;
00819               outliermsg.header.frame_id = "/tmp";
00820               pcl_ros::transformPointCloud("/map",to_map ,outliermsg,pctm);
00821               pcm_pub_in.publish(pctm);
00822           }
00823 
00824       }
00825 
00826       while (ros::ok())
00827       {
00828           ROS_INFO("PUB FILTERED %i", foreground_filtered_sum.points.size() );
00829           if (foreground_filtered_sum.points.size() > 0)
00830           {
00831               sensor_msgs::PointCloud2 outliermsg;
00832 
00833               pcl::toROSMsg(foreground_filtered_sum,outliermsg);
00834               //outliermsg.header = act->header;
00835               outliermsg.header.frame_id = "/map";
00836               outliermsg.header.stamp = ros::Time::now();
00837 
00838 
00839               btTransform to_map = fr_cl_to_map[0] * fr_cl_to_tool[1].inverse();
00840               sensor_msgs::PointCloud2 pctm;
00841               outliermsg.header.frame_id = "/tmp";
00842               pcl_ros::transformPointCloud("/map",to_map ,outliermsg,pctm);
00843               pcm_pub_in.publish(pctm);
00844 
00845               for (int z =0; (z < 10) && ros::ok(); z++)
00846               {
00847                   ros::spinOnce();
00848                   ros::Duration(1).sleep();
00849               }
00850           }
00851       } */
00852 
00853 }
00854 
00855 typedef actionlib::SimpleActionClient<articulation_pr2::ArticulatedObjectAction> Client;
00856 
00857 double reachable_distance(tf::Stamped<tf::Pose> secondlast, tf::Stamped<tf::Pose> last)
00858 {
00859 
00860     btTransform t[1000];
00861 
00862     t[0].setOrigin(secondlast.getOrigin());
00863     t[0].setRotation(secondlast.getRotation());
00864     t[1].setOrigin(last.getOrigin());
00865     t[1].setRotation(last.getRotation());
00866 
00867     ROS_INFO("START");
00868     bool reachable = true;
00869 
00870     double distance = 0;
00871     //calulate the distance we can further follow the extrapolated trajectory within the workspace bounds
00872     for (int k = 2; reachable && (k < 1000); ++k)
00873     {
00874         t[k] = t[k-1] * (t[0].inverseTimes(t[1]));
00875         tf::Stamped<tf::Pose> act;
00876         act.frame_id_ = last.frame_id_;
00877         act.setOrigin(t[k].getOrigin());
00878         act.setRotation(t[k].getRotation());
00879         //pubPose(act);
00880         //printPose("interpolated", act);
00881         geometry_msgs::PoseStamped pose;
00882         double start_angles[7];
00883         double solution[7];
00884         tf::poseStampedTFToMsg(RobotArm::getInstance(0)->tool2wrist(act),pose);
00885         reachable &= RobotArm::getInstance(0)->run_ik(pose,start_angles,solution,"r_wrist_roll_link");
00886         if (reachable)
00887             distance += (t[k].getOrigin() - t[k-1].getOrigin()).length();
00888         //ROS_INFO(reachable ? "reachable %f" : "not reachable %f", distance);
00889     }
00890     return distance;
00891 }
00892 
00893 void calc_better_base_pose(articulation_pr2::ArticulatedObjectResultConstPtr result, double desired_length)
00894 {
00895     if (result->posterior_model.track.pose_resampled.size() < 2)
00896     {
00897         ROS_ERROR("too few points (%i) in the trajectory to interpolate it", (int)result->posterior_model.track.pose_resampled.size());
00898         return;
00899     }
00900 
00901     double trajectory_length = 0;
00902     geometry_msgs::Pose act = result->posterior_model.track.pose_resampled[0];
00903     tf::Pose lastTF;
00904     tf::poseMsgToTF(act,lastTF);
00905 
00906     for (int pos_i = 0; pos_i < result->posterior_model.track.pose_resampled.size(); ++pos_i)
00907     {
00908         geometry_msgs::Pose act = result->posterior_model.track.pose_resampled[pos_i];
00909         ROS_INFO("%f %f %f  %f %f %f %f", act.position.x, act.position.y, act.position.z,
00910                  act.orientation.x,  act.orientation.y,  act.orientation.z,  act.orientation.w);
00911         tf::Pose actTF;
00912         tf::poseMsgToTF(act,actTF);
00913         trajectory_length += (actTF.getOrigin() - lastTF.getOrigin()).length();
00914         lastTF = actTF;
00915     }
00916     ROS_INFO("TRAJECTORY LENGTH: %f", trajectory_length);
00917 
00918 
00919     tf::Pose tmpPose;
00920     tf::Stamped<tf::Pose> secondlast;
00921     tf::poseMsgToTF(result->posterior_model.track.pose_resampled[result->posterior_model.track.pose_resampled.size() - 2],tmpPose);
00922     secondlast.setOrigin(tmpPose.getOrigin());
00923     secondlast.setRotation(tmpPose.getRotation());
00924     secondlast.frame_id_ = result->posterior_model.track.header.frame_id;
00925 
00926     tf::Stamped<tf::Pose> last;
00927     tf::poseMsgToTF(result->posterior_model.track.pose_resampled[result->posterior_model.track.pose_resampled.size() - 1],tmpPose);
00928     last.setOrigin(tmpPose.getOrigin());
00929     last.setRotation(tmpPose.getRotation());
00930     last.frame_id_ = result->posterior_model.track.header.frame_id;
00931 
00932     double dist = reachable_distance(secondlast,last);
00933 
00934     ROS_INFO("XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX");
00935     ROS_INFO("FURTHER REACHABLE DIST : %f", dist);
00936     ROS_INFO("XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX");
00937 
00938     // check if we find a pose that bringts our trajectory extrapolated to the desired_length into the workspace, if not so already
00939     std::vector<int> arm;
00940     std::vector<tf::Stamped<tf::Pose> > goal;
00941     if (trajectory_length  < desired_length)
00942     {
00943 
00944         double covered_length = 0;
00945         geometry_msgs::Pose act = result->posterior_model.track.pose_resampled[0];
00946         tf::Pose lastTF;
00947         tf::poseMsgToTF(act,lastTF);
00948 
00949         for (int pos_i = 0; pos_i < result->posterior_model.track.pose_resampled.size(); ++pos_i)
00950         {
00951             geometry_msgs::Pose act = result->posterior_model.track.pose_resampled[pos_i];
00952             ROS_INFO("%f %f %f  %f %f %f %f", act.position.x, act.position.y, act.position.z,
00953                      act.orientation.x,  act.orientation.y,  act.orientation.z,  act.orientation.w);
00954             tf::Pose actTF;
00955             tf::poseMsgToTF(act,actTF);
00956 
00957             tf::Stamped<tf::Pose> ps;
00958             ps.frame_id_ = result->posterior_model.track.header.frame_id;
00959             ps.setOrigin(actTF.getOrigin());
00960             ps.setRotation(actTF.getRotation());
00961             arm.push_back(0);
00962             goal.push_back(ps);
00963 
00964             covered_length +=  (actTF.getOrigin() - lastTF.getOrigin()).length();
00965 
00966             lastTF = actTF;
00967         }
00968         btTransform t[1000];
00969 
00970         t[0].setOrigin(secondlast.getOrigin());
00971         t[0].setRotation(secondlast.getRotation());
00972         t[1].setOrigin(last.getOrigin());
00973         t[1].setRotation(last.getRotation());
00974 
00975         for (int k = 2; (covered_length < desired_length) && (k < 1000); ++k)
00976         {
00977             t[k] = t[k-1] * (t[0].inverseTimes(t[1]));
00978             tf::Stamped<tf::Pose> act;
00979             act.frame_id_ = last.frame_id_;
00980             act.setOrigin(t[k].getOrigin());
00981             act.setRotation(t[k].getRotation());
00982             covered_length += (t[k].getOrigin() - t[k-1].getOrigin()).length();
00983             arm.push_back(0);
00984             goal.push_back(act);
00985         }
00986         tf::Stamped<tf::Pose> betterBasePose;
00987         RobotArm::findBaseMovement(betterBasePose, arm, goal, false, false);
00988 
00989         ROS_ERROR("<base> BETTER BASE POSE: bin/ias_drawer_executive -4 %f %f %f %f", betterBasePose.getOrigin().x(), betterBasePose.getOrigin().y(), betterBasePose.getRotation().z(), betterBasePose.getRotation().w());
00990         betterBasePose = Geometry::getPoseIn("map", betterBasePose);
00991         ROS_WARN("<map> BETTER BASE POSE: bin/ias_drawer_executive -4 %f %f %f %f", betterBasePose.getOrigin().x(), betterBasePose.getOrigin().y(), betterBasePose.getRotation().z(), betterBasePose.getRotation().w());
00992     }
00993 
00994 }
00995 
00996 
00997 int articulate(const string & filename, double desired_length,
00998                const string & kinect_rgb_optical_frame = "/openni_rgb_optical_frame", const string & arm = "r",
00999                const string & kinect_cloud_topic = "/kinect/cloud_throttled", bool model = true)
01000 {
01001 
01002     ROS_INFO("ARTICULATE %s %f %s %s %s", filename.c_str(), desired_length, kinect_rgb_optical_frame.c_str(), arm.c_str(), kinect_cloud_topic.c_str());
01003 
01004     ros::NodeHandle node_handle;
01005 
01006     ros::Publisher pct_pub = node_handle.advertise<sensor_msgs::PointCloud2>( "/pc", 10 , true);
01007 
01008     Client client(arm + "_articulate_object", true); // true -> don't need ros::spin()
01009     client.waitForServer();
01010     {
01011         articulation_pr2::ArticulatedObjectGoal goal;
01012         goal.timeout = ros::Duration(20.00);
01013         goal.initial_pulling_direction.header.frame_id = "torso_lift_link";
01014         goal.initial_pulling_direction.vector.x = -1;// pull towards robot
01015         goal.desired_velocity = 0.04;
01016 
01017         // Fill in goal here
01018         client.sendGoal(goal);
01019         client.waitForResult(ros::Duration(5.0));
01020         while (client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
01021         {
01022             //if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
01023             printf("Current State: %s\n", client.getState().toString().c_str());
01024             if (client.getState() == actionlib::SimpleClientGoalState::ABORTED)
01025             {
01026                 ROS_ERROR("SLIPPED OFF OF HANDLE ?");
01027                 return -1;
01028             }
01029             ros::Duration(0.5).sleep();
01030         }
01031 
01032         articulation_pr2::ArticulatedObjectResultConstPtr result = client.getResult();
01033         //write result to bag file
01034         {
01035             rosbag::Bag bag;
01036             bag.open(filename, rosbag::bagmode::Write);
01037 
01038             bag.write("articulation_result", ros::Time::now(), result);
01039 
01040             bag.close();
01041         }
01042 
01043         ROS_INFO("FRAME of articulation model : %s", result->posterior_model.track.header.frame_id.c_str());
01044         //std::vector<geometry_msgs::Pose>::iterator pose_it;
01045         //for (pose_it = result->posterior_model.track.pose_projected.begin(); pose_it != result->posterior_model.track.pose_projected.end(); ++pose_it) {
01046         tf::Stamped<tf::Pose> lastPose;
01047         lastPose.setOrigin(btVector3(0,0,10000));
01048 
01049         //RobotHead::getInstance()->lookAtThreaded("r_gripper_tool_frame",0,0,0);
01050 
01051         /*rosbag::Bag bag;
01052 
01053         bag.open(filename, rosbag::bagmode::Write);
01054 
01055             bag.write("articulation_result", ros::Time::now(), result);
01056 
01057             bag.close();*/
01058 
01059         //calculate how long the trajectory could have been follow within the current workspace to decide if we hit a workspace limit or the end of the
01060         // articulation model
01061         calc_better_base_pose(result,desired_length);
01062 
01063         if (!model)
01064             return 0;
01065 
01066 
01067         rosbag::Bag cloudbag;
01068         cloudbag.open(filename + "_clouds", rosbag::bagmode::Write);
01069 
01071         tf::Pose middle;
01072         tf::poseMsgToTF(result->posterior_model.track.pose_resampled[result->posterior_model.track.pose_resampled.size() / 2], middle);
01073         RobotHead::getInstance()->lookAtThreaded(result->posterior_model.track.header.frame_id.c_str(),middle.getOrigin().x(),middle.getOrigin().y(),middle.getOrigin().z());
01074 
01075         std::vector<tf::Stamped<tf::Pose> > opening_trajectory;
01076         for (int pos_i = 0; pos_i < result->posterior_model.track.pose_resampled.size() - 1; ++pos_i)
01077         {
01078             geometry_msgs::Pose act = result->posterior_model.track.pose_resampled[pos_i];
01079             //ROS_INFO("%f %f %f  %f %f %f %f", act.position.x, act.position.y, act.position.z,
01080             //       act.orientation.x,  act.orientation.y,  act.orientation.z,  act.orientation.w);
01081             tf::Pose actTF;
01082             tf::poseMsgToTF(act,actTF);
01083             tf::Stamped<tf::Pose> actSTF;
01084             actSTF.setOrigin(actTF.getOrigin());
01085             actSTF.setRotation(actTF.getRotation());
01086             actSTF.frame_id_ = result->posterior_model.track.header.frame_id.c_str();
01087             if (pos_i == 0)
01088             {
01089                 if (arm == "r")
01090                     Gripper::getInstance(0)->open();
01091                 else
01092                     Gripper::getInstance(1)->open();
01093             }
01094             //RobotHead::getInstance()->lookAtThreaded("r_gripper_tool_frame",0,0,0);
01095 
01096             bool reachable = RobotArm::getInstance(arm == "r" ? 0 : 1)->reachable(actSTF);
01097 
01098             //ROS_INFO("REACHABLE: %s", reachable ? " YES." : " NO.");
01099 
01100             btTransform relative = lastPose.inverseTimes(actSTF);
01101             btVector3 axis = relative.getRotation().getAxis();
01102             //ROS_INFO("AXIS OF ROTATION : %f %f %f, angle %f", axis.x(),axis.y(),axis.z(), relative.getRotation().getAngle());
01103 
01104 
01105             if ((RobotArm::getInstance(arm == "r" ? 0 : 1)->reachable(actSTF)) &&
01106                     (((actSTF.getOrigin() - lastPose.getOrigin()).length() >= 0.05))) // || (pos_i == result->posterior_model.track.pose_resampled.size() - 1)))
01107             {
01108                 //            printPose("OPENING TRAJECTORY: ", actSTF);
01109 
01110                 //if (!reachable)
01111                 //  ROS_ERROR("INSIDE: %s", reachable ? " YES." : " NO.");
01112                 opening_trajectory.push_back (actSTF);
01113                 if (arm == "r")
01114                     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(actSTF);
01115                 else
01116                     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(actSTF);
01117                 lastPose = actSTF;
01118 
01119 
01120                 {
01121 
01122                     tf::Stamped<tf::Pose> cam = Geometry::getPose("/map", kinect_rgb_optical_frame.c_str());
01123                     tf::Stamped<tf::Pose> grip = Geometry::getPose("map",("/" + arm + "_gripper_tool_frame").c_str());
01124 
01125                     geometry_msgs::PoseStamped camPS;
01126                     tf::poseStampedTFToMsg(cam,camPS);
01127 
01128                     geometry_msgs::PoseStamped gripPS;
01129                     tf::poseStampedTFToMsg(grip,gripPS);
01130 
01131                     sensor_msgs::PointCloud2 pc = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>(kinect_cloud_topic));
01132 
01133                     //tf::Stamped<tf::Pose> net_stamped = RobotArm::getPose(("/" + arm + "_gripper_tool_frame").c_str(), kinect_rgb_optical_frame.c_str());
01134                     //tf::Transform net_transform;
01135                     //net_transform.setOrigin(net_stamped.getOrigin());
01136                     //net_transform.setRotation(net_stamped.getRotation());
01137 
01138                     //sensor_msgs::PointCloud2 pct; //in gripper frame
01139 
01140                     //pcl_ros::transformPointCloud("/" + arm + "_gripper_tool_frame",net_transform,pc,pct);
01141                     //pct.header.frame_id = "/map";
01142 
01143                     //xxx void pcl_ros::transformPointCloud     (       const std::string &     target_frame,           const tf::Transform &   net_transform,          const sensor_msgs::PointCloud2 &        in,             sensor_msgs::PointCloud2 &      out
01144 
01145                     //pcl::PointCloud<pcl::PointXYZ> cloud;
01146 
01147                     //pcl::fromROSMsg(pct,cloud);
01148 
01149                     //pct_pub.publish(pct);
01150                     pct_pub.publish(pc);
01151 
01152                     cloudbag.write("/pc", pc.header.stamp, pc);
01153 
01154                     {
01155                         tf::Stamped<tf::Pose> cl_to_tool = Geometry::getPose(("/" + arm + "_gripper_tool_frame").c_str(), kinect_rgb_optical_frame.c_str());
01156                         //printPose("via tf", cl_to_tool);
01157                         cl_to_tool = Geometry::getPoseIn(kinect_rgb_optical_frame.c_str(), actSTF);
01158                         btTransform trans = cl_to_tool;
01159                         trans = trans.inverse();
01160                         cl_to_tool.setOrigin(trans.getOrigin());
01161                         cl_to_tool.setRotation(trans.getRotation());
01162                         cl_to_tool.frame_id_ = ("/" + arm + "_gripper_tool_frame").c_str();
01163                         //printPose("via model", cl_to_tool);
01164                         geometry_msgs::PoseStamped cl_to_tool_msg;
01165                         tf::poseStampedTFToMsg(cl_to_tool,cl_to_tool_msg);
01166                         cloudbag.write("/cl_to_tool", pc.header.stamp, cl_to_tool_msg);
01167                     }
01168 
01169                     {
01170                         tf::Stamped<tf::Pose> cl_to_base  = Geometry::getPose("/base_link", kinect_rgb_optical_frame.c_str());
01171                         geometry_msgs::PoseStamped cl_to_base_msg;
01172                         tf::poseStampedTFToMsg(cl_to_base,cl_to_base_msg);
01173                         cloudbag.write("/cl_to_base", pc.header.stamp, cl_to_base_msg);
01174                     }
01175 
01176                     {
01177                         tf::Stamped<tf::Pose> cl_to_map  = Geometry::getPose("/map", kinect_rgb_optical_frame.c_str());
01178                         geometry_msgs::PoseStamped cl_to_map_msg;
01179                         tf::poseStampedTFToMsg(cl_to_map,cl_to_map_msg);
01180                         cloudbag.write("/cl_to_map", pc.header.stamp, cl_to_map_msg);
01181                     }
01182 
01183                     {
01184                         tf::Stamped<tf::Pose> base_to_map  = Geometry::getPose("/map", "/base_link");
01185                         geometry_msgs::PoseStamped base_to_map_msg;
01186                         tf::poseStampedTFToMsg(base_to_map,base_to_map_msg);
01187                         cloudbag.write("/base_to_map", pc.header.stamp, base_to_map_msg);
01188                     }
01189 
01190                     //ros::Rate rt(50.0);
01191 
01192                     //for (int k = 0; k < 10; ++k)
01193                     //{
01194                     //    rt.sleep();
01195                     //    ros::spinOnce();
01196                     //}
01197                 }
01198 
01199             }
01200 
01201             if (pos_i == 0)
01202             {
01203                 if (arm == "r")
01204                     Gripper::getInstance(0)->close();
01205                 else
01206                     Gripper::getInstance(1)->close();
01207             }
01208         }
01209 
01210         cloudbag.close();
01211 
01212         for (int pos_i = result->posterior_model.track.pose_resampled.size() - 2; pos_i >= 0 ; --pos_i)
01213         {
01214             geometry_msgs::Pose act = result->posterior_model.track.pose_resampled[pos_i];
01215             //ROS_INFO("%f %f %f  %f %f %f %f", act.position.x, act.position.y, act.position.z,
01216             //act.orientation.x,  act.orientation.y,  act.orientation.z,  act.orientation.w);
01217             tf::Pose actTF;
01218             tf::poseMsgToTF(act,actTF);
01219             tf::Stamped<tf::Pose> actSTF;
01220             actSTF.setOrigin(actTF.getOrigin());
01221             actSTF.setRotation(actTF.getRotation());
01222             actSTF.frame_id_ = result->posterior_model.track.header.frame_id.c_str();
01223 
01224             if ((RobotArm::getInstance(arm == "r" ? 0 : 1)->reachable(actSTF)) &&
01225                     (((actSTF.getOrigin() - lastPose.getOrigin()).length() >= 0.10) || (pos_i == result->posterior_model.track.pose_resampled.size() - 1)))
01226             {
01227                 if (arm == "r")
01228                     RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(actSTF);
01229                 else
01230                     RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(actSTF);
01231                 lastPose = actSTF;
01232             }
01233 
01234             if (pos_i == 0)
01235             {
01236                 if (arm == "r")
01237                     Gripper::getInstance(0)->open();
01238                 else
01239                     Gripper::getInstance(1)->open();
01240             }
01241         }
01242 
01243 
01244         for (uint ot_point = 0; ot_point < opening_trajectory.size(); ot_point++)
01245             printPose("OPENING TRaJECT0RY: ", opening_trajectory[ot_point]);
01246         yamlWriter trajectory_writer;
01247         trajectory_writer.writeTrajectory (std::string(filename) + ".yaml", opening_trajectory);
01248         std::cerr << "Trajectory written to: " << std::string(filename) + ".yaml" << std::endl;
01249 
01250         printf("Yay! The dishes are now clean");
01251     }
01252     if (0)
01253     {
01254         articulation_pr2::ArticulatedObjectGoal goal;
01255         goal.timeout = ros::Duration(20.00);
01256         goal.initial_pulling_direction.header.frame_id = "torso_lift_link";
01257         goal.initial_pulling_direction.vector.x = 1;// pull towards robot
01258         goal.desired_velocity = 0.04;
01259 
01260         // Fill in goal here
01261         client.sendGoal(goal);
01262         client.waitForResult(ros::Duration(5.0));
01263         while (client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
01264         {
01265             //if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
01266             printf("Current State: %s\n", client.getState().toString().c_str());
01267             ros::Duration(0.5).sleep();
01268         }
01269         printf("Yay! The dishes are now clean");
01270     }
01271 
01272     return 0;
01273 }
01274 
01275 tf::Stamped<tf::Pose> toPose(double x, double y, double z, double ox, double oy, double oz, double ow, const char fixed_frame[])
01276 {
01277     tf::Stamped<tf::Pose> p0;
01278     p0.frame_id_=fixed_frame;
01279     p0.stamp_=ros::Time();
01280     p0.setOrigin(btVector3(x,y,z));
01281     p0.setRotation(btQuaternion(ox,oy,oz,ow));
01282     return p0;
01283 }
01284 
01285 tf::Stamped<tf::Pose> toPose(const char text[], const char fixed_frame[])
01286 {
01287     double x,y,z, ox,oy,oz,ow;
01288     sscanf(text,"%f %f %f %f %f %f %f", &x, &y, &z, &ox, &oy, &oz, &ow);
01289     return toPose(x,y,z, ox,oy,oz,ow,fixed_frame);
01290 }
01291 
01292 void copyState(double *src, double *tar, int num = 7)
01293 {
01294     for (int i = 0; i < 7 ; ++i)
01295         tar[i] = src[i];
01296 }
01297 
01298 void addState(double *a, double *b, double *tar, int num = 7)
01299 {
01300     for (int i = 0; i < 7 ; ++i)
01301         tar[i] = a[i] + b[i];
01302 }
01303 
01304 void addStateDiscounted(double *a, double *b, double discount, double *tar, int num = 7)
01305 {
01306     for (int i = 0; i < 7 ; ++i)
01307         tar[i] = a[i] + (b[i] * discount);
01308 }
01309 
01310 
01311 
01312 void cart_err_monit(int side)
01313 {
01314     RobotArm::RobotArm *arm = RobotArm::getInstance(side);
01315     ros::Rate rate(5);
01316     while (ros::ok())
01317     {
01318         ros::spinOnce();
01319         rate.sleep();
01320         btVector3 err = arm->cartError();
01321         ROS_INFO("CARTESIAN ERROR :D %f x %f y %f z %f", err.length(), err.x(), err.y(), err.z());
01322     }
01323 }
01324 
01325 
01326 
01327 void findBothArms(tf::Stamped<tf::Pose> leftArm, tf::Stamped<tf::Pose> rightArm)
01328 {
01329 
01330     std::vector<int> arm;
01331     std::vector<tf::Stamped<tf::Pose> > goal;
01332     tf::Stamped<tf::Pose> result;
01333 
01334     arm.push_back(1);
01335     goal.push_back(leftArm);
01336     arm.push_back(0);
01337     goal.push_back(rightArm);
01338 
01339     RobotArm::findBaseMovement(result, arm, goal,false, false);
01340 }
01341 
01342 
01343 ros::NodeHandle *node_handle_ = 0;
01344 bool have_vis_pub = false;
01345 ros::Publisher vis_pub_;
01346 
01347 void pubPose(tf::Stamped<tf::Pose> &t)
01348 {
01349 
01350     if (!have_vis_pub)
01351     {
01352         vis_pub_ = node_handle_->advertise<geometry_msgs::PoseStamped>( "/cart_poses", 100, true );
01353         have_vis_pub = true;
01354     }
01355 
01356     geometry_msgs::PoseStamped pose;
01357     pose.header.frame_id = t.frame_id_;
01358     pose.header.stamp = ros::Time::now();
01359     pose.pose.orientation.x = t.getRotation().x();
01360     pose.pose.orientation.y = t.getRotation().y();
01361     pose.pose.orientation.z = t.getRotation().z();
01362     pose.pose.orientation.w = t.getRotation().w();
01363     pose.pose.position.x = t.getOrigin().x();
01364     pose.pose.position.y = t.getOrigin().y();
01365     pose.pose.position.z = t.getOrigin().z();
01366 
01367     //ros::Rate rate(10.0);
01368     vis_pub_.publish( pose );
01369 
01370     //ROS_INFO("Published Pose");
01371     for (int k = 0; k < 10; ++k)
01372     {
01373         ros::spinOnce();
01374         ros::Duration(0.01).sleep();
01375     }
01376 }
01377 
01378 void pubPose(tf::Stamped<tf::Pose> &t, ros::Publisher vis_pub)
01379 {
01380     geometry_msgs::PoseStamped pose;
01381     pose.header.frame_id = t.frame_id_;
01382     pose.header.stamp = ros::Time::now();
01383     pose.pose.orientation.x = t.getRotation().x();
01384     pose.pose.orientation.y = t.getRotation().y();
01385     pose.pose.orientation.z = t.getRotation().z();
01386     pose.pose.orientation.w = t.getRotation().w();
01387     pose.pose.position.x = t.getOrigin().x();
01388     pose.pose.position.y = t.getOrigin().y();
01389     pose.pose.position.z = t.getOrigin().z();
01390 
01391     //ros::Rate rate(10.0);
01392     vis_pub.publish( pose );
01393 
01394     ROS_INFO("Published Pose");
01395 
01396     ros::spinOnce();
01397 }
01398 
01399 
01400 tf::Stamped<tf::Pose> turnToSide(int side,tf::Stamped<tf::Pose> toolPose, tf::Stamped<tf::Pose> bowlPose)
01401 {
01402     toolPose = Geometry::getPoseIn("/base_link", toolPose);
01403     bowlPose = Geometry::getPoseIn("/base_link", bowlPose);
01404 
01405     RobotArm *arm = RobotArm::getInstance(side);
01406 
01407     tf::Stamped<tf::Pose> bestPose = toolPose;
01408     tf::Stamped<tf::Pose> worstPose = toolPose;
01409     double bestX = -100;
01410     double bestAngle = 0;
01411     double worstX = 100;
01412 
01413     for (double angle = -M_PI; (angle < 2 * M_PI) && (ros::ok()); angle += M_PI / 10.0)
01414     {
01415         btTransform curr;
01416         curr.setOrigin(toolPose.getOrigin());
01417         curr.setRotation(toolPose.getRotation());
01418 
01419         bowlPose.setRotation(btQuaternion(0,0,0,1));
01420 
01421         tf::Stamped<tf::Pose> act = Geometry::rotateAroundPose(toolPose, bowlPose, 0,0, angle);
01422 
01423         tf::Stamped<tf::Pose> actInBase = Geometry::getPoseIn("/base_link", act);
01424         double actx = actInBase.getOrigin().y() * (side ? 1 : -1);
01425         if (actx > bestX)
01426         {
01427             bestX = actx;
01428             bestPose = act;
01429             bestAngle = angle;
01430         }
01431         else if (actx < worstX)
01432         {
01433             worstX = actx;
01434             worstPose = act;
01435         }
01436     }
01437 
01438     return bestPose;
01439 }
01440 
01441 
01442 
01443 tf::Stamped<tf::Pose> turnBowlStraight(int currside, tf::Stamped<tf::Pose> &bowlPose)
01444 {
01445 
01446     RobotHead::getInstance()->lookAt("/base_link",1,0,0.5);
01447 
01448     RobotArm *arm = RobotArm::getInstance(currside);
01449 
01450     bowlPose = OperateHandleController::getBowlPose();
01451     bowlPose = Geometry::getPoseIn("/base_link", bowlPose);
01452     pubPose(bowlPose);
01453     printPose("Bowl Pose vision", bowlPose);
01454 
01455     tf::Stamped<tf::Pose> bowlInBase = Geometry::getPoseIn("/base_link", bowlPose);
01456     //bowlInBase.setOrigin(btVector3(0,0,0));
01457 
01458     tf::Stamped<tf::Pose> bowlCorrected = bowlPose;
01459     bowlCorrected.setRotation(bowlInBase.getRotation().inverse() * bowlPose.getRotation());
01460 
01461     pubPose(bowlCorrected);
01462     pubPose(bowlCorrected);
01463     pubPose(bowlCorrected);
01464     printPose("Bowl Pose corrected", bowlCorrected);
01465 
01466     tf::Stamped<tf::Pose> newToolPose = arm->getToolPose("/base_link");
01467     printPose("Tool Pose", newToolPose);
01468     btQuaternion cor = bowlInBase.getRotation().inverse();
01469 
01470     printPose("Bowl Pose", bowlInBase);
01471     tf::Stamped<tf::Pose> actt = Geometry::rotateAroundPose(newToolPose, bowlInBase, cor);
01472     printPose("act Pose", actt);
01473 
01474     //newToolPose.setRotation(bowlInBase.getRotation().inverse() * newToolPose.getRotation());
01475 
01476     printPose("nt Pose (rot)", newToolPose);
01477 
01478     //newToolPose = Geometry::getPoseIn("/map", newToolPose);
01479     //return newToolPose;
01480     return actt;
01481 }
01482 
01483 void releaseWhenPulled(int side)
01484 {
01485 
01486     RobotArm *arm = RobotArm::getInstance(side);
01487     Gripper *grip = Gripper::getInstance(side);
01488     ros::Rate rate(5);
01489     double threshold = 0;
01490     double numtaken = 0;
01491     while (ros::ok())
01492     {
01493         ros::spinOnce();
01494         rate.sleep();
01495         btVector3 err = arm->cartError();
01496         ROS_INFO("CARTESIAN ERROR :D %f x %f y %f z %f", err.length(), err.x(), err.y(), err.z());
01497 
01498         threshold += err.length();
01499         numtaken += 1;
01500 
01501         ROS_INFO("CARTESIAN ERROR :D %f (%f) x %f y %f z %f", err.length(), threshold / numtaken, err.x(), err.y(), err.z());
01502 
01503         if ((numtaken > 5) && (err.length() > (threshold / numtaken) * 3.0))
01504         {
01505             boost::thread t1(&Gripper::open, grip, 0.09);
01506             tf::Stamped<tf::Pose> toolPose = arm->getToolPose("/map");
01507             toolPose.setOrigin(toolPose.getOrigin() + btVector3(0,0,0.05));
01508             arm->universal_move_toolframe_ik_pose(toolPose);
01509             return;
01510         }
01511 
01512     }
01513 }
01514 
01515 int current(int argc, char** argv);
01516 
01517 int measures(int argc, char** argv);
01518 
01519 int main(int argc, char** argv)
01520 {
01521     // Init the ROS node
01522     //char nodename[] = "ias_drawer_executive"'
01523     //sprintf(nodename,"%s%i",nodename,
01524 
01525     ros::init(argc, argv, "ias_drawer_executive");
01526 
01527     printf("ias_drawer_executive %i\n", argc);
01528     printf("usage: ias_drawer_executive <drawer_position> <drawer_height> [<drawer_position> <drawer_height> .. <drawer_position> <drawer_height>] \n");
01529     printf("drawer_position:-1 .. dont move base \n");
01530     printf("                 0 .. island left \n");
01531     printf("                 1 .. island middle \n");
01532     printf("                 2 .. island right \n");
01533     printf("                 3 .. under oven \n");
01534     printf("                 4 .. left of dishwasher \n");
01535     printf("                 5 .. dishwasher \n");
01536     printf("                 6 .. right of dishwasher \n");
01537     printf("                 7 .. right of oven \n");
01538     printf("                 8 .. fridge \n");
01539     printf("drawer_height:-1 .. dont grasp/open drawer\n");
01540     printf("               0 .. low (~34cm) \n");
01541     printf("               1 .. middle (~64cm) \n");
01542     printf("               2 .. high (~78cm) \n");
01543     printf("               3 .. higher (~97cm) \n\n");
01544 
01545     ros::NodeHandle node_handle;
01546     node_handle_ = &node_handle;
01547 
01548     ROS_INFO("argc %i", argc);
01549     for (int i = 0; i < argc; i ++)
01550     {
01551         ROS_INFO("i %i, %s", i, argv[i]);
01552     }
01553 
01554     current(argc, argv);
01555     measures(argc, argv);
01556 
01557 
01558     /*if ((argc > 1) && (atoi(argv[1]) > -2) )
01559         for (int i = 0; i < (argc - 1) / 2 ; ++i)
01560         {
01561             if (argc >= (i * 2 + 3))
01562             {
01563                 if (atoi(argv[i* 2 + 1]) >= 0)
01564                 {
01565                     ROS_INFO("TARGET POSE IN MAP %f %f %f %f",Poses::poses[atoi(argv[i* 2 + 1])][0],Poses::poses[atoi(argv[i* 2 + 1])][1],Poses::poses[atoi(argv[i* 2 + 1])][2],Poses::poses[atoi(argv[i* 2 + 1])][3]);
01566                     RobotDriver::getInstance()->moveBase(Poses::poses[atoi(argv[i* 2 + 1])]);
01567                 }
01568                 if (atoi(argv[i* 2 + 2]) >= 0)
01569                     OperateHandleController::operateHandle(atoi(argv[i* 2 + 2]));
01570             }
01571         }*/
01572 
01573     if ((argc > 1) && (atoi(argv[1]) > -2) )
01574         for (int i = 0; i < (argc - 1) / 2 ; ++i)
01575         {
01576             if (argc >= (i * 2 + 3))
01577             {
01578                 if (atoi(argv[i* 2 + 1]) >= 0)
01579                 {
01580                     Torso *torso = Torso::getInstance();
01581                     //boost::shared_ptr<boost::thread> t;
01582                     //if (atoi(argv[i* 2 + 1])==8)
01583                     //t = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&Torso::up, torso)));
01584                     //else
01585                     //t = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&Torso::down, torso)));
01586                     boost::thread t((atoi(argv[i* 2 + 1])==8) ? &Torso::up : &Torso::down, torso);
01587                     ROS_INFO("TARGET POSE IN MAP %f %f %f %f",Poses::poses[atoi(argv[i* 2 + 1])][0],Poses::poses[atoi(argv[i* 2 + 1])][1],Poses::poses[atoi(argv[i* 2 + 1])][2],Poses::poses[atoi(argv[i* 2 + 1])][3]);
01588                     RobotDriver::getInstance()->moveBase(Poses::poses[atoi(argv[i* 2 + 1])]);
01589                     t.join();
01590                 }
01591                 if (atoi(argv[i* 2 + 2]) >= 0)
01592                 {
01593                     tf::Stamped<tf::Pose> aM  = OperateHandleController::getHandlePoseFromMarker(0,atoi(argv[i* 2 + 2]));
01594                     OperateHandleController::operateHandle(0,aM);
01595                 }
01596             }
01597         }
01598 
01600     if (atoi(argv[1]) == -4)
01601     {
01602 
01603         RobotArm::getInstance(0)->tucked = true;
01604         for (int i = 0; i < (argc - 1 ) / 4; ++i)
01605         {
01606             double p[4];
01607             p[0] = atof(argv[2 + i * 4]);
01608             p[1] = atof(argv[3 + i * 4]);
01609             p[2] = atof(argv[4 + i * 4]);
01610             p[3] = atof(argv[5 + i * 4]);
01611             ROS_INFO("going to %f %f %f %f", p[0], p[1], p[2], p[3]);
01612             RobotDriver::getInstance()->moveBase(p);
01613         }
01614     }
01615 
01616     if (atoi(argv[1]) == -45)
01617     {
01618 
01619         //while (ros::ok()) {
01620         //   rate.sleep();
01621         //   ros::spinOnce();
01622         //   double r[2],l[2];
01623         //   p->getCenter(r,l);
01624         //   //ROS_INFO("CENTERS %f %f , %f %f", r[0], r[1], l[0], l[1]);
01625         //   double d[2];
01626         //   d[0] = r[0] - l[0];
01627         //   d[1] = r[1] - l[1];
01628         //   double k = fabs(d[1]);
01629         //   if (k > 1)
01630         //     k = 1;
01631         //   ROS_INFO("DIFF %f %f            argv %f", d[0], d[1], atof(argv[1]));
01632         //   if (d[1] > limit) {
01633         //      ROS_INFO("pos");
01634         //      arm->rotate_toolframe_ik(increment * k,0,0);
01635         //   }
01636         //   if (d[1] < -limit) {
01637         //      ROS_INFO("neg");
01638         //      arm->rotate_toolframe_ik(-increment * k,0,0);
01639         //   }
01640         //
01641         RobotArm::getInstance(atoi(argv[2]))->rotate_toolframe_ik(atof(argv[3]),atof(argv[4]),atof(argv[5]));
01642     }
01643 
01644     if (atoi(argv[1]) == -5)
01645     {
01646         //RobotArm arm;
01647         Gripper *gripper = Gripper::getInstance(0);
01648         gripper->open();
01649         Pressure::getInstance()->reset();
01650         gripper->close();
01651         RobotArm *arm = RobotArm::getInstance();
01652         arm->stabilize_grip();
01653     }
01654 
01656     if (atoi(argv[1]) == -3)
01657     {
01658         //bool move_ik
01659         double x,y,z,ox,oy,oz,ow;
01660         x = atof(argv[3]);
01661         y = atof(argv[4]);
01662         z = atof(argv[5]);
01663         ox = atof(argv[6]);
01664         oy = atof(argv[7]);
01665         oz = atof(argv[8]);
01666         ow = atof(argv[9]);
01667         printf("moving tool to ik goal pos in map %f %f %f or %f %f %f %f \n", x,y,z,ox,oy,oz,ow);
01668         while (ros::ok())
01669         {
01670             RobotArm::getInstance(atoi(argv[2]))->universal_move_toolframe_ik(x,y,z,ox,oy,oz,ow,"map");
01671             if (argc < 11)
01672                 return 0;
01673         }
01674     }
01675 
01676     if (atoi(argv[1]) == -300)
01677     {
01678         //bool move_ik
01679         double x,y,z,ox,oy,oz,ow;
01680         x = atof(argv[3]);
01681         y = atof(argv[4]);
01682         z = atof(argv[5]);
01683         ox = atof(argv[6]);
01684         oy = atof(argv[7]);
01685         oz = atof(argv[8]);
01686         ow = atof(argv[9]);
01687         double elbow_hint = atof(argv[10]);
01688         RobotArm::getInstance(atoi(argv[2]))->raise_elbow = true;
01689         RobotArm::getInstance(atoi(argv[2]))->preset_angle = elbow_hint;
01690         printf("moving tool to ik goal pos in map %f %f %f or %f %f %f %f \n", x,y,z,ox,oy,oz,ow);
01691         RobotArm::getInstance(atoi(argv[2]))->universal_move_toolframe_ik(x,y,z,ox,oy,oz,ow,"map");
01692         RobotArm::getInstance(atoi(argv[2]))->raise_elbow = false;
01693     }
01694 
01696     if (atoi(argv[1]) == -2)
01697     {
01698         //bool move_ik
01699         double x,y,z,ox,oy,oz,ow;
01700         x = atof(argv[3]);
01701         y = atof(argv[4]);
01702         z = atof(argv[5]);
01703         ox = atof(argv[6]);
01704         oy = atof(argv[7]);
01705         oz = atof(argv[8]);
01706         ow = atof(argv[9]);
01707         printf("moving tool %s to ik goal pos %f %f %f or %f %f %f %f \n", (atoi(argv[2])==0) ? "right" : "left" , x,y,z,ox,oy,oz,ow);
01708         RobotArm::getInstance(atoi(argv[2]))->universal_move_toolframe_ik(x,y,z,ox,oy,oz,ow,"base_link");
01709         // arm->move_toolframe_ik(x,y,z,ox,oy,oz,ow);
01710 
01711         //btQuaternion myq(ox,oy,oz,ow);
01712 
01713         //btQuaternion qx = myq *     btQuaternion(M_PI / 2,0,0);;
01714         //btQuaternion qy = myq *     btQuaternion(0,M_PI / 2,0);;
01715         //btQuaternion qz = myq *     btQuaternion(0,0,M_PI / 2);;
01716 
01717         //ROS_INFO("ROT X %f %f %f %f", qx.x(),qx.y(),qx.z(),qx.w());
01718         //ROS_INFO("ROT Y %f %f %f %f", qy.x(),qy.y(),qy.z(),qy.w());
01719         //ROS_INFO("ROT Z %f %f %f %f", qz.x(),qz.y(),qz.z(),qz.w());
01720 
01721         //arm->move_ik(x,y,z,ox,oy,oz,ow);
01722     }
01723 
01725     if (atoi(argv[1]) == -6)
01726     {
01727         //bool move_ik
01728         double x,y,z,ox,oy,oz,ow;
01729         x = atof(argv[3]);
01730         y = atof(argv[4]);
01731         z = atof(argv[5]);
01732         ox = atof(argv[6]);
01733         oy = atof(argv[7]);
01734         oz = atof(argv[8]);
01735         ow = atof(argv[9]);
01736         printf("moving tool to ik goal pos in map %f %f %f or %f %f %f %f \n", x,y,z,ox,oy,oz,ow);
01737         RobotArm::getInstance(atoi(argv[2]))->universal_move_toolframe_ik(x,y,z,ox,oy,oz,ow,argv[10]);
01738     }
01739 
01740 
01741     //if (atoi(argv[1]) == -6)
01742     //{
01743     //while (ros::ok())
01744     //{
01745     //tf::Stamped<tf::Pose> aM = AverageTF::getMarkerTransform("/4x4_1",20);
01746     //}
01747     //}
01748 
01749     //if (atoi(argv[1]) == -7)
01750     //{
01751     //RobotArm::getInstance(1)->startTrajectory(RobotArm::getInstance(1)->twoPointTrajectory(Poses::lf0,Poses::lf1));
01752     //RobotArm::getInstance(1)->startTrajectory(RobotArm::getInstance(1)->twoPointTrajectory(Poses::lf2,Poses::lf3));
01753     //}
01754 
01755 
01756     //if (atoi(argv[1]) == -8)
01757     //{
01758     //    printf("ias_drawer_executive -8 arm(0=r,1=l) r_x r_y r_z frame_id");
01759     //    printf("rotate around frame downstream from wrist");
01760     //   RobotArm::getInstance(atoi(argv[2]))->rotate_toolframe_ik(atof(argv[3]),atof(argv[4]),atof(argv[5]), argv[6]);
01761     //}
01762 
01764     if (atoi(argv[1]) == -9)
01765         DemoScripts::takePlate();
01766 
01768     if (atoi(argv[1]) == -10)
01769     {
01770         double target[4];
01771         target[0] = atof(argv[2]);
01772         target[1] = atof(argv[3]);
01773         target[2] = 0;
01774         target[3] = 1;
01775         ROS_INFO("POSE IN BASE %f %f %f", target[0],target[1], target[2]);
01776         RobotDriver::getInstance()->driveInOdom(target, 1);
01777     }
01778 
01780     if (atoi(argv[1]) == -11)
01781     {
01782         btVector3 vec = OperateHandleController::getPlatePose();
01783         ROS_INFO("POSE IN MAP %f %f %f", vec.x(), vec.y(), vec.z());
01784     }
01785 
01787     if (atoi(argv[1]) == -12)
01788         DemoScripts::takeBottle();
01790     if (atoi(argv[1]) == -13)
01791     {
01792         RobotArm::getInstance(0)->raise_elbow = true;
01793         RobotArm::getInstance(1)->raise_elbow = true;
01794         OperateHandleController::spinnerL(atof(argv[2]), atof(argv[3]),atof(argv[4]));
01795     }
01796 
01797     if (atoi(argv[1]) == -14)
01798     {
01799 
01800         RobotArm::getInstance(1)->universal_move_toolframe_ik(1.25, -0.7000, 1, 0.005001, -0.053009, -0.029005, 0.998160,"map");
01801         RobotArm::getInstance(1)->universal_move_toolframe_ik(1.165, -0.655, 1.151, 0.005, -0.053, -0.029, 0.998, "map");
01802         RobotArm::getInstance(1)->universal_move_toolframe_ik(0.9, -0.655, 1.251, 0.005, -0.053, -0.029, 0.998, "map");
01803         RobotArm::getInstance(1)->universal_move_toolframe_ik(0.9, -0.255, 1.251, 0.005, -0.053, -0.029, 0.998, "map");
01804     }
01805 
01806     if (atoi(argv[1]) == -15)   //check for maximum base movement
01807     {
01808         RobotDriver *driver = RobotDriver::getInstance();
01809         double relativePose[2] = {0,0};
01810         for (relativePose[0] = 0; relativePose[0] < 1; relativePose[0] = relativePose[0] + 0.025)
01811         {
01812             ROS_INFO("CAN DRIVE %f %i", relativePose[0], driver->checkCollision(relativePose));
01813         }
01814     }
01815 
01816     if (atoi(argv[1]) == -16)
01817     {
01818         std::vector<int> arm;
01819         std::vector<tf::Stamped<tf::Pose> > goal;
01820         tf::Stamped<tf::Pose> result;
01821 
01822         tf::Stamped<tf::Pose> p0;
01823         p0.frame_id_="map";
01824         p0.stamp_=ros::Time();
01825         p0.setOrigin(btVector3(0.9, -0.255, 1.251));
01826         p0.setRotation(btQuaternion(0.005, -0.053, -0.029, 0.998));
01827         goal.push_back(p0);
01828         arm.push_back(1);
01829 
01830         RobotArm::findBaseMovement(result, arm, goal,true, false);
01831 
01832         RobotArm::getInstance(1)->universal_move_toolframe_ik(0.9, -0.255, 1.251, 0.005, -0.053, -0.029, 0.998, "map");
01833     }
01834 
01835     if (atoi(argv[1]) == -17)
01836     {
01837         //    tf::Stamped<tf::Pose> toPose(double x, double y, double z, double ox, double oy, double oz, double ow, const char fixed_frame[])
01838         char posestrings[][100] = {{"0.542329 -0.981344 1.23612 0.0330708  0.00176235 0.595033  0.803019"},
01839             {"0.556517 -0.920874 1.23603 0.0298695 -0.00432607 0.545786  0.837381"},
01840             {"0.572956 -0.861407 1.23635 0.0268959 -0.00695657 0.494586  0.868685"},
01841             {"0.581633 -0.828156 1.23589 0.0252092 -0.00877769 0.475406  0.879362"},
01842             {"0.604209 -0.781168 1.23568 0.0179254  0.00291321 0.436192  0.89967"},
01843             {"0.622422 -0.748086 1.23539 0.0152842  0.00913191 0.340254  0.940165"},
01844             {"0.639726 -0.721124 1.23581 0.0164386  0.0162648  0.278332  0.960206"},
01845             {"0.661803 -0.694327 1.23576 0.0219128  0.0176437  0.246738  0.968674"},
01846             {"0.698809 -0.657863 1.23729 0.0284113  0.0207615  0.238006  0.970626"},
01847             {"0.740968 -0.625336 1.23853 0.0375375  0.0235445  0.227272  0.972823"},
01848             {"0.784319 -0.598149 1.23992 0.0450486  0.0276238  0.212765  0.975673"},
01849             {"0.82972  -0.572236 1.24111 0.0451087  0.0322855  0.169386  0.983987"},
01850             {"0.875225 -0.553171 1.24195 0.0358857  0.032936   0.122281  0.9913"},
01851             {"0.956881 -0.532016 1.24365 0.0254018  0.0329301  0.0418438 0.998258"},
01852             {"0.972319 -0.530781 1.2467  0.0022027  0.0133422  0.0679044 0.9976"},
01853             {"1.02179  -0.52356  1.24665 0.0022027  0.0133422  0.0679044 0.9976"}
01854         };
01855         tf::Stamped<tf::Pose> oldposes[20];
01856         for (int k = 0; k < 16; ++k)
01857         {
01858             oldposes[k] = toPose(posestrings[k],"map");
01859             printPose("traj : ", oldposes[k]);
01860         }
01861         for (int k = 1; k < 14; ++k)
01862         {
01863             tf::Pose diffPose = oldposes[k].inverse() * oldposes[k-1];
01864             //diffPose.setRotation(diffPose.getRotation().normalize());
01865             //printPose("difference", diffPose);
01866             double distance = diffPose.getOrigin().length();
01867             btQuaternion adjusted = diffPose.getRotation()  * (1 / distance);
01868             adjusted.normalize();
01869             ROS_INFO("DIST : %f QUAT ADJ : %f %f %f %f", distance, adjusted.x(), adjusted.y(), adjusted.z(), adjusted.w());
01870         }
01871     }
01872 
01873 
01874     if (atoi(argv[1]) == -18)
01875     {
01876         Gripper::getInstance(atoi(argv[2]))->open();
01877     }
01878     if (atoi(argv[1]) == -19)
01879     {
01880         if (argc == 4)
01881             Gripper::getInstance(atoi(argv[2]))->close(atof(argv[3]));
01882         else
01883             Gripper::getInstance(atoi(argv[2]))->close();
01884     }
01885 
01886     if (atoi(argv[1]) == -20)
01887     {
01888         Torso::getInstance()->pos(atof(argv[2]));
01889     }
01890 
01892     if (atoi(argv[1]) == -22)
01893     {
01894         //RobotHead::getInstance()->lookAt(argv[2],atof(argv[3]),atof(argv[4]),atof(argv[5]));
01895         RobotHead::getInstance()->lookAtThreaded(argv[2],atof(argv[3]),atof(argv[4]),atof(argv[5]));
01896         ros::Rate rt(10);
01897         int numpy = 50;
01898         while (ros::ok() && (--numpy > 0))
01899         {
01900             ros::spinOnce();
01901             rt.sleep();
01902         }
01903         RobotHead::getInstance()->stopThread();
01904         while (ros::ok())
01905         {
01906             ros::spinOnce();
01907             rt.sleep();
01908         }
01909     }
01910 
01911     if (atoi(argv[1]) == -23)
01912     {
01913         tf::Stamped<tf::Pose> in;
01914         in.setOrigin(btVector3(atof(argv[2]),atof(argv[3]),atof(argv[4])));
01915         in.frame_id_ = "base_link";
01916         tf::Stamped<tf::Pose> out = Geometry::getPoseIn("map",in);
01917         printf("pos in map %f %f %f \n", out.getOrigin().x(),out.getOrigin().y(),out.getOrigin().z());
01918     }
01919 
01920     // move tool without driving
01921     if (atoi(argv[1]) == -24)
01922     {
01923         //bool move_ik
01924         double x,y,z,ox,oy,oz,ow;
01925         x = atof(argv[3]);
01926         y = atof(argv[4]);
01927         z = atof(argv[5]);
01928         ox = atof(argv[6]);
01929         oy = atof(argv[7]);
01930         oz = atof(argv[8]);
01931         ow = atof(argv[9]);
01932         printf("moving tool to ik goal pos in map %f %f %f or %f %f %f %f \n", x,y,z,ox,oy,oz,ow);
01933         //RobotArm::getInstance(atoi(argv[2]))->universal_move_toolframe_ik(x,y,z,ox,oy,oz,ow,"map");
01934 
01935         tf::Stamped<tf::Pose> p0;
01936         p0.frame_id_="map";
01937         p0.stamp_=ros::Time();
01938         p0.setOrigin(btVector3(x,y,z));
01939         p0.setRotation(btQuaternion(ox,oy,oz,ow));
01940         RobotArm::getInstance(atoi(argv[2]))->move_toolframe_ik_pose(p0);
01941     }
01942 
01943     // test marker stuff
01944     if (atoi(argv[1]) == -25)
01945     {
01946         ros::NodeHandle node_handle;
01947         ros::Publisher vis_pub = node_handle.advertise<visualization_msgs::Marker>( "/robot_arm_marker", 0 );
01948 
01949         visualization_msgs::Marker marker;
01950         marker.header.frame_id = "/base_link";
01951         marker.header.stamp = ros::Time::now();
01952         marker.ns = "goalpoints";
01953         marker.id =  10000;
01954         marker.type = visualization_msgs::Marker::ARROW;
01955         marker.action = visualization_msgs::Marker::ADD;
01956 
01957         double x,y,z,ox,oy,oz,ow;
01958         x = atof(argv[3]);
01959         y = atof(argv[4]);
01960         z = atof(argv[5]);
01961         ox = atof(argv[6]);
01962         oy = atof(argv[7]);
01963         oz = atof(argv[8]);
01964         ow = atof(argv[9]);
01965         marker.pose.orientation.x = ox;
01966         marker.pose.orientation.y = oy;
01967         marker.pose.orientation.z = oz;
01968         marker.pose.orientation.w = ow;
01969         marker.pose.position.x = x;
01970         marker.pose.position.y = y;
01971         marker.pose.position.z = z;
01972         marker.scale.x = 0.1;
01973         marker.scale.y = 0.1;
01974         marker.scale.z = 0.1;
01975         marker.color.r = 1.0;
01976         marker.color.g = 0.0;
01977         marker.color.b = 0.0;
01978         marker.color.a = 1.0;
01979 
01980         ROS_INFO("PUBLISH MARKER %i", marker.id);
01981 
01982         ros::Rate rate(10.0);
01983         while (ros::ok())
01984         {
01985             marker.id = ++marker.id;
01986             marker.pose.position.x = marker.pose.position.x + .01;
01987             vis_pub.publish( marker );
01988             ros::spinOnce();
01989             rate.sleep();
01990         }
01991     }
01992 
01994     if (atoi(argv[1]) == -26)
01995     {
01996         OperateHandleController::openGrippers();
01997     }
01998 
01999     if (atoi(argv[1]) == -27)
02000     {
02001         Perception3d::getHandlePoseFromLaser(0);
02002     }
02003 
02004 
02006     if (atoi(argv[1]) == -28)
02007     {
02008         RobotHead::getInstance()->lookAt("/map", 1.243111, -0.728864, 0.9);
02009         std::vector<tf::Stamped<tf::Pose> *> handlePoses;
02010         handlePoses.clear();
02011         tf::Stamped<tf::Pose> bottle = Perception3d::getBottlePose();
02012         ROS_INFO("BOTTLE in %s : %f %f %f", bottle.frame_id_.c_str(), bottle.getOrigin().x(),bottle.getOrigin().y(),bottle.getOrigin().z());
02013     }
02014 
02016     if (atoi(argv[1]) == -29)
02017     {
02018         std::vector<tf::Stamped<tf::Pose> > targetPose;
02019         std::vector<std::string> frame_ids;
02020         RobotDriver::getInstance()->driveToMatch(targetPose, frame_ids);
02021     }
02022 
02023     if (atoi(argv[1]) == -30)
02024     {
02025         Gripper::getInstance(atoi(argv[2]))->updatePressureZero();
02026         Gripper::getInstance(atoi(argv[2]))->closeCompliant(atof(argv[3]));
02027     }
02028 
02030     if (atoi(argv[1]) == -31)
02031         DemoScripts::serveBottle();
02032 
02034     if (atoi(argv[1]) == -32)
02035         DemoScripts::servePlateToIsland();
02036 
02038     /*if (atoi(argv[1]) == -33)
02039     {
02040         double p[] = {.1, 0.896, 0.003, 1.000};
02041         //RobotDriver::getInstance()->moveBase(p, false);
02042 
02043         pr2_controllers_msgs::JointTrajectoryGoal goalA = RobotArm::getInstance(0)->twoPointTrajectory(Poses::prepDishR1,Poses::prepDishR1);
02044         boost::thread t2(&RobotArm::startTrajectory, RobotArm::getInstance(0), goalA);
02045 
02046         Torso *torso = Torso::getInstance();
02047         boost::thread t2u(&Torso::up, torso);
02048 
02049         RobotArm::getInstance(0)->tucked = true;
02050 
02051         RobotDriver::getInstance()->moveBase(p, false);
02052 
02053         t2u.join();
02054         t2.join();
02055 
02056         RobotHead::getInstance()->lookAt("/map",0.449616, 0.9, 1.078124);
02057 
02058         tf::Stamped<tf::Pose> toyPose = Perception3d::getBottlePose(); // in map!
02059 
02060         RobotArm *arm = RobotArm::getInstance(0);
02061 
02062         ROS_INFO("LEGO POSE");
02063         arm->printPose(toyPose);
02064 
02065         tf::Stamped<tf::Pose> toyPoseBase = Geometry::getPoseIn("base_link",toyPose);
02066         toyPoseBase.setRotation(btQuaternion(0,0,0,1));
02067 
02068         tf::Stamped<tf::Pose> ap_30 = toyPoseBase;
02069         ap_30.setOrigin(toyPoseBase.getOrigin() + btVector3(-.3,0,0));
02070         ap_30 = arm->getPoseIn("map", ap_30);
02071 
02072         tf::Stamped<tf::Pose> ap_20 = toyPoseBase;
02073         ap_20.setOrigin(toyPoseBase.getOrigin() + btVector3(-.2,0,0));
02074         ap_20 = arm->getPoseIn("map", ap_20);
02075 
02076         tf::Stamped<tf::Pose> ap_10 = toyPoseBase;
02077         ap_10.setOrigin(toyPoseBase.getOrigin() + btVector3(-.1,0,0));
02078         ap_10 = arm->getPoseIn("map", ap_10);
02079 
02080         tf::Stamped<tf::Pose> ap_05 = toyPoseBase;
02081         ap_05.setOrigin(toyPoseBase.getOrigin() + btVector3(-.05,0,0));
02082         ap_05 = arm->getPoseIn("map", ap_05 );
02083 
02084         tf::Stamped<tf::Pose> ap_00 = toyPoseBase;
02085         ap_00.setOrigin(toyPoseBase.getOrigin() + btVector3(0,0,0));
02086         ap_00 = arm->getPoseIn("map", ap_00);
02087 
02088 
02089         tf::Stamped<tf::Pose> ap_0010 = toyPoseBase;
02090         ap_0010.setOrigin(toyPoseBase.getOrigin() + btVector3(0,0,0.15));
02091         ap_0010 = arm->getPoseIn("map", ap_0010);
02092 
02093         tf::Stamped<tf::Pose> ap_1010 = toyPoseBase;
02094         ap_1010.setOrigin(toyPoseBase.getOrigin() + btVector3(-.1,0,0.15));
02095         ap_1010 = arm->getPoseIn("map", ap_1010);
02096 
02097         tf::Stamped<tf::Pose> ap_3010 = toyPoseBase;
02098         ap_3010.setRotation(btQuaternion(0,0,.707,.707));
02099         ap_3010.setOrigin(toyPoseBase.getOrigin() + btVector3(-.3,0,0.15));
02100         ap_3010 = arm->getPoseIn("map", ap_3010);
02101 
02102         arm->printPose(ap_30);
02103         arm->printPose(ap_20);
02104         arm->printPose(ap_10);
02105         arm->printPose(ap_05);
02106         arm->printPose(ap_00);
02107         ROS_INFO("-");
02108         arm->printPose(ap_0010);
02109         arm->printPose(ap_1010);
02110         arm->printPose(ap_3010);
02111 
02112         Gripper::getInstance(0)->open();
02113 
02114         arm->universal_move_toolframe_ik_pose(ap_30);
02115         arm->universal_move_toolframe_ik_pose(ap_20);
02116         arm->universal_move_toolframe_ik_pose(ap_10);
02117         arm->universal_move_toolframe_ik_pose(ap_05);
02118         arm->universal_move_toolframe_ik_pose(ap_00);
02119 
02120         Gripper::getInstance(0)->closeCompliant();
02121         Gripper::getInstance(0)->close();
02122 
02123         arm->universal_move_toolframe_ik_pose(ap_0010);
02124         arm->universal_move_toolframe_ik_pose(ap_1010);
02125         arm->universal_move_toolframe_ik_pose(ap_3010);
02126     }*/
02127 
02129     if (atoi(argv[1]) == -34)
02130     {
02131         tf::Stamped<tf::Pose> rightTip = RobotArm::getInstance(0)->getToolPose();
02132         tf::Stamped<tf::Pose> leftTip = RobotArm::getInstance(1)->getToolPose();
02133 
02134         leftTip.setOrigin(btVector3(leftTip.getOrigin().x(), leftTip.getOrigin().y(), rightTip.getOrigin().z()));
02135 
02136         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(leftTip);
02137 
02138         rightTip = RobotArm::getInstance(0)->getToolPose();
02139         leftTip = RobotArm::getInstance(1)->getToolPose();
02140 
02141         double averageX = (rightTip.getOrigin().x() + leftTip.getOrigin().x()) * 0.5;
02142         double averageY = (rightTip.getOrigin().y() + leftTip.getOrigin().y()) * 0.5;
02143         double averageZ = (rightTip.getOrigin().z() + leftTip.getOrigin().z()) * 0.5;
02144 
02145         ROS_INFO("AVERAGE Y pre: %f", averageY);
02146 
02147         OperateHandleController::spinnerL(.45 - averageX,0 - averageY,.935 - averageZ);
02148 
02149     }
02150 
02151 
02153     if (atoi(argv[1]) == -35)
02154         DemoScripts::serveToTable();
02155 
02157     if (atoi(argv[1]) == -36)
02158     {
02159 
02160         /*Translation: [0.621, 1.421, 0.757]
02161         - Rotation: in Quaternion [-0.714, -0.010, 0.051, 0.698]
02162                  in RPY [-1.591, 0.059, 0.085]
02163         ruehr@satie:~/sandbox/tumros-internal/highlevel/ias_drawer_executive$ rosrun tf tf_echo map r_gripper_tool_frame
02164         At time 1286637310.022
02165         - Translation: [0.168, 1.391, 0.734]
02166         - Rotation: in Quaternion [-0.722, 0.028, 0.105, 0.684]
02167                  in RPY [-1.615, 0.190, 0.105]
02168 
02169                   Translation: [-0.031, 1.818, 0.051]
02170         - Rotation: in Quaternion [-0.003, -0.006, 0.031, 0.999]
02171                  in RPY [-0.006, -0.013, 0.062]
02172 
02173 
02174                  */
02175 
02176         {
02177 
02178 
02179             std::vector<int> arm;
02180             std::vector<tf::Stamped<tf::Pose> > goal;
02181             tf::Stamped<tf::Pose> result;
02182 
02183             tf::Stamped<tf::Pose> p0;
02184             p0.frame_id_="map";
02185             p0.stamp_=ros::Time();
02186             p0.setOrigin(btVector3(0.64, 1.421, 0.757));
02187             p0.setRotation(btQuaternion(-0.714, -0.010, 0.051, 0.698));
02188             goal.push_back(p0);
02189             arm.push_back(0);
02190 
02191             tf::Stamped<tf::Pose> p3;
02192             p3.frame_id_="map";
02193             p3.stamp_=ros::Time();
02194             p3.setOrigin(btVector3(0.55, 1.391, 0.734));
02195             p3.setRotation(btQuaternion(-0.714, -0.010, 0.051, 0.698));
02196             goal.push_back(p3);
02197             arm.push_back(0);
02198 
02199             tf::Stamped<tf::Pose> p1;
02200             p1.frame_id_="map";
02201             p1.stamp_=ros::Time();
02202             p1.setOrigin(btVector3(0.168, 1.391, 0.734));
02203             p1.setRotation(btQuaternion(-0.714, -0.010, 0.051, 0.698));
02204             goal.push_back(p1);
02205             arm.push_back(0);
02206 
02207             Gripper::getInstance(0)->open();
02208 
02209             RobotArm::findBaseMovement(result, arm, goal,true, false);
02210 
02211             RobotArm *a = RobotArm::getInstance(0);
02212             a->time_to_target = 2;
02213             a->universal_move_toolframe_ik_pose(p3);
02214             a->universal_move_toolframe_ik_pose(p0);
02215             Gripper::getInstance(0)->closeCompliant();
02216             Gripper::getInstance(0)->close();
02217             //a->universal_move_toolframe_ik_pose(p3);
02218             a->universal_move_toolframe_ik_pose(p1);
02219             Gripper::getInstance(0)->open();
02220 
02221             //RobotArm::findBaseMovement(result, arm, goal,false);
02222         }
02223 
02224 
02225     }
02226 
02228     if (atoi(argv[1]) == -37)
02229     {
02230         /*{
02231             std::vector<int> arm;
02232             std::vector<tf::Stamped<tf::Pose> > goal;
02233             btVector3 result;
02234 
02235             tf::Stamped<tf::Pose> p0;
02236             p0.frame_id_="map";
02237             p0.stamp_=ros::Time();
02238             p0.setOrigin(btVector3(0.64, 1.421, 0.757));
02239             p0.setRotation(btQuaternion(-0.714, -0.010, 0.051, 0.698));
02240             goal.push_back(p0);
02241             arm.push_back(0);
02242 
02243             tf::Stamped<tf::Pose> p3;
02244             p3.frame_id_="map";
02245             p3.stamp_=ros::Time();
02246             p3.setOrigin(btVector3(0.55, 1.391, 0.734));
02247             p3.setRotation(btQuaternion(-0.714, -0.010, 0.051, 0.698));
02248             goal.push_back(p3);
02249             arm.push_back(0);
02250 
02251             tf::Stamped<tf::Pose> p1;
02252             p1.frame_id_="map";
02253             p1.stamp_=ros::Time();
02254             p1.setOrigin(btVector3(0.168, 1.391, 0.734));
02255             p1.setRotation(btQuaternion(-0.714, -0.010, 0.051, 0.698));
02256             goal.push_back(p1);
02257             arm.push_back(0);
02258 
02259             Gripper::getInstance(0)->open();
02260 
02261             RobotArm::findBaseMovement(result, arm, goal,true, false);
02262 
02263             RobotArm *a = RobotArm::getInstance(0);
02264             a->time_to_target = 2;
02265             a->universal_move_toolframe_ik_pose(p3);
02266             a->universal_move_toolframe_ik_pose(p0);
02267         }*/
02268         //tf::Stamped<tf::Pose> p0;
02269         //p0.frame_id_="map";
02270         //p0.stamp_=ros::Time();
02271         //p0.setOrigin(btVector3(0.64, 1.421, 0.757));
02272         //p0.setRotation(btQuaternion(-0.714, -0.010, 0.051, 0.698));
02273         //p0 = RobotArm::getInstance(0)->getPoseIn("base_link",p0);
02274 
02275 
02276         tf::Stamped<tf::Pose> p0 = RobotArm::getInstance(0)->getToolPose("base_link");
02277 
02278         OperateHandleController::operateHandle(0,p0);
02279         //     OperateHandleController::close(handle);
02280         //Translation: [0.828, 1.973, 1.313]
02281 //- Rotation: in Quaternion [-0.701, -0.031, 0.039, 0.711]
02282         //          in RPY [-1.556, 0.012, 0.099]
02283 
02284 
02285     }
02286 
02287 
02288 
02289     if (atoi(argv[1]) == -38)
02290     {
02291         ros::Rate rate(5);
02292         while (ros::ok())
02293         {
02294             ROS_INFO("GRIPPER OPENING R%f L%f", Gripper::getInstance(0)->getAmountOpen(), Gripper::getInstance(1)->getAmountOpen());
02295             rate.sleep();
02296         }
02297     }
02298 
02299 
02301     if (atoi(argv[1]) == -39)
02302         DemoScripts::takeSilverware();
02303 
02305     if (atoi(argv[1]) == -40)
02306         DemoScripts::openDrawer();
02307 
02309     if (atoi(argv[1]) == -41)
02310         DemoScripts::takePlateFromIsland();
02311 
02312     if (atoi(argv[1]) == -42)
02313         OperateHandleController::plateTuckPose();
02314 
02315 
02316     if (atoi(argv[1]) == -43)
02317         OperateHandleController::plateAttackPose();
02318 
02319     // open fridge with laser handle
02320     if (atoi(argv[1]) == -44)
02321         OperateHandleController::plateCarryPose();
02322 
02323     //DemoScripts::openFridge();
02324 
02325 
02326     // runs the whole demo once
02327     if (atoi(argv[1]) == -100)
02328     {
02329 
02330         int idx = 0;
02331         if (argc > 1)
02332             idx = atoi(argv[2]);
02333 
02334         ROS_INFO("INDEX TO DEMO :%i", idx);
02335 
02336         //int handle = 0;
02337 
02338         switch (idx)
02339         {
02340 
02341         case 0:
02342 
02343             //handle = DemoScripts::openFridge();
02344 
02345         case 1:
02346 
02347             //DemoScripts::takeBottle();
02348 
02349         case 2:
02350 
02351             //DemoScripts::closeFridge(handle);
02352             DemoScripts::takeBottleFromFridge();
02353 
02354         case 3:
02355 
02356             DemoScripts::serveBottle();
02357 
02358         case 4:
02359 
02360             DemoScripts::openDrawer();
02361 
02362         case 5:
02363 
02364             DemoScripts::takePlate();
02365 
02366         case 6:
02367 
02368             DemoScripts::servePlateToIsland();
02369 
02370         case 7:
02371 
02372             DemoScripts::takeSilverware();
02373 
02374         case 8:
02375 
02376             DemoScripts::serveToTable();
02377 
02378         case 9:
02379 
02380             DemoScripts::takePlateFromIsland();
02381 
02382         case 10:
02383             //2nd time :)
02384             DemoScripts::serveToTable();
02385         case 11:
02386             //2nd time :)
02387             DemoScripts::putObjectIntoFridge();
02388         }
02389     }
02390 
02391     if (atoi(argv[1]) == -101)
02392     {
02393         //OperateHandleController::getPlate(0);
02394 
02395         RobotDriver::getInstance()->moveBaseP(-0.571, 2.819,0.729, 0.685);
02396 
02397         OperateHandleController::pickPlate(btVector3(-0.600, 3.453, 0.87));
02398 
02399 
02400         //RobotArm::getInstance(0)->stabilize_grip();
02401         //RobotArm::getInstance(1)->stabilize_grip();
02402         //OperateHandleController::plateCarryPose();
02403     }
02404 
02405 
02406     if (atoi(argv[1]) == -102)
02407     {
02408         RobotArm::getInstance(atoi(argv[2]))->stabilize_grip();
02409     }
02410 
02411     if (atoi(argv[1]) == -103)
02412     {
02413 
02414         ros::Rate rate(5.0);
02415 
02416         while (ros::ok())
02417         {
02418             btVector3 diff = RobotArm::getInstance(0)->cartError();
02419             ROS_INFO("DIFF %f %f %f DISTANCE %f", diff.x(), diff.y(), diff.y(), diff.length());
02420             ros::spinOnce();
02421             rate.sleep();
02422         }
02423     }
02424 
02425     // primitive and dumb approach of lifting elbow keeping endeffector in place
02426     if (atoi(argv[1]) == -104)
02427     {
02428 
02429         ros::Rate rate(5.0);
02430 
02431         double jiggle = 0;
02432 
02433         RobotArm *arm = RobotArm::getInstance(0);
02434 
02435         double state[7];
02436         double goalstate[7];
02437 
02438         double primitive[100][7];
02439         int numinprim = 0;
02440 
02441         double mp[100][7];
02442         for (int k = 0; k < 100; k ++)
02443         {
02444             for (int j = 0; j < 7; j++)
02445             {
02446                 mp[k][j] = 0;
02447                 primitive[k][j] = 0;
02448             }
02449         }
02450         mp[0][0] = 1;
02451         mp[1][1] = 1;
02452         mp[2][2] = 1;
02453         mp[3][3] = 1;
02454         mp[4][4] = 1;
02455         mp[5][5] = 1;
02456         mp[6][6] = 1;
02457         //int pcnt = 8;
02458 
02459         arm->getJointState(state);
02460         arm->getJointState(goalstate);
02461 
02462         tf::Stamped<tf::Pose> goalP = arm->runFK(state);
02463 
02464         while (ros::ok())
02465         {
02466 
02467             jiggle += -.01;
02468 
02469             double add[] = {0,jiggle,0,0,0,0,0};
02470 
02471             double curr[7];
02472 
02473             addState(state,add,curr);
02474 
02475             copyState(curr,state);
02476 
02477             double initialincrement = 0.075;
02478             //initialincrement = 0.01;
02479 
02480             double increment = initialincrement;
02481 
02482             tf::Stamped<tf::Pose> newp = arm->runFK(curr);
02483 
02484             double oldDist= (goalP.getOrigin() - newp.getOrigin()).length();
02485             double oldDistAngle = (goalP.getRotation().inverse() * newp.getRotation()).getAngle();
02486 
02487             double flzeroes[] = {0,1,0,0,0,0,0,0,0,0,0};
02488             int numflippedzero = 1;
02489             double flipped[7];
02490             copyState(flzeroes,flipped,9);
02491             int numflipped = numflippedzero;
02492 
02493             int metric = 0; // o = distance, 1 = angle
02494 
02495             bool found = false;
02496 
02497             while ((!found) && (ros::ok()))
02498             {
02499                 if (numflipped == 7)
02500                 {
02501                     increment /= 2.0f;
02502                     numflipped =numflippedzero;
02503                     copyState(flzeroes,flipped,9);
02504                     ROS_INFO("DECREMENTING INCREMENT to %f", increment);
02505                 }
02506 
02507                 int jnt = rand() % 7;
02508                 while (flipped[jnt] == 1)
02509                     jnt = rand() % 7;
02510 
02511                 //if (numflipped == numflippedzero)
02512                 //jnt = 7 + metric;
02513 
02514                 flipped[jnt] = 1;
02515                 numflipped += 1;
02516 
02517                 //double act_add[] = {0,0,0,0,0,0,0};
02518                 //act_add[jnt] = increment;
02519                 double currm[7];
02520                 double currp[7];
02521 
02522                 //addState(state,act_add,curr);
02523                 copyState(state,currm);
02524                 copyState(state,currp);
02525 
02526                 double distance = 0;
02527                 for (int k = 0; k < 7; k++)
02528                     distance += mp[jnt][k] * mp[jnt][k];
02529                 distance = sqrtf(distance);
02530 
02531                 double actual_increment = increment / distance;
02532 
02533                 for (int k = 0; k < 7; k++)
02534                 {
02535                     currm[k] -= mp[jnt][k] * actual_increment;
02536                     currp[k] += mp[jnt][k] * actual_increment;
02537                 }
02538 
02539                 //tf::Stamped<tf::Pose> oldp = arm->runFK(state);
02540                 tf::Stamped<tf::Pose> newp = arm->runFK(currp);
02541                 double distp = (goalP.getOrigin() - newp.getOrigin()).length();
02542                 tf::Stamped<tf::Pose> newm = arm->runFK(currm);
02543                 double distm = (goalP.getOrigin() - newm.getOrigin()).length();
02544 
02545                 double distpangle = (goalP.getRotation().inverse() * newp.getRotation()).getAngle();
02546                 double distmangle = (goalP.getRotation().inverse() * newm.getRotation()).getAngle();
02547 
02548                 //if (oldDist < 0.01)
02549                 //metric = false;
02550                 //else
02551                 //  metric = true;
02552                 bool madeMove = false;
02553                 double effect = 0;
02554                 if (metric == 0)
02555                 {
02556                     if (distp < oldDist)
02557                     {
02558                         //state[jnt] += increment;
02559                         //change[jnt] += increment;
02560                         effect = actual_increment;
02561                         oldDist = distp;
02562                         oldDistAngle = distpangle;
02563                         madeMove = true;
02564                     }
02565                     else if (distm < oldDist)
02566                     {
02567                         //state[jnt] -= increment;
02568                         //change[jnt] -= increment;
02569                         effect = -actual_increment;
02570                         oldDist = distm;
02571                         oldDistAngle = distmangle;
02572                         madeMove = true;
02573                     }
02574                 }
02575                 else
02576                 {
02577                     if (distpangle < oldDistAngle)
02578                     {
02579                         //state[jnt] += increment;
02580                         //change[jnt] += increment;
02581                         effect = actual_increment;
02582                         oldDist = distp;
02583                         oldDistAngle = distpangle;
02584                         madeMove = true;
02585                     }
02586                     else if (distmangle < oldDistAngle)
02587                     {
02588                         //state[jnt] -= increment;
02589                         //change[jnt] -= increment;
02590                         effect = -actual_increment;
02591                         oldDist = distm;
02592                         oldDistAngle = distmangle;
02593                         madeMove = true;
02594                     }
02595                 }
02596 
02597 
02598                 if (madeMove)
02599                 {
02600                     addStateDiscounted(state, mp[jnt], effect, state);
02601                     ROS_ERROR("%i Sucessful increment %f Resetting INCREMENT %f , METRIC : %i", jnt ,effect, initialincrement, metric);
02602                     increment = increment * 2;
02603                     if (increment > initialincrement)
02604                         increment = initialincrement;
02605                     numflipped = numflippedzero;
02606                     copyState(flzeroes,flipped,9);
02607                     //metric = !metric;
02608                     addStateDiscounted(primitive[metric],mp[jnt],effect,primitive[metric]);
02609                     for (int j = 0; j < 7; j ++)
02610                     {
02611                         mp[7][j] = primitive[0][j];
02612                         mp[8][j] = primitive[1][j];
02613                     }
02614 
02615                     numinprim ++;
02616                     ROS_ERROR("Motion primitive D: %f %f %f %f %f %f %f", primitive[0][0], primitive[0][1],primitive[0][2],primitive[0][3],primitive[0][4],primitive[0][5],primitive[0][6]);
02617                     ROS_ERROR("Motion primitive A: %f %f %f %f %f %f %f", primitive[1][0], primitive[1][1],primitive[1][2],primitive[1][3],primitive[1][4],primitive[1][5],primitive[1][6]);
02618                 }
02619 
02620 //                ROS_INFO("%i STATE: %f %f %f %f %f %f %f, D :old  %f plus %f minus %f ANG %f %f %f",jnt, state[0],state[1],state[2],state[3],state[4],state[5],state[6], oldDist,distp,distm,
02621                 //                       oldDistAngle, distpangle, distmangle);
02622                 ROS_INFO("%s %i d %f a %f STATE: %f %f %f %f %f %f %f", (metric==0) ? "dist" : "angl", jnt, oldDist, oldDistAngle, state[0],state[1],state[2],state[3],state[4],state[5],state[6]);
02623 
02624                 if ((oldDist < 0.01) && (metric == 0))
02625                     metric = 1;
02626                 if ((oldDistAngle < .05) && (metric == 1))
02627                     metric = 0;
02628                 //metric = !metric;
02629 
02630                 if ((oldDist < 0.01) && (oldDistAngle < 0.05))
02631                 {
02632                     double poseA[7];
02633                     double poseB[7];
02634 
02635                     ROS_INFO(" ");
02636                     ROS_INFO(" ");
02637                     ROS_INFO(" ");
02638                     ROS_INFO(" ");
02639                     ROS_ERROR("MOVING ARM NOW");
02640                     ROS_INFO(" ");
02641                     ROS_INFO(" ");
02642                     ROS_INFO(" ");
02643                     ROS_INFO(" ");
02644                     ROS_INFO("OLD STATE %f %f %f %f %f %f %f", goalstate[0],goalstate[1],goalstate[2],goalstate[3],goalstate[4],goalstate[5],goalstate[6]);
02645                     ROS_INFO("NEW STATE %f %f %f %f %f %f %f", state[0],state[1],state[2],state[3],state[4],state[5],state[6]);
02646 
02647                     double stateDiff[] = {0,0,0,0,0,0,0};
02648                     addStateDiscounted(goalstate,state,-1,stateDiff);
02649                     ROS_INFO("DIFF STATE %f %f %f %f %f %f %f", stateDiff[0],stateDiff[1],stateDiff[2],stateDiff[3],stateDiff[4],stateDiff[5],stateDiff[6]);
02650 
02651 
02652                     for (int k = 0; k < 7; ++k)
02653                     {
02654                         poseA[k] = goalstate[k];
02655                         poseB[k] = state[k];
02656                         goalstate[k] = state[k];
02657                     }
02658                     arm->startTrajectory(arm->twoPointTrajectory(poseA, poseB));
02659                     found = true;
02660                 }
02661 
02662             }
02663         }
02664     }
02665 
02666 
02667     if (atoi(argv[1]) == -105)
02668     {
02669 
02670         RobotArm *arm = RobotArm::getInstance(0);
02671         double amount =0;
02672 
02673         double start[] = {-0.896923, 0.513388, -1.284203, -1.535167, 4.452332, -0.837711, -9.765933};
02674         double d[] = {0.037500, 0.250000, 0.337500, 0.000000, -0.300000, -0.300000, 0.000000};
02675         double next[7] = {-0.896923, 0.513388, -1.284203, -1.535167, 4.452332, -0.837711, -9.765933};
02676         double act[7];
02677 
02678 
02679         while (ros::ok())
02680         {
02681             amount += 0.1;
02682 
02683             for (int k = 0; k < 7; ++k)
02684             {
02685                 act[k] = next[k];
02686                 next[k] = start[k] + (- d[k] * amount);
02687             }
02688             ROS_INFO("amount %f" , amount);
02689             arm->startTrajectory(arm->twoPointTrajectory(start, next));
02690 
02691         }
02692 
02693     }
02694 
02695     if (atoi(argv[1]) == -106)
02696     {
02697 
02698         int side = atoi(argv[2]);
02699         RobotArm *arm = RobotArm::getInstance(side);
02700         double state[7];
02701         arm->getJointState(state);
02702         double solution[7];
02703 
02704         tf::Stamped<tf::Pose> actPose;
02705         arm->getWristPose(actPose,"base_link");
02706 
02707         geometry_msgs::PoseStamped stamped_pose;
02708         stamped_pose.header.frame_id = "base_link";
02709         stamped_pose.header.stamp = ros::Time::now();
02710         stamped_pose.pose.position.x=actPose.getOrigin().x();
02711         stamped_pose.pose.position.y=actPose.getOrigin().y();
02712         stamped_pose.pose.position.z=actPose.getOrigin().z();
02713         stamped_pose.pose.orientation.x=actPose.getRotation().x();
02714         stamped_pose.pose.orientation.y=actPose.getRotation().y();
02715         stamped_pose.pose.orientation.z=actPose.getRotation().z();
02716         stamped_pose.pose.orientation.w=actPose.getRotation().w();
02717 
02718         double increment = -0.25;
02719 
02720         double last = 0;
02721         int numup = 0;
02722         ros::Rate rate(2.0);
02723 
02724         double lastState[7];
02725         double dif[7];
02726         int numit = 0;
02727 
02728         for (double add = 0; ros::ok(); add+= 0.01)
02729         {
02730             rate.sleep();
02731             state[2] += increment;
02732             //double new_state[7];
02733             //bool f =
02734             arm->run_ik(stamped_pose,state,solution,side == 0 ? "r_wrist_roll_link" : "l_wrist_roll_link");
02735             ROS_INFO("inc %f SEED STATE %f %f %f %f %f %f %f",increment, state[0],state[1],state[2],state[3],state[4],state[5],state[6]);
02736             ROS_INFO("%f SOLUTION %f %f %f %f %f %f %f", state[1], solution[0], solution[1],solution[2], solution[3], solution[4], solution[5], solution[6]);
02737             double pose[7];
02738             numit++;
02739 
02740             //if (numit < 4)
02741 
02742             double sum = 0;
02743             for (int k = 0; k < 7; ++k)
02744             {
02745                 pose[k] = solution[k];
02746                 sum += pose[k] * pose[k];
02747             }
02748 
02749             if (sum > 0.01)
02750                 arm->startTrajectory(arm->goalTraj(pose),false);
02751 
02753             //else {
02754             //m->startTrajectory(arm->goalTraj(pose, dif));
02755             //ROS_INFO("VELOCITY");
02756             //}
02757 
02758             if ((last == solution[1]) && (numup > 3))
02759             {
02760                 increment=-increment;
02761                 numup = -4;
02762             }
02763 
02764             ROS_ERROR("DIFFERENCE %f %f %f %f %f %f %f",lastState[0] - solution[0], lastState[1] - solution[1],lastState[2] - solution[2], lastState[3] - solution[3], lastState[4] - solution[4], lastState[5] - solution[5], lastState[6] - solution[6]);
02765 
02766             for (int k = 0; k < 7 ; ++k)
02767             {
02768                 dif[k] = (-(lastState[k] - solution[k])) / 1.0;
02769                 lastState[k] = solution[k];
02770                 numit ++;
02771             }
02772 
02773             last = solution[1];
02774 
02775             numup++;
02776             //pr2_controllers_msgs::JointTrajectoryGoal goalTraj(double *poseA);
02777         }
02778 
02779 
02780         //bool RobotArm::run_ik(geometry_msgs::PoseStamped pose, double start_angles[7],double solution[7], std::string link_name)
02781 
02782     }
02783 
02784 
02785 
02786     if (atoi(argv[1]) == -107)
02787     {
02788 
02789         int side = 1;
02790         RobotArm *arm = RobotArm::getInstance(side);
02791         double state[7];
02792         arm->getJointState(state);
02793         //double solution[7];
02794 
02795         tf::Stamped<tf::Pose> actPose;
02796         arm->getWristPose(actPose,"base_link");
02797 
02798         geometry_msgs::PoseStamped stamped_pose;
02799         stamped_pose.header.frame_id = "base_link";
02800         stamped_pose.header.stamp = ros::Time::now();
02801         stamped_pose.pose.position.x=actPose.getOrigin().x();
02802         stamped_pose.pose.position.y=actPose.getOrigin().y();
02803         stamped_pose.pose.position.z=actPose.getOrigin().z();
02804         stamped_pose.pose.orientation.x=actPose.getRotation().x();
02805         stamped_pose.pose.orientation.y=actPose.getRotation().y();
02806         stamped_pose.pose.orientation.z=actPose.getRotation().z();
02807         stamped_pose.pose.orientation.w=actPose.getRotation().w();
02808 
02809 
02810         //double last = 0;
02811         //int numup = 0;
02812         ros::Rate rate(50.0);
02813 
02814         //double lastState[7];
02815         //double dif[7];
02816         //int numit = 0;
02817 
02818         //for (double add = 0; ros::ok(); add+= 0.01)
02819         while (ros::ok())
02820         {
02821             rate.sleep();
02822             ros::spinOnce();
02823 
02824             double jErr[7];
02825             double jDes[7];
02826 
02827             arm->getJointStateErr(jErr);
02828             arm->getJointStateDes(jDes);
02829 
02830             double increment = .1;
02831 
02832             //if (fabs(jErr[1]) > 0.01) {
02833             {
02834 
02835                 double stateP[7];
02836                 double statePK[7];
02837                 double stateM[7];
02838                 //double solStateP[7];
02839                 //double solStatePK[7];
02840                 //double solStateM[7];
02841 
02842                 arm->getJointState(stateP);
02843                 arm->getJointState(statePK);
02844                 arm->getJointState(stateM);
02845 
02846                 double stA[7];
02847                 double stB[7];
02848                 double stC[7];
02849                 arm->getJointState(stA);
02850                 arm->getJointState(stB);
02851                 arm->getJointState(stC);
02852 
02853                 stB[2] += increment;
02854 
02855                 double stAs[7];
02856                 double stBs[7];
02857                 double stCs[7];
02858 
02859                 arm->run_ik(stamped_pose,stA,stAs,side == 0 ? "r_wrist_roll_link" : "l_wrist_roll_link");
02860                 arm->run_ik(stamped_pose,stB,stBs,side == 0 ? "r_wrist_roll_link" : "l_wrist_roll_link");
02861 
02862                 double newinc = (jErr[1] / (stBs[1] - stAs[1])) * increment;
02863 
02864                 stC[2] += newinc;
02865 
02866                 arm->run_ik(stamped_pose,stC,stCs,side == 0 ? "r_wrist_roll_link" : "l_wrist_roll_link");
02867 
02868                 ROS_INFO("curr (sta[1]) %f  stCs %f", stA[1], stCs[1]);
02869 
02870                 double pose[7];
02871                 double sum = 0;
02872                 for (int k = 0; k < 7; ++k)
02873                 {
02874                     pose[k] = stCs[k];
02875                     sum += stCs[k] * stCs[k];
02876                 }
02877 
02878                 if (sum > 0.01)
02879                     arm->startTrajectory(arm->goalTraj(pose));
02880 
02881                 if (stBs[2]==0)
02882                 {
02883                     increment = -increment;
02884                     ROS_INFO("HIT LIMIT?");
02885                 }
02886 
02887 
02888 
02889             }
02890 
02891 
02892         }
02893 
02894     }
02895 
02896 
02898     if (atoi(argv[1]) == -108)
02899     {
02900         ROS_INFO("OPEN HANDLE AT MAP COORDINATE  arm orientation(0=vert 1=horiz) x_map y_map z_map");
02901 
02902         //tf::Stamped<tf::Pose> p0 = RobotArm::getInstance(0)->getToolPose("base_link");
02903         tf::Stamped<tf::Pose> p0;
02904         p0.setOrigin(btVector3(atof(argv[4]),atof(argv[5]),atof(argv[6])));
02905         p0.frame_id_ = "map";
02906         p0 = Geometry::getPoseIn("base_link",p0);
02907 
02908         if (atoi(argv[3]) == 0)
02909             p0.setRotation(btQuaternion(1,0,0,0));
02910         else
02911             p0.setRotation(btQuaternion(0.707,0,0,0.707));
02912 
02913         OperateHandleController::operateHandle(atoi(argv[2]),p0);
02914     }
02915 
02916 
02918     if (atoi(argv[1]) == -109)
02919     {
02920         //OperateHandleController::plateTuckPose();
02921         OperateHandleController::plateAttackPose();
02922 
02923         //bin/ias_drawer_executive -109 .482 1.073 0.761
02924 
02925         //OperateHandleController::pickPlate(btVector3(atof(argv[2]),atof(argv[3]),atof(argv[4])), .28);
02926         OperateHandleController::pickPlate(btVector3(atof(argv[2]),atof(argv[3]),atof(argv[4])), .27);
02927 
02928         OperateHandleController::spinnerL(0,0,-.2);
02929 
02930         OperateHandleController::openGrippers();
02931 
02932     }
02933 
02934 
02935 
02936 
02937     if (atoi(argv[1]) == -110)
02938     {
02939 
02940         //boost::thread t2(&cart_err_monit, 0);
02941 
02942         int side = 0;
02943         RobotArm::RobotArm *arm = RobotArm::getInstance(side);
02944 
02945         double pts[][7] =
02946         {
02947             {0.478704, -1.0355, 1.18101, 0.767433, 0.639987, 0.022135, 0.0311955},
02948             {0.489086, -0.984206, 1.17956, 0.797904, 0.601535, 0.01726, 0.0347398},
02949             {0.494529, -0.937741, 1.1803, 0.830545, 0.555891, 0.0110758, 0.0325103},
02950             {0.504333, -0.909376, 1.18066, 0.849808, 0.526105, 0.00709967, 0.03147},
02951             {0.507886, -0.884252, 1.17954, 0.8814, 0.471274, 0.00699274, 0.0313926},
02952             {0.516993, -0.854729, 1.18006, 0.903026, 0.428457, 0.00859376, 0.0299171},
02953             {0.527833, -0.832331, 1.1803, 0.920176, 0.390256, 0.0125722, 0.0286066},
02954             {0.541463, -0.80644, 1.18, 0.931353, 0.362808, 0.0186723, 0.0245782},
02955             {0.571712, -0.760535, 1.17887, 0.936451, 0.349496, 0.024334, 0.017896},
02956             {0.608236, -0.715618, 1.17839, 0.944274, 0.327791, 0.0273483, 0.0123364},
02957             {0.647457, -0.676296, 1.17812, 0.954053, 0.298037, 0.0302956, 0.00623379},
02958             {0.690692, -0.638766, 1.17999, 0.964469, 0.262043, 0.0336022, 0.00195834},
02959             {0.734141, -0.609302, 1.18042, 0.974717, 0.220844, 0.0339708, -0.00102721},
02960             {0.781735, -0.583995, 1.17916, 0.983083, 0.180164, 0.0327274, -0.00426907},
02961             {0.828575, -0.564397, 1.17937, 0.990023, 0.137179, 0.0315954, -0.00617472},
02962             {0.870116, -0.550422, 1.17831, 0.995336, 0.0920069, 0.0283872, -0.00586025},
02963             {0.921693, -0.544899, 1.17853, 0.998734, 0.0415909, 0.0273629, -0.00714236},
02964             {0.971471, -0.549669, 1.17854, 0.998732, 0.0416648, 0.0273237, -0.00716123}
02965         };
02966 
02967 
02968         //for (double z= 1.3; z <= 1.4; z +=0.025)
02969         double z = 1.35;
02970         {
02971             int numf = 0;
02972             bool found = true;
02973 
02974             double stA[7];
02975             arm->getJointState(stA);
02976 
02977             RobotHead::getInstance()->lookAt("/map", 1.243111, -0.728864, 0.7);
02978 
02979             for (int k = 0; k <= 16; ++k)
02980             {
02981                 tf::Stamped<tf::Pose> act;
02982                 act.frame_id_ = "map";
02983                 act.setOrigin(btVector3(pts[k][0],pts[k][1],z));
02984                 act.setRotation(btQuaternion(pts[k][3],pts[k][4],pts[k][5],pts[k][6]));
02985                 act = arm->tool2wrist(act);
02986 
02987                 geometry_msgs::PoseStamped stamped_pose;
02988                 stamped_pose.header.frame_id = "map";
02989                 stamped_pose.header.stamp = ros::Time::now();
02990                 stamped_pose.pose.position.x=act.getOrigin().x();
02991                 stamped_pose.pose.position.y=act.getOrigin().y();
02992                 stamped_pose.pose.position.z=z;
02993                 stamped_pose.pose.orientation.x=pts[k][3];
02994                 stamped_pose.pose.orientation.y=pts[k][4];
02995                 stamped_pose.pose.orientation.z=pts[k][5];
02996                 stamped_pose.pose.orientation.w=pts[k][6];
02997 
02998                 double stAs[7];
02999                 int ret = arm->run_ik(stamped_pose,stA,stAs,side == 0 ? "r_wrist_roll_link" : "l_wrist_roll_link");
03000                 ROS_INFO("       z %f k %i %s", z, k, ret ? "found." : "not found.");
03001                 numf += ret;
03002                 if (ret==0)
03003                     found = false;
03004                 else
03005                 {
03006                     for (int i = 0; i < 7; ++i)
03007                         stA[i] = stAs[i];
03008                 }
03009             }
03010             ROS_INFO("FOR HEIGHT z %f %s num %i", z, found ? "found." : "not found.", numf);
03011             if (found)
03012             {
03013 
03014                 double stA[7];
03015                 arm->getJointState(stA);
03016                 ros::Rate rate(3.0);
03017                 int idx[] = {16,15,14,13,12,11,10,9,8,7,6,5,4,3,2,1,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,16,16};
03018                 for (int o = 0; o <= 16+16; ++o)
03019                     //for (int k = 16; k >= 0; --k)
03020                 {
03021                     int k = idx[o];
03022                     tf::Stamped<tf::Pose> act;
03023                     act.frame_id_ = "map";
03024                     act.setOrigin(btVector3(pts[k][0],pts[k][1],z));
03025                     act.setRotation(btQuaternion(pts[k][3],pts[k][4],pts[k][5],pts[k][6]));
03026                     act = arm->tool2wrist(act);
03027 
03028                     geometry_msgs::PoseStamped stamped_pose;
03029                     stamped_pose.header.frame_id = "map";
03030                     stamped_pose.header.stamp = ros::Time::now();
03031                     stamped_pose.pose.position.x=act.getOrigin().x();
03032                     stamped_pose.pose.position.y=act.getOrigin().y();
03033                     stamped_pose.pose.position.z=z;
03034                     stamped_pose.pose.orientation.x=pts[k][3];
03035                     stamped_pose.pose.orientation.y=pts[k][4];
03036                     stamped_pose.pose.orientation.z=pts[k][5];
03037                     stamped_pose.pose.orientation.w=pts[k][6];
03038 
03039                     double stAs[7];
03040                     int ret = arm->run_ik(stamped_pose,stA,stAs,side == 0 ? "r_wrist_roll_link" : "l_wrist_roll_link");
03041 
03042                     //ROS_INFO("       z %f k %i %s", z, k, ret ? "found." : "not found.");
03043                     ROS_INFO("k : %i",k);
03044 
03045                     rate.sleep();
03046                     if (ret)
03047                     {
03048                         double pose[7];
03049                         double sum = 0;
03050                         for (int k = 0; k < 7; ++k)
03051                         {
03052                             pose[k] = stAs[k];
03053                             sum += stAs[k] * stAs[k];
03054                         }
03055                         if (sum > 0.01)
03056                             arm->startTrajectory(arm->goalTraj(pose),false);
03057                     }
03058                     if (k == 0)
03059                     {
03060 
03061                         RobotHead::getInstance()->lookAt("/map", 1.243111, -0.728864, 0.9);
03062                         tf::Stamped<tf::Pose> bottle = Perception3d::getBottlePose();
03063 
03064                         double ptA[] = {0.41491862845470812, 1.3468554401788568, 1.501748997727044, -2.0247783614692936, -16.507431415382143, -1.3292235155277217, 15.027356561279952};
03065                         double ptB[] = {0.040263624618489424, 0.96465557759293075, 0.27150676981727662, -1.6130504582945409, -14.582800985450046, -1.1869058378819473, 14.819427432123987};
03066                         RobotArm *arml = RobotArm::getInstance(1);
03067                         arml->startTrajectory(arml->goalTraj(ptA,1.5));
03068                         arml->startTrajectory(arml->goalTraj(ptB,1.5));
03069                         Gripper::getInstance(1)->open();
03070                         RobotArm::getInstance(1)->universal_move_toolframe_ik(bottle.getOrigin().x() - .1, bottle.getOrigin().y(), bottle.getOrigin().z() + .03, 0.005, -0.053, -0.029005, 0.998160, "map");
03071                         RobotArm::getInstance(1)->universal_move_toolframe_ik(bottle.getOrigin().x(), bottle.getOrigin().y(), bottle.getOrigin().z() + .03, 0.005, -0.053, -0.029005, 0.998160, "map");
03072 
03073                         Gripper::getInstance(1)->closeCompliant();
03074                         Gripper::getInstance(1)->close(0.5);
03075                         RobotArm::getInstance(1)->universal_move_toolframe_ik(bottle.getOrigin().x() - .1, bottle.getOrigin().y(), bottle.getOrigin().z() + .03, 0.005, -0.053, -0.029005, 0.998160, "map");
03076                         arml->startTrajectory(arml->goalTraj(ptB,1.5));
03077                         RobotArm::getInstance(1)->universal_move_toolframe_ik(0.208, 0.120, 0.785 , -0.020, -0.101, -0.662, 0.742, "base_link");
03078                     }
03079                 }
03080             }
03081         }
03082 
03083         Gripper::getInstance(0)->open();
03084 
03085         double target[4];
03086         target[0] = -0.3;
03087         target[1] = 0;
03088         target[2] = 0;
03089         target[3] = 1;
03090         ROS_INFO("POSE IN BASE %f %f %f", target[0],target[1], target[2]);
03091         RobotDriver::getInstance()->driveInOdom(target, 1);
03092 
03093         pr2_controllers_msgs::JointTrajectoryGoal goalAT = RobotArm::getInstance(0)->twoPointTrajectory(Poses::prepDishRT,Poses::prepDishRT);
03094         boost::thread t2T(&RobotArm::startTrajectory, RobotArm::getInstance(0), goalAT,true);
03095         t2T.join();
03096 
03097         //RobotArm::getInstance(1)->universal_move_toolframe_ik(0.308, 0.120, 0.91 , -0.020, -0.101, -0.662, 0.742, "base_link");
03098 
03099         //[1.1811415860679706, 0.40573692064179873, 1.772267589813042, -2.0337541455750725, 0.15224039094452749, -0.61844366806403794, 5.0541238004106495]
03100 
03101 
03102     }
03103 
03104 
03105 
03106     if (atoi(argv[1]) == -111)
03107     {
03108 
03109         RobotArm::RobotArm *arm = RobotArm::getInstance(0);
03110         Torso *torso = Torso::getInstance();
03111         Gripper *gripper = Gripper::getInstance(0);
03112 
03113         boost::thread t2(&Torso::up, torso);
03114 
03115         OperateHandleController::plateTuckPose();
03116 
03117         RobotDriver::getInstance()->moveBaseP(0.305, -0.515, -0.348, 0.938, false);
03118         t2.join();
03119 
03120         tf::Stamped<tf::Pose> handleHint;
03121         handleHint.setOrigin(btVector3( 0.919, -0.553, 1.35 ));
03122         handleHint.frame_id_ = "/map";
03123         tf::Stamped<tf::Pose> handlePos = Perception3d::getHandlePoseFromLaser(handleHint);
03124 
03125         //yes TWO Times
03126         handlePos = Perception3d::getHandlePoseFromLaser(handleHint);
03127 
03128 
03129         handlePos = Geometry::getPoseIn("map",handlePos);
03130 
03131         ROS_INFO("Handle in Map");
03132         arm->printPose(handlePos);
03133 
03134         handlePos.setRotation(btQuaternion( 0.998732, 0.0416648, 0.0273237, -0.00716123));
03135 
03136         tf::Stamped<tf::Pose> handlePosApp = handlePos;
03137         tf::Stamped<tf::Pose> handlePosAppB = handlePos;
03138         handlePosApp.setOrigin(handlePosApp.getOrigin() + btVector3(-.05,0,1.35 - handlePos.getOrigin().z() ));
03139         handlePosAppB.setOrigin(handlePosAppB.getOrigin() + btVector3(.05,0,1.35 - handlePos.getOrigin().z() ));
03140         tf::Stamped<tf::Pose> handlePosAppM = handlePos;
03141         handlePosAppM.setOrigin(handlePosAppM.getOrigin() + btVector3(-.10,0,1.35 - handlePos.getOrigin().z() ));
03142 
03143 
03144         arm->time_to_target = 2;
03145         arm->move_toolframe_ik_pose(handlePosAppM);
03146         arm->time_to_target = 1;
03147 
03148         Approach *apr = new Approach();
03149         apr->init(0,handlePosApp, handlePosAppB, Approach::front);
03150 
03151         gripper->close();
03152 
03153         double distA = (apr->increment(0,0.5));
03154         if (distA == 0)
03155         {
03156             ROS_ERROR("DIDNT TOUCH IN THE FIRST 5 CM OF APPROACH");
03157             distA = (apr->increment(0.5,1));
03158         }
03159         //back up 5 centimeter
03160         apr->move_to((distA - .5));
03161         gripper->open();
03162         //go 2.75 cm forward from touch position
03163         apr->move_to((distA + .375));
03164 
03165         gripper->closeCompliant();
03166 
03167         gripper->close();
03168 
03169         arm->stabilize_grip();
03170 
03171     }
03172 
03173 
03174 
03175     if (atoi(argv[1]) == -112)
03176     {
03177 
03178         OperateHandleController::plateTuckPose();
03179 
03180         RobotDriver::getInstance()->moveBaseP(0.305, -0.515, -0.348, 0.938, false);
03181         {
03182             std::vector<int> arm;
03183             std::vector<tf::Stamped<tf::Pose> > goal;
03184             tf::Stamped<tf::Pose> result;
03185 
03186             tf::Stamped<tf::Pose> p0;
03187             p0.frame_id_="map";
03188             p0.stamp_=ros::Time();
03189             p0.setOrigin(btVector3(0.8, -0.455, 1.251));
03190             p0.setRotation(btQuaternion(0.005, -0.053, -0.029, 0.998));
03191             //goal.push_back(p0);
03192             //arm.push_back(1);
03193 
03194             tf::Stamped<tf::Pose> p1;
03195             p1.frame_id_="map";
03196             p1.stamp_=ros::Time();
03197             p1.setOrigin(btVector3(0.8, -0.655, 1.251));
03198             p1.setRotation(btQuaternion(0.005, -0.053, -0.029, 0.998));
03199             //goal.push_back(p1);
03200             //arm.push_back(1);
03201 
03202             btVector3 bottle(1.227, -0.741, 0.986);
03203 
03204             tf::Stamped<tf::Pose> p2;
03205             p2.frame_id_="map";
03206             p2.stamp_=ros::Time();
03207             //p2.setOrigin(btVector3(1.165 - fridgeLink, -0.655, 1.151));
03208             p2.setOrigin(bottle + btVector3(-.1,0,.03));
03209             p2.setRotation(btQuaternion(0.005, -0.053, -0.029, 0.998));
03210             //goal.push_back(p2);
03211             //arm.push_back(1);
03212 
03213             tf::Stamped<tf::Pose> p3;
03214             p3.frame_id_="map";
03215             p3.stamp_=ros::Time();
03216             //p3.setOrigin(btVector3(1.265000 - fridgeLink, -0.655000, 0.951000));
03217             p3.setOrigin(bottle + btVector3(.02,0,.03));
03218             p3.setRotation(btQuaternion(0.005001, -0.053009, -0.029005, 0.998160));
03219             goal.push_back(p3);
03220             arm.push_back(1);
03221 
03222             RobotArm::findBaseMovement(result, arm, goal,false, false);
03223             //RobotArm::findBaseMovement(result, arm, goal,false);
03224         }
03225 
03226     }
03227 
03228 
03229     if (atoi(argv[1]) == -113)
03230     {
03231         ros::Rate rt(1);
03232         double ptA[] = {0.41491862845470812, 1.3468554401788568, 1.501748997727044, -2.0247783614692936, -16.507431415382143, -1.3292235155277217, 15.027356561279952};
03233         double ptB[] = {0.040263624618489424, 0.96465557759293075, 0.27150676981727662, -1.6130504582945409, -14.582800985450046, -1.1869058378819473, 14.819427432123987};
03234         RobotArm *arml = RobotArm::getInstance(1);
03235         arml->startTrajectory(arml->goalTraj(ptA));
03236         rt.sleep();
03237         rt.sleep();
03238         arml->startTrajectory(arml->goalTraj(ptB));
03239         rt.sleep();
03240         rt.sleep();
03241         Gripper::getInstance(1)->open();
03242         Gripper::getInstance(1)->close(0.5);
03243         arml->startTrajectory(arml->goalTraj(ptB));
03244         rt.sleep();
03245         rt.sleep();
03246         arml->startTrajectory(arml->goalTraj(ptA));
03247         rt.sleep();
03248         rt.sleep();
03249     }
03250 
03251 
03252     if (atoi(argv[1]) == -120)
03253     {
03254         RobotArm *arm = RobotArm::getInstance(atoi(argv[2]));
03255         double pose[7];
03256         for (int k = 0; k < 7; ++k)
03257         {
03258             pose[k] = atof(argv[3+k]);
03259         }
03260         double duration = 5;
03261         if (argc > 10)
03262             duration = atof(argv[10]);
03263         arm->startTrajectory(arm->goalTraj(pose,duration));
03264     }
03265 
03266     if (atoi(argv[1]) == -121)
03267     {
03268         RobotArm *arm = RobotArm::getInstance(atoi(argv[2]));
03269         double pose[7];
03270         for (int k = 0; k < 7; ++k)
03271         {
03272             pose[k] = atof(argv[3+k]);
03273         }
03274         ros::Rate rate(5);
03275 
03276         while (ros::ok())
03277         {
03278             arm->startTrajectory(arm->goalTraj(pose),false);
03279             pose[6] += atof(argv[10]);
03280             ros::spinOnce();
03281             rate.sleep();
03282         }
03283     }
03284 
03285     if (atoi(argv[1]) == -130)
03286     {
03287         Pressure *r = Pressure::getInstance(0);
03288         double a[22], b[22];
03289         ros::Rate rate(25);
03290         double sum = 0;
03291         double num = 0;
03292 
03293         double table[100];
03294         double tablesum=0;
03295         for (int i= 0; i < 100; ++i)
03296         {
03297             table[i] = 0;
03298         }
03299 
03300         for (int x = 0; x < 10; ++x)
03301         {
03302             r->getCurrent(a,b,false);
03303             rate.sleep();
03304         }
03305         while (ros::ok())
03306         {
03307             double last = a[11];
03308             r->getCurrent(a,b,false);
03309             rate.sleep();
03310             sum += fabs(a[11]-last);
03311             num += 1;
03312 
03313             double logchange = log(fabs(a[11] - last) + 1 );
03314             if (logchange > 9.9)
03315                 logchange = 9.9;
03316             logchange *= 10;
03317             table[int(logchange)] += 1;
03318             tablesum+=1;
03319             double posi = 0;
03320             int i;
03321             for (i = 0; i <= int(logchange); ++i)
03322                 posi += table[i];
03323 
03324             ROS_INFO("at %.5fi %.3i logchange %.5f ", posi / tablesum, i, logchange);
03325             //ROS_INFO("log change %f 11 : %f , change %f, avg change %f", logchange, a[11], last-a[11], sum / num);
03326         }
03327     }
03328 
03329 
03330     if (atoi(argv[1]) == -131)
03331     {
03332         cart_err_monit(atoi(argv[2]));
03333     }
03334 
03335     if (atoi(argv[1]) == -200)
03336     {
03337         //tf::Stamped<tf::Pose> leftEdge;
03338         //leftEdge.setOrigin(btVector3(-.27389, 1.529, .888));
03339         //leftEdge.setOrigin(btVector3(-.3855, 2.243, 0.938));
03340         //leftEdge.setOrigin(btVector3(atof(argv[2]),atof(argv[3]),atof(argv[4])));
03341         //leftEdge.frame_id_ = "map";
03342         //tf::Stamped<tf::Pose> rightEdge;
03343         //rightEdge.frame_id_ = "map";
03344         //rightEdge.setOrigin(btVector3(0.0107, 1.797, .878));
03345         //rightEdge.setOrigin(btVector3(-.35585, 1.908, .94));
03346         //rightEdge.setOrigin(btVector3(atof(argv[5]),atof(argv[6]),atof(argv[7])));
03347         //btVector3 rel = leftEdge.getOrigin() - rightEdge.getOrigin();
03348         //double at2 = atan2(rel.y(), rel.x());
03349         //btQuaternion ori(btVector3(0,0,1), at2);
03350 
03351         //leftEdge.setRotation(ori);
03352         //rightEdge.setRotation(ori);
03353 
03354         boost::thread tp1(OperateHandleController::plateTuckPose);
03355 
03356         RobotHead::getInstance()->lookAt("/map",-.5,2.1,0.9,true);
03357 
03358         tp1.join();
03359 
03360         tf::Stamped<tf::Pose> midEdge;
03361         midEdge.frame_id_ = "map";
03362         //midEdge.setRotation(ori);
03363         //midEdge.setOrigin((rightEdge.getOrigin() + leftEdge.getOrigin()) * .5f);
03364 
03365         ros::Publisher ppub = node_handle_->advertise<geometry_msgs::PoseStamped>( "/cart_roi", 100, true );
03366         tf::Stamped<tf::Pose> roi_center;
03367         roi_center.frame_id_ = "/map";
03368         roi_center.setOrigin(btVector3(-.5,2.1,0.9));
03369         roi_center.setRotation(btQuaternion(0,0,0,1)); // switch on
03370         pubPose(roi_center,ppub);
03371 
03372         geometry_msgs::PoseStamped midpose = *(ros::topic::waitForMessage<geometry_msgs::PoseStamped>("output_grip_pose"));
03373 
03374         roi_center.setOrigin(btVector3(-1,-1,-1)); // deactivate bread detection by sending negative z roi
03375         pubPose(roi_center,ppub);
03376 
03377         btVector3 pnt;
03378         tf::pointMsgToTF(midpose.pose.position,pnt);
03379         btQuaternion quat;
03380         tf::quaternionMsgToTF(midpose.pose.orientation, quat);
03381 
03382         midEdge.setOrigin( pnt );
03383         midEdge.setRotation( quat );
03384 
03385 
03386         tf::Stamped<tf::Pose> baseInCart;
03387         baseInCart.frame_id_ = "map";
03388         baseInCart.setOrigin(btVector3(-0.002, -0.378, -0.864));
03389         baseInCart.setRotation(btQuaternion(-0.000, 0.001, 0.713, 0.701));
03390 
03391         btTransform basePose = midEdge * baseInCart;
03392         tf::Stamped<tf::Pose> newBasePose(basePose,ros::Time::now(),"map");
03393         //newBasePose.frame_id_ = "/map";
03394         //newBasePose.setOrigin(basePose);
03395         //newBasePose.setRotation(btQuaternion(-0.000, 0.001, 0.713, 0.701));
03396 
03397         RobotDriver::getInstance()->driveInMap(newBasePose);
03398 
03399         ros::Rate rt(5);
03400 
03401         tf::Stamped<tf::Pose> rightGrasp;
03402         rightGrasp.frame_id_ = "map";
03403         //rightGrasp.setOrigin(btVector3(-0.363, 2.004, 0.953));
03404         //rightGrasp.setRotation(btQuaternion(0.531, -0.495, 0.517, 0.455));
03405 
03406         tf::Stamped<tf::Pose> leftGrasp;
03407         leftGrasp.frame_id_ = "map";
03408         //leftGrasp.setOrigin(btVector3(-0.380, 2.159, 0.957));
03409         //leftGrasp.setRotation(btQuaternion(-0.457, -0.506, -0.500, 0.535));
03410 
03411         // gripper poses relative to handle center coordinate frame
03412         tf::Pose lRel;
03413         //lRel = midEdge.inverseTimes(leftGrasp);
03414         lRel.setOrigin(btVector3(0.08, 0, 0.02));
03415         lRel.setRotation(btQuaternion(0.681431, 0.004566, 0.731829, 0.007564));
03416         printPose("lRel",lRel);
03417 
03418         tf::Pose rRel;
03419         rRel.setOrigin(btVector3(-0.08, 0 ,0.02));
03420         rRel.setRotation(btQuaternion(-0.006577, -0.725437, 0.013462, 0.688126));
03421         rRel.setRotation(btQuaternion(0.681431, 0.004566, 0.731829, 0.007564));
03422         //rRel = midEdge.inverseTimes(rightGrasp);
03423         printPose("rRel",rRel);
03424 
03425         tf::Pose btrightGrasp =  midEdge * rRel ; // tf::Pose nxt = start * rel;
03426         tf::Pose btleftGrasp =  midEdge * lRel;
03427 
03428         rightGrasp.setOrigin(btrightGrasp.getOrigin());
03429         leftGrasp.setOrigin(btleftGrasp.getOrigin());
03430 
03431         rightGrasp.setRotation(btrightGrasp.getRotation());
03432         leftGrasp.setRotation(btleftGrasp.getRotation());
03433 
03434         tf::Stamped<tf::Pose> rightBase = Geometry::getPoseIn("base_link", rightGrasp);
03435         tf::Stamped<tf::Pose> leftBase = Geometry::getPoseIn("base_link", leftGrasp);
03436 
03437         // bigger y is left
03438         if (rightBase.getOrigin().y() > leftBase.getOrigin().y())
03439         {
03440             tf::Stamped<tf::Pose> tmp = rightGrasp;
03441             rightGrasp = leftGrasp;
03442             leftGrasp = tmp;
03443         }
03444 
03445 
03446         tf::Stamped<tf::Pose> preLeft;
03447         preLeft.setOrigin(btVector3(0.3, .1, .75));
03448         preLeft.frame_id_ = "base_link";
03449 
03450         tf::Stamped<tf::Pose> preRight;
03451         preRight.setOrigin(btVector3(0.3,  -.1, .75));
03452         preRight.frame_id_ = "base_link";
03453 
03454         preRight = Geometry::getPoseIn("map",preRight);
03455         preLeft = Geometry::getPoseIn("map",preLeft);
03456 
03457         preRight.setRotation(rightGrasp.getRotation());
03458         preLeft.setRotation(leftGrasp.getRotation());
03459 
03460         preRight = Geometry::getPoseIn("base_link",preRight);
03461         preLeft = Geometry::getPoseIn("base_link",preLeft);
03462 
03463         //{
03464         //    boost::thread t1(&RobotArm::universal_move_toolframe_ik_pose,RobotArm::getInstance(0),preRight);
03465         //    boost::thread t2(&RobotArm::universal_move_toolframe_ik_pose,RobotArm::getInstance(1),preLeft);
03466         //    t2.join();
03467         //    t1.join();
03468         // }
03469         RobotArm::moveBothArms(preLeft, preRight);
03470 
03471         //boost::thread t0(&RobotArm::universal_move_toolframe_ik,RobotArm::getInstance(0),0.3, -.1, .75, -0.563, -0.432, -0.465, 0.530,"base_link");
03472         //RobotArm::getInstance(1)->universal_move_toolframe_ik(0.3,  .1, .75, -0.563, -0.432, -0.465, 0.530,"base_link");
03473         //t0.join();
03474 
03475         tf::Stamped<tf::Pose> rightGraspSub = rightGrasp;
03476         rightGraspSub.setOrigin(rightGraspSub.getOrigin() - btVector3(0,0,.1));
03477         tf::Stamped<tf::Pose> leftGraspSub = leftGrasp;
03478         leftGraspSub.setOrigin(leftGraspSub.getOrigin() - btVector3(0,0,.1));
03479 
03480         if (1)
03481         {
03482             //RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(rightGraspSub);
03483             //{
03484             //    boost::thread t1(&RobotArm::universal_move_toolframe_ik_pose,RobotArm::getInstance(0),rightGraspSub);
03485             //    boost::thread t2(&RobotArm::universal_move_toolframe_ik_pose,RobotArm::getInstance(1),leftGraspSub);
03486             //    t2.join();
03487             //    t1.join();
03488             //}
03489 
03490             RobotArm::moveBothArms(leftGraspSub, rightGraspSub);
03491 
03492             OperateHandleController::openGrippers();
03493 
03494             //{
03495             //   boost::thread t1(&RobotArm::universal_move_toolframe_ik_pose,RobotArm::getInstance(0),rightGrasp);
03496             //    boost::thread t2(&RobotArm::universal_move_toolframe_ik_pose,RobotArm::getInstance(1),leftGrasp);
03497             //    t2.join();
03498             //    t1.join();
03499             // }
03500             RobotArm::moveBothArms(leftGrasp, rightGrasp);
03501 
03502             boost::thread a(&Gripper::close,Gripper::getInstance(0),0);
03503             boost::thread b(&Gripper::close,Gripper::getInstance(1),0);
03504 
03505             a.join();
03506             b.join();
03507 
03508 
03509             //{
03510             //    boost::thread t1(&RobotArm::universal_move_toolframe_ik_pose,RobotArm::getInstance(0),rightGraspPl);
03511             //    boost::thread t2(&RobotArm::universal_move_toolframe_ik_pose,RobotArm::getInstance(1),leftGraspPl);
03512             //    t2.join();
03513             //    t1.join();
03514             // }
03515 
03516             tf::Stamped<tf::Pose> rightGraspPl = rightGrasp;
03517             rightGraspPl = Geometry::getPoseIn("base_link", rightGrasp);
03518             rightGraspPl.setOrigin(rightGraspPl.getOrigin() + btVector3(0.1,0,0));
03519             tf::Stamped<tf::Pose> leftGraspPl = leftGrasp;
03520             leftGraspPl = Geometry::getPoseIn("base_link", leftGrasp);
03521             leftGraspPl.setOrigin(leftGraspPl.getOrigin() + btVector3(0.1,0,0));
03522 
03523             RobotArm::moveBothArms(leftGraspPl, rightGraspPl);
03524 
03525             //{
03526             //    boost::thread t1(&RobotArm::universal_move_toolframe_ik_pose,RobotArm::getInstance(0),rightGrasp);
03527             //    boost::thread t2(&RobotArm::universal_move_toolframe_ik_pose,RobotArm::getInstance(1),leftGrasp);
03528             //    t2.join();
03529             //    t1.join();
03530             // }
03531 
03532             RobotArm::moveBothArms(leftGrasp, rightGrasp);
03533 
03534             OperateHandleController::openGrippers();
03535 
03536             //{
03537             //    boost::thread t1(&RobotArm::universal_move_toolframe_ik_pose,RobotArm::getInstance(0),rightGraspSub);
03538             //    boost::thread t2(&RobotArm::universal_move_toolframe_ik_pose,RobotArm::getInstance(1),leftGraspSub);
03539             //    t2.join();
03540             //    t1.join();
03541             // }
03542 
03543             RobotArm::moveBothArms(leftGraspSub, rightGraspSub);
03544 
03545 
03546             //{
03547             //    boost::thread t1(&RobotArm::universal_move_toolframe_ik_pose,RobotArm::getInstance(0),preRight);
03548             //    boost::thread t2(&RobotArm::universal_move_toolframe_ik_pose,RobotArm::getInstance(1),preLeft);
03549             //    t2.join();
03550             //    t1.join();
03551             // }
03552             RobotArm::moveBothArms(preLeft, preRight);
03553         }
03554 
03555         ROS_INFO("Entering nop loop, spinning");
03556         if (0)
03557             while (ros::ok())
03558             {
03559                 //pubPose(leftEdge);
03560                 //printPose("leftEdge" , leftEdge);
03561                 //rt.sleep();
03562                 //ros::spinOnce();
03563                 //pubPose(rightEdge);
03564                 //printPose("rightEdge" , rightEdge);
03565                 //rt.sleep();
03566                 //ros::spinOnce();
03567 
03568                 pubPose(rightGrasp);
03569                 printPose("rightGrasp" , rightGrasp);
03570                 rt.sleep();
03571                 ros::spinOnce();
03572                 pubPose(leftGrasp);
03573                 printPose("leftGrasp" , leftGrasp);
03574                 rt.sleep();
03575                 ros::spinOnce();
03576             }
03577 
03578     }
03579 
03580 
03581     if (atoi(argv[1]) == -201)
03582     {
03583         while (ros::ok())
03584         {
03585             pr2_controllers_msgs::JointTrajectoryGoal goalAT = RobotArm::getInstance(0)->twoPointTrajectory(Poses::prepDishRT,Poses::prepDishRT);
03586             pr2_controllers_msgs::JointTrajectoryGoal goalBT = RobotArm::getInstance(1)->twoPointTrajectory(Poses::prepDishLT,Poses::prepDishLT);
03587             boost::thread t2T(&RobotArm::startTrajectory, RobotArm::getInstance(0), goalAT,true);
03588             boost::thread t3T(&RobotArm::startTrajectory, RobotArm::getInstance(1), goalBT,true);
03589             t2T.join();
03590             t3T.join();
03591 
03592             pr2_controllers_msgs::JointTrajectoryGoal goalA = RobotArm::getInstance(0)->twoPointTrajectory(Poses::prepDishR1,Poses::prepDishR1);
03593             pr2_controllers_msgs::JointTrajectoryGoal goalB = RobotArm::getInstance(1)->twoPointTrajectory(Poses::prepDishL1,Poses::prepDishL1);
03594             boost::thread t2(&RobotArm::startTrajectory, RobotArm::getInstance(0), goalA,true);
03595             boost::thread t3(&RobotArm::startTrajectory, RobotArm::getInstance(1), goalB,true);
03596             t2.join();
03597             t3.join();
03598         }
03599 
03600     }
03601 
03602     if (atoi(argv[1]) == -202)
03603     {
03604         while (ros::ok())
03605         {
03606             RobotArm::getInstance(1)->universal_move_toolframe_ik(0.195, 0.215, 0.621, -0.438, 0.529, -0.163, 0.708,"base_link");
03607             RobotArm::getInstance(1)->universal_move_toolframe_ik(0.297, 0.357, 0.987, -0.457, 0.446, -0.339, 0.690,"base_link");
03608         }
03609     }
03610 
03611     if (atoi(argv[1]) == -203)
03612     {
03613 
03614         RobotHead::getInstance()->lookAt("/map",-1.625371,1.143165,0.833442,true);
03615 
03616         //activate bread detection by sending roi
03617         ros::Publisher ppub = node_handle_->advertise<geometry_msgs::PoseStamped>( "/bread_roi", 100, true );
03618         tf::Stamped<tf::Pose> roi_center;
03619         roi_center.frame_id_ = "/map";
03620         roi_center.setOrigin(btVector3(-1.7,1.28,0.833442));
03621         roi_center.setRotation(btQuaternion(0,0,0,1)); // switch on
03622         pubPose(roi_center,ppub);
03623 
03624         tf::Stamped<tf::Pose> midEdge;
03625         midEdge.frame_id_ = "map";
03626         midEdge.setRotation(btQuaternion(-0.473, 0.504, 0.504, 0.518));
03627         //midEdge.setOrigin((rightEdge.getOrigin() + leftEdge.getOrigin()) * .5f);
03628 
03629         geometry_msgs::PoseStamped midpose = *(ros::topic::waitForMessage<geometry_msgs::PoseStamped>("/bread_pose"));
03630 
03631         roi_center.setOrigin(btVector3(-1,-1,-1)); // deactivate bread detection by sending negative z roi
03632         pubPose(roi_center,ppub);
03633 
03634         btVector3 pnt;
03635         tf::pointMsgToTF(midpose.pose.position,pnt);
03636 
03637         RobotArm::getInstance(1)->universal_move_toolframe_ik(0.297, 0.357, 0.987, -0.457, 0.446, -0.339, 0.690,"base_link");
03638 
03639         midEdge.setOrigin(pnt + btVector3(0,0,0.02) + btVector3(0,0,0.1));
03640         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(midEdge);
03641 
03642         Gripper::getInstance(1)->open();
03643 
03644         midEdge.setOrigin(pnt + btVector3(0,0,0.02) + btVector3(0,0,0));
03645         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(midEdge);
03646 
03647         Gripper::getInstance(1)->updatePressureZero();
03648         Gripper::getInstance(1)->closeCompliant();
03649         Gripper::getInstance(1)->close(0.04);
03650 
03651         midEdge.setOrigin(pnt + btVector3(0,0,0.02) + btVector3(0,0,0.05));
03652         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(midEdge);
03653 
03654         tf::Stamped<tf::Pose> putdown;
03655         putdown.frame_id_ = "/map";
03656         putdown.setOrigin(btVector3(-1.595, 0.907, midEdge.getOrigin().z() + 0.03));
03657         putdown.setRotation(midEdge.getRotation()); // switch on
03658 
03659         //midEdge.setOrigin(pnt + btVector3(0,0,0.02) + btVector3(0,0,0));
03660         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(putdown);
03661 
03662         putdown.setOrigin(btVector3(-1.595, 0.907, midEdge.getOrigin().z() - 0.02));
03663         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(putdown);
03664 
03665         Gripper::getInstance(1)->open();
03666 
03667         putdown.setOrigin(btVector3(-1.595, 0.907, midEdge.getOrigin().z() + 0.03));
03668         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(putdown);
03669 
03670 
03671         //midEdge.setOrigin(pnt + btVector3(0,0,0.02) + btVector3(0,0,0.1));
03672 
03673         RobotArm::getInstance(1)->universal_move_toolframe_ik(0.297, 0.357, 0.987, -0.457, 0.446, -0.339, 0.690,"base_link");
03674 
03675         //- Translation: [-1.595, 0.907, 0.876]- Rotation: in Quaternion [-0.627, -0.019, 0.778, -0.034]
03676 
03677 
03678     }
03679 
03680 
03681     if (atoi(argv[1]) == -204)
03682     {
03683 
03684         RobotHead::getInstance()->lookAt("/map",-1.625371,1.143165,0.833442,true);
03685         //activate bread detection by sending roi
03686         ros::Publisher ppub = node_handle_->advertise<geometry_msgs::PoseStamped>( "/bread_roi", 100, true );
03687         tf::Stamped<tf::Pose> roi_center;
03688         roi_center.frame_id_ = "/map";
03689         roi_center.setOrigin(btVector3(-1.7,1.28,0.833442));
03690         roi_center.setRotation(btQuaternion(0,0,0,1)); // switch on
03691         pubPose(roi_center,ppub);
03692         ros::Rate rt(10);
03693         for (int j = 0; j < 50 ; j++)
03694         {
03695             rt.sleep();
03696             ros::spinOnce();
03697         }
03698 
03699     }
03700 
03701     if (atoi(argv[1]) == -205)
03702     {
03703 
03704         RobotHead::getInstance()->lookAt("/map",-1.625371,1.143165,0.833442,true);
03705 
03706         //activate bread detection by sending roi
03707         ros::Publisher ppub = node_handle_->advertise<geometry_msgs::PoseStamped>( "/bread_roi", 100, true );
03708         tf::Stamped<tf::Pose> roi_center;
03709         roi_center.frame_id_ = "/map";
03710         roi_center.setOrigin(btVector3(-1,-1,-1)); // deactivate bread detection by sending negative z roi
03711         roi_center.setRotation(btQuaternion(0,0,0,1)); // switch on
03712         pubPose(roi_center,ppub);
03713         ros::Rate rt(10);
03714         for (int j = 0; j < 50 ; j++)
03715         {
03716             rt.sleep();
03717             ros::spinOnce();
03718         }
03719     }
03720 
03721 
03722     if (atoi(argv[1]) == -133)
03723     {
03724         OperateHandleController::plateAttackPose();
03725 
03726         RobotArm *arm = RobotArm::getInstance(atoi(argv[2]));
03727 
03728         double x,y,z,ox,oy,oz,ow;
03729         x = atof(argv[3]);
03730         y = atof(argv[4]);
03731         z = atof(argv[5]);
03732         ox = atof(argv[6]);
03733         oy = atof(argv[7]);
03734         oz = atof(argv[8]);
03735         ow = atof(argv[9]);
03736         printf("moving tool to ik goal pos in map %f %f %f or %f %f %f %f \n", x,y,z,ox,oy,oz,ow);
03737         arm->universal_move_toolframe_ik(x,y,z,ox,oy,oz,ow,"map");
03738         double state[7];
03739         arm->getJointState(state);
03740 
03741         double pose[7];
03742         for (int k = 0; k < 7; ++k)
03743         {
03744             pose[k] = state[k];
03745             ROS_INFO("k %i , state %f", k, pose[k]);
03746         }
03747         pose[0] = 0;
03748         double duration = 20;
03749         //if (argc > 10)
03750         //duration = atof(argv[10]);
03751 
03752         arm->startTrajectory(arm->goalTraj(pose,duration));
03753     }
03754 
03755     // slice it, dont dice it
03756     if (atoi(argv[1]) == -210)
03757     {
03758 
03759         Torso::getInstance()->up();
03760 
03761         double numslices = atoi(argv[2]); // 0 = one slice for computer scientists! -1 = none
03762         double slicethickness = 0.02;
03763 
03764 
03765         boost::thread t0(&OperateHandleController::plateAttackPose);
03766 
03767         RobotHead::getInstance()->lookAt("/slicer",0,0,-0.3);
03768 
03769         tf::Stamped<tf::Pose> newBasePose;
03770         newBasePose.frame_id_ = "/map";
03771         newBasePose.setOrigin(btVector3(-2.679, 1.397, 0.052));
03772         newBasePose.setRotation(btQuaternion(-0.001, 0.000, 0.046, 0.999));
03773 
03774         RobotDriver::getInstance()->driveInMap(newBasePose);
03775 
03776         t0.join();
03777 
03778         RobotHead::getInstance()->lookAt("/slicer",0,0,-0.3);
03779 
03780 
03781         //base
03782         //Translation: [-2.679, 1.397, 0.052]
03783         //- Rotation: in Quaternion [-0.001, 0.000, 0.046, 0.999]
03784         // in RPY [-0.002, 0.000, 0.092]
03785         //r_gripper_tool_frame // right of
03786         //- Translation: [-2.106, 1.153, 0.901]
03787         //- Rotation: in Quaternion [0.521, -0.491, -0.510, -0.477]
03788         //  in RPY [3.030, 1.546, -1.619]
03789         // l_gripper_tool_frame = button
03790         //- Translation: [-2.032, 1.389, 1.121]
03791         //- Rotation: in Quaternion [-0.503, 0.487, 0.502, 0.507]
03793 
03794 
03795         tf::Stamped<tf::Pose> leftEdge;
03796         leftEdge.setOrigin(btVector3(-2.032, 1.389, 1.121));
03797         leftEdge.frame_id_ = "map";
03798 
03799         tf::Stamped<tf::Pose> rightEdge;
03800         rightEdge.setOrigin(btVector3(-2.106, 1.153, 0.901));
03801         rightEdge.frame_id_ = "map";
03802 
03803         btVector3 rel = leftEdge.getOrigin() - rightEdge.getOrigin();
03804 
03805         double at2 = atan2(rel.y(), rel.x());
03806 
03807         double analog_synthesizer_tb = .303; // just to make it straight
03808 
03809         btQuaternion ori(btVector3(0,0,1), at2 + analog_synthesizer_tb);
03810 
03811         tf::Stamped<tf::Pose> midEdge;
03812 
03813         midEdge.frame_id_ = "map";
03814 
03815         rightEdge.setRotation(ori);
03816         leftEdge.setRotation(ori);
03817         midEdge.setRotation(ori);
03818 
03819         midEdge.setOrigin((rightEdge.getOrigin() + leftEdge.getOrigin()) * .5f);
03820 
03821         //Translation: [-0.229, 0.029, -0.109]
03822         //- Rotation: in Quaternion [-0.014, 0.692, -0.007, 0.722]
03823         //   in RPY [-0.613, 1.520, -0.603]
03824 
03825         //publish "midedge" as position of the slicer
03826         //rosrun tf static_transform_publisher -2.069000 1.271000 1.011000 0.000000 0.000000 0.706806 0.707407 map slicer 100
03827 
03828         tf::Stamped<tf::Pose> pur; //pick up the bread
03829         pur.setOrigin(btVector3(-0.2, 0.029, -0.109));
03830         pur.setRotation(btQuaternion(-0.014, 0.692, -0.007, 0.722));
03831         pur.frame_id_ = "slicer";
03832 
03833         //Translation: [-0.045, 0.191, -0.079]
03834         //- Rotation: in Quaternion [0.004, 0.720, 0.022, 0.693]
03835         //    in RPY [2.379, 1.518, 2.404]
03836         tf::Stamped<tf::Pose> pre; //preslicing pose
03837         pre.setOrigin(btVector3(-0.005, 0.191, -0.079));
03838         pre.setRotation(btQuaternion(0.004, 0.720, 0.022, 0.693));
03839         pre.frame_id_ = "slicer";
03840 
03841 
03842         //- Translation: [-0.061, -0.017, -0.074]
03843         //- Rotation: in Quaternion [-0.022, 0.738, 0.001, 0.675]
03844         //    in RPY [-2.835, 1.477, -2.802]
03845         tf::Stamped<tf::Pose> post; //after slicing
03846         post.setOrigin(btVector3(-0.005, -0.017, -0.079));
03847         post.setRotation(btQuaternion(-0.022, 0.738, 0.001, 0.675));
03848         post.frame_id_ = "slicer";
03849 
03850 
03851         //- Translation: [0.118, -0.037, 0.110]
03852         //- Rotation: in Quaternion [-0.011, 0.705, 0.000, 0.709]
03853         //    in RPY [-1.261, 1.555, -1.245]
03854         tf::Stamped<tf::Pose> butup;
03855         butup.setOrigin(btVector3(0.118, -0.037, 0.14));
03856         butup.setRotation(btQuaternion(-0.011, 0.705, 0.000, 0.709));
03857         butup.frame_id_ = "slicer";
03858         tf::Stamped<tf::Pose> butdown;
03859         //butdown.setOrigin(btVector3(0.118, -0.037, 0.08));
03860         //butdown.setOrigin(btVector3(0.118, -0.037, 0.07));
03861         butdown.setOrigin(btVector3(0.118, -0.037, 0.065));
03862         butdown.setRotation(btQuaternion(-0.011, 0.705, 0.000, 0.709));
03863         butdown.frame_id_ = "slicer";
03864 
03865         RobotArm *rarm = RobotArm::getInstance(0);
03866         Gripper *rgrip = Gripper::getInstance(0);
03867         RobotArm *larm = RobotArm::getInstance(1);
03868         Gripper *lgrip = Gripper::getInstance(1);
03869 
03870         ros::Rate onesec(1);
03871 
03872         if (1)
03873         {
03874             tf::Stamped<tf::Pose> nextPoseR = pur;
03875             nextPoseR.setOrigin(pur.getOrigin() + btVector3(-numslices * slicethickness,0,0.15));
03876             rarm->universal_move_toolframe_ik_pose(nextPoseR);
03877             rgrip->open();
03878 
03879             nextPoseR.setOrigin(pur.getOrigin() + btVector3(-numslices * slicethickness,0,0));
03880             rarm->universal_move_toolframe_ik_pose(nextPoseR);
03881             rgrip->close(0.04);
03882 
03883             //nextPoseR.setOrigin(pur.getOrigin() + btVector3(-numslices * slicethickness -0.05,0,0.02));
03884             //rarm->universal_move_toolframe_ik_pose(nextPoseR);
03885 
03886             boost::thread buttona(&RobotArm::move_toolframe_ik_pose, larm, butup);
03887 
03888             nextPoseR.setOrigin(pur.getOrigin() + btVector3(-numslices * slicethickness -0.05,0,0.05));
03889             rarm->universal_move_toolframe_ik_pose(nextPoseR);
03890 
03891             buttona.join();
03892 
03893             lgrip->close();
03894 
03895             boost::thread button(&RobotArm::move_toolframe_ik_pose, larm, butdown);
03896 
03897             for (double nums = numslices; nums >= -1.0; nums-=1.0)
03898             {
03899 
03900                 nextPoseR = pre;
03901                 nextPoseR.setOrigin(pre.getOrigin() + btVector3(-nums * slicethickness, 0,0));
03902                 rarm->universal_move_toolframe_ik_pose(nextPoseR);
03903 
03904                 button.join();
03905 
03906                 onesec.sleep();
03907                 onesec.sleep();
03908 
03909                 nextPoseR = post;
03910                 nextPoseR.setOrigin(post.getOrigin() + btVector3(-nums * slicethickness, 0,0));
03911 
03912                 rarm->universal_move_toolframe_ik_pose(nextPoseR);
03913 
03914 
03915                 if (nums > 0)
03916                 {
03917                     nextPoseR = pre;
03918                     nextPoseR.setOrigin(pre.getOrigin()  + btVector3(-nums * slicethickness, 0,0));
03919                     rarm->universal_move_toolframe_ik_pose(nextPoseR);
03920                 }
03921             }
03922 
03923 
03924 
03925             boost::thread t2(&RobotArm::universal_move_toolframe_ik_pose,larm,butup);
03926 
03927             nextPoseR.setOrigin(pur.getOrigin() + btVector3(0,0,0.15));
03928             rarm->universal_move_toolframe_ik_pose(nextPoseR);
03929             nextPoseR.setOrigin(pur.getOrigin() + btVector3(-0.02,0,0.02));
03930             rarm->universal_move_toolframe_ik_pose(nextPoseR);
03931             nextPoseR.setOrigin(pur.getOrigin() + btVector3(0.04,0,0.0));
03932             rarm->universal_move_toolframe_ik_pose(nextPoseR);
03933             rgrip->open();
03934             nextPoseR.setOrigin(pur.getOrigin() + btVector3(0,0,0.15));
03935             rarm->universal_move_toolframe_ik_pose(nextPoseR);
03936 
03937             OperateHandleController::plateAttackPose();
03938 
03939         }
03940 
03941 
03942         if (0)
03943         {
03944             larm->move_toolframe_ik_pose(butup);
03945             lgrip->close();
03946             larm->move_toolframe_ik_pose(butdown);
03947             onesec.sleep();
03948             onesec.sleep();
03949             onesec.sleep();
03950             larm->move_toolframe_ik_pose(butup);
03951         }
03952 
03953 
03954 
03955         ros::Rate rt(10);
03956 
03957         ROS_INFO("Entering nop loop, spinning");
03958         if (0)
03959             while (ros::ok())
03960             {
03961                 //pubPose(leftEdge);
03962                 //printPose("leftEdge" , leftEdge);
03963                 //rt.sleep();
03964                 //ros::spinOnce();
03965                 //pubPose(rightEdge);
03966                 //printPose("rightEdge" , rightEdge);
03967                 //rt.sleep();
03968                 //ros::spinOnce();
03969 
03970                 pubPose(midEdge);
03971                 printPose("midEdge" , midEdge);
03972                 rt.sleep();
03973                 ros::spinOnce();
03974                 //pubPose(leftGrasp);
03975                 //printPose("leftGrasp" , leftGrasp);
03976                 rt.sleep();
03977                 ros::spinOnce();
03978             }
03979 
03980     }
03981 
03982     if (atoi(argv[1]) == -211)
03983     {
03984 
03985         Pressure::getInstance(0);
03986         Pressure::getInstance(1);
03987 
03988         OperateHandleController::plateTuckPose();
03989 
03990         tf::Stamped<tf::Pose> newBasePose;
03991         newBasePose.frame_id_ = "/map";
03992         newBasePose.setOrigin(btVector3(-2.679, 1.397, 0.052));
03993         newBasePose.setRotation(btQuaternion(-0.001, 0.000, 0.046, 0.999));
03994 
03995         RobotDriver::getInstance()->driveInMap(newBasePose);
03996 
03997         OperateHandleController::plateAttackPose();
03998 
03999         tf::Stamped<tf::Pose> start;
04000         start.frame_id_ = "/map";
04001         start.setOrigin(btVector3(-2.144, 1.69, 0.889));
04002         start.setRotation(btQuaternion(-0.270, 0.666, -0.247, 0.650));
04003 
04004         tf::Stamped<tf::Pose> end;
04005         end.frame_id_ = "/map";
04006         end.setOrigin(btVector3(-2.153, 1.573, 0.898));
04007         end.setRotation(btQuaternion(-0.270, 0.666, -0.247, 0.650));
04008 
04009         OperateHandleController::singleSidedPick(1,start,end);
04010 
04011         tf::Stamped<tf::Pose> larm = RobotArm::getInstance(1)->getToolPose("/map");
04012         btVector3 pos = larm.getOrigin();
04013         pos.setY(2.15);
04014         larm.setOrigin(pos);
04015 
04016         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(larm);
04017 
04018         //Translation: [-2.278, 1.979, 0.868] - Rotation: in Quaternion [-0.671, 0.187, 0.279, 0.661]
04019         larm.setOrigin(btVector3(-2.278, 1.979, 0.87));
04020         larm.setRotation(btQuaternion(-0.671, 0.187, 0.279, 0.661));
04021 
04022         // [-2.226, 2.135, 0.891]
04023         //- Rotation: in Quaternion [-0.376, 0.550, -0.068, 0.742]
04024 
04025         larm.setOrigin(btVector3(-2.226, 2.135, 0.87));
04026         larm.setRotation(btQuaternion(-0.376, 0.550, -0.068, 0.742));
04027         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(larm);
04028 
04029 
04030         //Translation: [-2.300, 2.051, 0.892]
04031         //- Rotation: in Quaternion [-0.533, 0.468, 0.116, 0.695]
04032 
04033         larm.setOrigin(btVector3(-2.300, 2.051, 0.87));
04034         larm.setRotation(btQuaternion(-0.533, 0.468, 0.116, 0.695));
04035         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(larm);
04036 
04037         //RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(larm);
04038 
04039         Gripper::getInstance(1)->open();
04040 
04041         //exit(0);
04042 
04043         OperateHandleController::plateAttackPose();
04044 
04045         //drive to grasp plate pose
04046         //RobotDriver::getInstance()->moveBaseP(-2.844, 1.403,0.055, 0.998);
04047         RobotDriver::getInstance()->moveBaseP(-2.804, 1.95,  0.055, 0.998);
04048 
04049         OperateHandleController::pickPlate(btVector3(-2.18, 1.95, 0.865),.26);
04050 
04051         tf::Stamped<tf::Pose> basePos;
04052         RobotDriver::getInstance()->getRobotPose(basePos);
04053         basePos.setOrigin(basePos.getOrigin() + btVector3(-.15,0,0));
04054 
04055         RobotDriver::getInstance()->driveInMap(basePos);
04056 
04057         RobotDriver::getInstance()->moveBaseP(-3.390, -1.027, -0.697, 0.717);
04058 
04059         OperateHandleController::spinnerL(0.25,0,-.25);
04060 
04061         OperateHandleController::openGrippers();
04062 
04063         OperateHandleController::plateAttackPose();
04064 
04065 
04066     }
04067     if (atoi(argv[1]) == -212)
04068     {
04069         Pressure::getInstance(0);
04070         Pressure::getInstance(1);
04071         OperateHandleController::plateAttackPose();
04072 
04073         RobotDriver::getInstance()->moveBaseP(-2.685, 2.022,0,1);
04074         RobotHead::getInstance()->lookAt("/map",-1.8,1.6,.5,true);
04075 
04076         tf::Stamped<tf::Pose> bowlPose_ = OperateHandleController::getBowlPose();
04077         btVector3 bowlPose = bowlPose_.getOrigin();// = OperateHandleController::getBowlPose();
04078 
04079         //btVector3 bowlPose = OperateHandleController::getBowlPose();
04080         bowlPose.setZ(.909);
04081 
04082         tf::Stamped<tf::Pose> start;
04083         start.frame_id_ = "/map";
04084         //start.setOrigin(btVector3(-2.215, 1.955, 0.909));
04085         start.setOrigin(bowlPose - btVector3(.15,0,0));
04086         start.setRotation(btQuaternion(-0.631, 0.304, 0.379, 0.605));
04087         printPose("start", start);
04088 
04089         tf::Stamped<tf::Pose> end;
04090         end.frame_id_ = "/map";
04091         //end.setOrigin(btVector3(-2.07, 1.955, 0.909));
04092         end.setOrigin(bowlPose - btVector3(0.05,0,0));
04093         end.setRotation(btQuaternion(-0.631, 0.304, 0.379, 0.605));
04094         printPose("end", end);
04095 
04096         OperateHandleController::singleSidedPick(0,start,end);
04097         //- Translation: [-2.130, 1.967, 0.883]     -  Rotation: in Quaternion [-0.505, 0.472, 0.540, 0.480]
04098 
04099         //[-2.215, 1.955, 0.909] Rotation: in Quaternion [-2.215, 1.955, 0.909]
04100 
04101         tf::Stamped<tf::Pose> pitch;
04102         pitch = RobotArm::getInstance(0)->getToolPose("/map");
04103         pitch.setRotation(btQuaternion(-0.615489,0.343613,0.456079,0.543226));
04104 
04105         RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(pitch);
04106 
04107         tf::Stamped<tf::Pose> carry;
04108         carry.frame_id_ = "/base_link";
04109         carry.setOrigin(btVector3(0.347, -0.045, 0.986));
04110         carry.setRotation(btQuaternion(-0.610, -0.094, 0.737, 0.276));
04111         RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(carry);
04112 
04113         RobotDriver::getInstance()->moveBaseP(-2.981, 2.031, 0.002, 1.000);
04114 
04115         RobotDriver::getInstance()->moveBaseP(-3.180, -1.117,  -0.725, 0.689);
04116 
04117         RobotArm::getInstance(0)->universal_move_toolframe_ik(-3.085, -1.792, 0.768, -0.371, 0.651, 0.132, 0.649,"/map");
04118 
04119         Gripper::getInstance(0)->open();
04120 
04121         OperateHandleController::plateAttackPose();
04122 
04123     }
04124 
04125 
04126 
04127 
04128     // runs the whole demo once
04129     if (atoi(argv[1]) == -500)
04130     {
04131 
04132         int idx = 0;
04133         if (argc > 1)
04134             idx = atoi(argv[2]);
04135 
04136         ROS_INFO("INDEX TO DEMO :%i", idx);
04137 
04138         //int handle = 0;
04139 
04140         switch (idx)
04141         {
04142 
04143         case 0:
04144 
04145             DemoScripts::sliceTheBread(3);
04146 
04147         case 1:
04148 
04149             DemoScripts::takeBreadPlate(0);
04150 
04151         case 2:
04152 
04153             DemoScripts::takeBowl(0);
04154 
04155         }
04156     }
04157 
04158 
04159     if (atoi(argv[1]) == -304)
04160     {
04161         //    while (ros::ok())
04162         if (argc != 10)
04163         {
04164             ROS_INFO("USAGE: bin/ias_drawer_executive -304 handleframe handlex handley handlz name_of_bagfile desired_trajectory_length arm (0 = right, 1 = left) handle orientation (0 = vertical, 1 = horizontal)" );
04165             exit (0);
04166         }
04167         {
04168 
04169             tf::Stamped<tf::Pose> handleHint;
04170 
04171             handleHint.setOrigin(btVector3(atof(argv[3]),atof(argv[4]),atof(argv[5])));
04172             handleHint.frame_id_ = argv[2];
04173             handleHint.setRotation(btQuaternion(0,0,0,1));
04174 
04175             handleHint = Geometry::getPoseIn("/map", handleHint);
04176 
04177             {
04178                 RobotArm::getInstance(atoi(argv[8]))->bring_into_reach(handleHint);
04179             }
04180 
04181             handleHint = Geometry::getPoseIn("/base_link", handleHint);
04182 
04183             tf::Stamped<tf::Pose> handlePos = handleHint;
04184             if (atoi(argv[9]) == 0)
04185                 handlePos.setRotation(btQuaternion(0,0,0,1));
04186             else
04187                 handlePos.setRotation(btQuaternion(-0.7,0,0,0.7));
04188 
04189             handlePos.setOrigin(handlePos.getOrigin() + btVector3(0,0,10));
04190 
04191             while ((handlePos.getOrigin() - handleHint.getOrigin()).length() > 0.5)
04192             {
04193                 handlePos = Perception3d::getHandlePoseFromLaser(handleHint);
04194             }
04195 
04196             ros::Rate rt(10);
04197             int numsec = 5;
04198             while (ros::ok() && (--numsec > 0))
04199             {
04200                 rt.sleep();
04201                 ros::spinOnce();
04202                 pubPose(handlePos);
04203             }
04204 
04205             tf::Stamped<tf::Pose> preGraspPose = handlePos;
04206             preGraspPose = Geometry::getPoseIn("/base_link", preGraspPose);
04207             preGraspPose.setOrigin(preGraspPose.getOrigin() - btVector3(0.05,0,0)); // todo: substract_in_frame
04208             preGraspPose = Geometry::getPoseIn("/map", preGraspPose);
04209 
04210             printPose("handlePos: ", handlePos);
04211             OperateHandleController::graspHandle(atoi(argv[8]), handlePos);
04212 
04213             tf::Stamped<tf::Pose> actualGraspPose = RobotArm::getInstance(atoi(argv[8]))->getToolPose("/map");
04214 
04215             //RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(preGraspPose);
04216 
04217             //RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(actualGraspPose);
04218             std::string arm;
04219             if (atoi(argv[8]) == 0)
04220                 arm = "r";
04221             else
04222                 arm = "l";
04223 
04224             articulate(argv[6], atof(argv[7]), "/kinect_head_rgb_optical_frame", arm, "/kinect_head/camera/rgb/points");
04225             //articulate(argv[6], atof(argv[7]), "/openni_rgb_optical_frame", arm, "/camera/rgb/points");
04226 
04227             Gripper::getInstance(atoi(argv[8]))->open();
04228 
04229             RobotArm::getInstance(atoi(argv[8]))->universal_move_toolframe_ik_pose(preGraspPose);
04230 
04231             boost::thread t1(&Gripper::close, Gripper::getInstance(atoi(argv[8])), 0);
04232 
04233             OperateHandleController::plateAttackPose();
04234             OperateHandleController::plateTuckPose();
04235         }
04236     }
04237 
04238 
04239     if (atoi(argv[1]) == -704)
04240     {
04241         //    while (ros::ok())
04242         if (argc != 10)
04243         {
04244             ROS_INFO("USAGE: bin/ias_drawer_executive -304 handleframe handlex handley handlz name_of_bagfile desired_trajectory_length arm (0 = right, 1 = left) handle orientation (0 = vertical, 1 = horizontal)" );
04245             exit (0);
04246         }
04247         {
04248 
04249             tf::Stamped<tf::Pose> handleHint;
04250 
04251             handleHint.setOrigin(btVector3(atof(argv[3]),atof(argv[4]),atof(argv[5])));
04252             handleHint.frame_id_ = argv[2];
04253 
04254             tf::Stamped<tf::Pose> preGraspPose = RobotArm::getInstance(atoi(argv[8]))->getToolPose("base_link");
04255 
04256             preGraspPose = Geometry::getPoseIn("/base_link", preGraspPose);
04257             preGraspPose.setOrigin(preGraspPose.getOrigin() - btVector3(0.05,0,0)); // todo: substract_in_frame
04258             preGraspPose = Geometry::getPoseIn("/map", preGraspPose);
04259 
04260             //printPose("handlePos: ", handlePos);
04261             //OperateHandleController::graspHandle(atoi(argv[8]), handlePos);
04262 
04263             tf::Stamped<tf::Pose> actualGraspPose = RobotArm::getInstance(atoi(argv[8]))->getToolPose("/map");
04264 
04265             //RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(preGraspPose);
04266 
04267             //RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(actualGraspPose);
04268             std::string arm;
04269             if (atoi(argv[8]) == 0)
04270                 arm = "r";
04271             else
04272                 arm = "l";
04273 
04274             articulate(argv[6], atof(argv[7]), "/kinect_head_rgb_optical_frame", arm, "/kinect_head/camera/rgb/points", false);
04275             //articulate(argv[6], atof(argv[7]), "/openni_rgb_optical_frame", arm, "/camera/rgb/points");
04276 
04277             //Gripper::getInstance(atoi(argv[8]))->open();
04278 
04279             //RobotArm::getInstance(atoi(argv[8]))->universal_move_toolframe_ik_pose(preGraspPose);
04280 
04281             //boost::thread t1(&Gripper::close, Gripper::getInstance(atoi(argv[8])), 0);
04282 
04283             //OperateHandleController::plateAttackPose();
04284             //OperateHandleController::plateTuckPose();
04285         }
04286     }
04287 
04288 
04289     if (atoi(argv[1]) == -705)
04290     {
04291         //    while (ros::ok())
04292         if (argc != 10)
04293         {
04294             ROS_INFO("USAGE: bin/ias_drawer_executive -304 handleframe handlex handley handlz name_of_bagfile desired_trajectory_length arm (0 = right, 1 = left) handle orientation (0 = vertical, 1 = horizontal)" );
04295             exit (0);
04296         }
04297         {
04298 
04299             tf::Stamped<tf::Pose> handleHint;
04300 
04301             handleHint.setOrigin(btVector3(atof(argv[3]),atof(argv[4]),atof(argv[5])));
04302             handleHint.frame_id_ = argv[2];
04303 
04304             tf::Stamped<tf::Pose> preGraspPose = RobotArm::getInstance(atoi(argv[8]))->getToolPose("base_link");
04305 
04306             preGraspPose = Geometry::getPoseIn("/base_link", preGraspPose);
04307             preGraspPose.setOrigin(preGraspPose.getOrigin() - btVector3(0.05,0,0)); // todo: substract_in_frame
04308             preGraspPose = Geometry::getPoseIn("/map", preGraspPose);
04309 
04310             //printPose("handlePos: ", handlePos);
04311             //OperateHandleController::graspHandle(atoi(argv[8]), handlePos);
04312 
04313             tf::Stamped<tf::Pose> actualGraspPose = RobotArm::getInstance(atoi(argv[8]))->getToolPose("/map");
04314 
04315             //RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(preGraspPose);
04316 
04317             //RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(actualGraspPose);
04318             std::string arm;
04319             if (atoi(argv[8]) == 0)
04320                 arm = "r";
04321             else
04322                 arm = "l";
04323 
04324             articulate(argv[6], atof(argv[7]), "/kinect_head_rgb_optical_frame", arm, "/kinect_head/camera/rgb/points", true);
04325             //articulate(argv[6], atof(argv[7]), "/openni_rgb_optical_frame", arm, "/camera/rgb/points");
04326 
04327             Gripper::getInstance(atoi(argv[8]))->open();
04328 
04329             RobotArm::getInstance(atoi(argv[8]))->universal_move_toolframe_ik_pose(preGraspPose);
04330 
04331             boost::thread t1(&Gripper::close, Gripper::getInstance(atoi(argv[8])), 0);
04332 
04333             OperateHandleController::plateAttackPose();
04334             OperateHandleController::plateTuckPose();
04335         }
04336     }
04337 
04338 
04339     if (atoi(argv[1]) == -305)
04340     {
04341 
04342         bool right = true;
04343 
04344         while (ros::ok())
04345         {
04346             RobotHead::getInstance()->lookAt("/base_link",1,0,0.5);
04347 
04348             right = !right;
04349 
04350             if (!((argc > 2) && (atoi(argv[2]) == -1)))
04351                 OperateHandleController::plateAttackPose();
04352 
04353             tf::Stamped<tf::Pose> bowlPose = OperateHandleController::getBowlPose();
04354             pubPose(bowlPose);
04355             pubPose(bowlPose);
04356             pubPose(bowlPose);
04357             printPose("Bowl Pose", bowlPose);
04358 
04359             if ((argc > 2) && (atoi(argv[2]) == -1))
04360                 exit(0);
04361 
04362             tf::Stamped<tf::Pose> prepur; //pick up the bread
04363             prepur.setOrigin(btVector3(-0.015, -0.078, 0.067));
04364             prepur.setRotation(btQuaternion(0.726, 0.004, -0.686, -0.043));
04365             prepur.frame_id_ = "bowly";
04366             btTransform prepur_ = bowlPose * prepur;//tf::Pose btrightGrasp =  midEdge * rRel ;
04367 
04368             tf::Stamped<tf::Pose> pur; //pick up the bread
04369             pur.setOrigin(btVector3(-0.015, -0.078, 0.017));
04370             pur.setRotation(btQuaternion(0.726, 0.004, -0.686, -0.043));
04371             pur.frame_id_ = "bowly";
04372             btTransform pur_ = bowlPose * pur;//tf::Pose btrightGrasp =  midEdge * rRel ;
04373 
04374             tf::Stamped<tf::Pose> pickup;
04375             pickup.frame_id_ = "map";
04376             pickup.setOrigin(pur_.getOrigin());
04377             pickup.setRotation(pur_.getRotation());
04378 
04379             tf::Stamped<tf::Pose> prepickup;
04380             prepickup.frame_id_ = "map";
04381             prepickup.setOrigin(prepur_.getOrigin());
04382             prepickup.setRotation(prepur_.getRotation());
04383 
04384             RobotArm *arm = RobotArm::getInstance(right ? 0 : 1);
04385             arm->time_to_target = 0;
04386             Gripper *grip = Gripper::getInstance(right ? 0 : 1);
04387 
04388             boost::thread t1(&Gripper::open, grip, 0.09);
04389 
04390             Pressure::getInstance(right ? 0 : 1)->reset();
04391             arm->universal_move_toolframe_ik_pose(prepickup);
04392 
04393             t1.join();
04394 
04395             arm->universal_move_toolframe_ik_pose(pickup);
04396             {
04397                 RobotArm *arm = RobotArm::getInstance(right ? 0 : 1);
04398                 tf::Stamped<tf::Pose> toolPose = arm->getToolPose("map");
04399 
04400                 btTransform pur;
04401                 pur.setOrigin(btVector3(-0.015, -0.078, 0.017));
04402                 pur.setRotation(btQuaternion(0.726, 0.004, -0.686, -0.043));
04403                 //btTransform pur_ = ;//tf::Pose btrightGrasp =  midEdge * rRel ;
04404                 btTransform tool;
04405                 tool.setOrigin(toolPose.getOrigin());
04406                 tool.setRotation(toolPose.getRotation());
04407 
04408                 btTransform id;
04409                 id.setIdentity();
04410                 pur = pur.inverseTimes(id);
04411 
04412                 btTransform bowl = tool * pur;
04413 
04414                 //tf::Stamped<tf::Pose> bowlPose;
04415                 bowlPose.frame_id_ = "/map";
04416                 bowlPose.setOrigin(bowl.getOrigin());
04417                 bowlPose.setRotation(bowl.getRotation());
04418 
04419                 pubPose(bowlPose);
04420                 printPose("Bowl Pose calc", bowlPose);
04421             }
04422 
04423             grip->closeCompliant();
04424             grip->close();
04425 
04426             arm->stabilize_grip();
04427 
04428             tf::Stamped<tf::Pose> tpush = arm->getToolPose("/map");
04429             tpush.setOrigin(tpush.getOrigin() + btVector3(0,0,-.02));
04430             arm->universal_move_toolframe_ik_pose(tpush);
04431 
04432             arm->stabilize_grip();
04433 
04434             tf::Stamped<tf::Pose> tp = arm->getToolPose("/map");
04435             tf::Stamped<tf::Pose> tp_high = arm->getToolPose("/map");
04436             tp_high = Geometry::getPoseIn("/base_link",tp_high);
04437             //tp_high.setOrigin(tp_high.getOrigin() + btVector3(-.1,right ? -0.1 : 0.1,0.1));
04438             tp_high.setOrigin(tp_high.getOrigin() + btVector3(-.1,0,0.1));
04439 
04440             arm->universal_move_toolframe_ik_pose(tp_high);
04441 
04442             if (argc > 2)
04443                 exit(0);
04444 
04445             arm->universal_move_toolframe_ik_pose(tp);
04446             grip->open();
04447             arm->universal_move_toolframe_ik_pose(prepickup);
04448 
04449             OperateHandleController::plateAttackPose();
04450 
04451 
04452         }
04453     }
04454 
04455 
04456     if (atoi(argv[1]) == -306)
04457     {
04458 
04459         int currside = atoi(argv[2]);
04460 
04461         bool closingCalibrated[2] = {false, false};
04462         double minClosing[2] = {0.0025, 0.0025};
04463 
04464         while (ros::ok())
04465         {
04466 
04467             RobotHead::getInstance()->lookAt("/base_link",1,0,0.5);
04468 
04469             tf::Stamped<tf::Pose> bowlPose; // = OperateHandleController::getBowlPose();
04470             pubPose(bowlPose);
04471             printPose("Bowl Pose vision", bowlPose);
04472 
04473             RobotArm *arm = RobotArm::getInstance(currside);
04474             Gripper *gripper = Gripper::getInstance(currside);
04475             //double side = (currside ? 1 : -1);
04476             tf::Stamped<tf::Pose> toolPose = arm->getToolPose("map");
04477 
04478             btTransform pur; //pick up the bread
04479             pur.setOrigin(btVector3(-0.015, -0.078, 0.017));
04480             pur.setRotation(btQuaternion(0.726, 0.004, -0.686, -0.043));
04481             //btTransform pur_ = ;//tf::Pose btrightGrasp =  midEdge * rRel ;
04482             btTransform tool;
04483             tool.setOrigin(toolPose.getOrigin());
04484             tool.setRotation(toolPose.getRotation());
04485 
04486             btTransform id;
04487             id.setIdentity();
04488             pur = pur.inverseTimes(id);
04489 
04490             btTransform bowl = tool * pur;
04491 
04492             //tf::Stamped<tf::Pose> bowlPose;
04493             bowlPose.frame_id_ = "/map";
04494             bowlPose.setOrigin(bowl.getOrigin());
04495             bowlPose.setRotation(bowl.getRotation());
04496 
04497             //pubPose(bowlPose);
04498             //printPose("Bowl Pose calc", bowlPose);
04499 
04500             //arm->evil_switch = true;
04501             arm->time_to_target = 0.3;
04502 
04503             ros::Rate rt(2);
04504 
04505             //turn to best ext
04506             tf::Stamped<tf::Pose> bestPose = turnToSide(currside,toolPose,bowlPose);
04507 
04508             arm->universal_move_toolframe_ik_pose(bestPose);
04509 
04510             // bowl vision turn
04511             tf::Stamped<tf::Pose> straightPose = turnBowlStraight(currside, bowlPose);
04512             straightPose = turnToSide(currside,straightPose,bowlPose);
04513 
04514             arm->universal_move_toolframe_ik_pose(straightPose);
04515 
04516             tf::Stamped<tf::Pose> worstPose = turnToSide(currside ? 0 : 1,straightPose,bowlPose);
04517 
04518             RobotArm *otherarm = RobotArm::getInstance(currside  ? 0 : 1);
04519 
04520             tf::Stamped<tf::Pose> highapp = worstPose;
04521             highapp.setOrigin(worstPose.getOrigin() + btVector3(0,0,0.05));
04522             Gripper *othergripper = Gripper::getInstance(currside ? 0 : 1);
04523 
04524             otherarm->universal_move_toolframe_ik_pose(highapp);
04525 
04526             double amountopen = 0;
04527             double additional_depth = -.014;
04528             while (amountopen < minClosing[currside ? 0 : 1] + 0.003)
04529             {
04530                 othergripper->open();
04531                 tf::Stamped<tf::Pose> grasppose = worstPose;
04532                 grasppose.setOrigin(worstPose.getOrigin() + btVector3(0,0,additional_depth));
04533                 otherarm->universal_move_toolframe_ik_pose(grasppose);
04534                 othergripper->closeCompliant();
04535                 othergripper->close();
04536                 ROS_INFO("Amount open %f", othergripper->getAmountOpen());
04537                 amountopen = othergripper->getAmountOpen();
04538                 additional_depth -= 0.025;
04539             }
04540             otherarm->stabilize_grip();
04541 
04542             tf::Stamped<tf::Pose> highleave = bestPose;
04543             highleave.setOrigin(bestPose.getOrigin() + btVector3(0,0,0.05));
04544             ros::Duration(0.5).sleep();
04545             gripper->open();
04546             arm->universal_move_toolframe_ik_pose(highleave);
04547 
04548             boost::thread t1(&Gripper::close, gripper, 0);
04549 
04550             currside ? OperateHandleController::plateAttackPoseLeft() : OperateHandleController::plateAttackPoseRight();
04551 
04552             t1.join();
04553             closingCalibrated[currside] = true;
04554             minClosing[currside] = gripper->getAmountOpen();
04555 
04556             currside = currside ? 0 : 1;
04557 
04558         }
04559 
04560     }
04561 
04562 
04563     if (atoi(argv[1]) == -307)
04564     {
04565 
04566         int currside = atoi(argv[2]);
04567 
04568         while (ros::ok())
04569         {
04570 
04571             RobotHead::getInstance()->lookAt("/base_link",1,0,0.5);
04572 
04573             RobotArm *arm = RobotArm::getInstance(currside);
04574 
04575             tf::Stamped<tf::Pose> bowlPose = OperateHandleController::getBowlPose();
04576             bowlPose = Geometry::getPoseIn("/base_link", bowlPose);
04577             pubPose(bowlPose);
04578             printPose("Bowl Pose vision", bowlPose);
04579 
04580             tf::Stamped<tf::Pose> bowlInBase = Geometry::getPoseIn("/base_link", bowlPose);
04581             //bowlInBase.setOrigin(btVector3(0,0,0));
04582 
04583             tf::Stamped<tf::Pose> bowlCorrected = bowlPose;
04584             bowlCorrected.setRotation(bowlInBase.getRotation().inverse() * bowlPose.getRotation());
04585 
04586             pubPose(bowlCorrected);
04587             pubPose(bowlCorrected);
04588             pubPose(bowlCorrected);
04589             printPose("Bowl Pose corrected", bowlCorrected);
04590 
04591             tf::Stamped<tf::Pose> newToolPose = arm->getToolPose("/base_link");
04592             printPose("Tool Pose", newToolPose);
04593             btQuaternion cor = bowlInBase.getRotation().inverse();
04594 
04595             printPose("Bowl Pose", bowlInBase);
04596             tf::Stamped<tf::Pose> act = Geometry::rotateAroundPose(newToolPose, bowlInBase, cor);
04597             printPose("act Pose", act);
04598 
04599             newToolPose.setRotation(bowlInBase.getRotation().inverse() * newToolPose.getRotation());
04600 
04601             printPose("nt Pose (rot)", newToolPose);
04602 
04603             arm->universal_move_toolframe_ik_pose(act);
04604 
04605             exit(0);
04606 
04607         }
04608     }
04609 
04610     if (atoi(argv[1]) == -310)
04611     {
04612         releaseWhenPulled(atoi(argv[2]));
04613     }
04614 
04615 
04616     if (atoi(argv[1]) == -600)
04617     {
04618 
04619         int idx = 0;
04620         if (argc > 1)
04621             idx = atoi(argv[2]);
04622 
04623         ROS_INFO("INDEX TO DEMO :%i", idx);
04624 
04625         //int handle = 0;
04626 
04627         switch (idx)
04628         {
04629         case 0:
04630 
04631         case 2:
04632 
04633             DemoScripts::takeBottleFromFridge();
04634 
04635         case 3:
04636 
04637             DemoScripts::serveBottle();
04638 
04639         case 4:
04640 
04641             DemoScripts::openDrawer();
04642 
04643         case 5:
04644 
04645             DemoScripts::takePlate();
04646 
04647         case 6:
04648         {
04649             RobotHead::getInstance()->lookAtThreaded("/map", -1.85, 2.1, 1);
04650             DemoScripts::servePlateToIsland();
04651         }
04652 
04653         case 7:
04654 
04655             DemoScripts::takeSilverware();
04656 
04657 
04658         case 8:
04659         {
04660             RobotHead::getInstance()->lookAtThreaded("/map", -1.85, 2.1, 1);
04661             DemoScripts::servePlateToIsland();
04662         }
04663 
04664         }
04665     }
04666 
04667     if (atoi(argv[1]) == -601)
04668     {
04669 
04670         int idx = 0;
04671         if (argc > 1)
04672             idx = atoi(argv[2]);
04673 
04674         ROS_INFO("INDEX TO DEMO :%i", idx);
04675 
04676         //int handle = 0;
04677 
04678         switch (idx)
04679         {
04680         case 0:
04681 
04682         case 2:
04683 
04684             DemoScripts::takeBottleFromFridge();
04685 
04686         case 3:
04687 
04688             DemoScripts::serveBottle();
04689 
04690         case 4:
04691 
04692             DemoScripts::openDrawer();
04693 
04694         case 5:
04695 
04696             DemoScripts::takePlate();
04697 
04698         case 6:
04699         {
04700             RobotHead::getInstance()->lookAtThreaded("/map", -1.85, 2.1, 1);
04701             DemoScripts::servePlateToIsland();
04702         }
04703 
04704         case 7:
04705 
04706             DemoScripts::takeSilverware();
04707 
04708         case 9:
04709         {
04710             RobotHead::getInstance()->lookAtThreaded("/map", .33, -2, 1);
04711             DemoScripts::serveToTable2();
04712         }
04713 
04714         case 10:
04715             DemoScripts::takePlateFromIsland();
04716 
04717         case 11:
04718         {
04719             RobotHead::getInstance()->lookAtThreaded("/map", .33, -2, 1);
04720             DemoScripts::serveToTable2();
04721         }
04722 
04723 
04724 
04725         }
04726     }
04727 
04728     // lift both hands 3 cms
04729     if (atoi(argv[1]) == -602)
04730     {
04731 
04732         tf::Stamped<tf::Pose> rP = RobotArm::getInstance(0)->getToolPose("base_link");
04733         rP.setOrigin(rP.getOrigin() + btVector3(0,-.025,0.03));
04734         rP = Geometry::getPoseIn("map", rP);
04735 
04736         tf::Stamped<tf::Pose> lP = RobotArm::getInstance(1)->getToolPose("base_link");
04737         lP.setOrigin(lP.getOrigin() + btVector3(0,0.025,0.03));
04738         lP = Geometry::getPoseIn("map", lP);
04739 
04740         RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(rP);
04741         RobotArm::getInstance(1)->universal_move_toolframe_ik_pose(lP);
04742     }
04743 
04744 
04745     if (atoi(argv[1]) == -603)
04746     {
04747         while (ros::ok())
04748 
04749         {
04750 
04751             tf::Stamped<tf::Pose> cam = Geometry::getPose("/map","/openni_rgb_optical_frame");
04752             tf::Stamped<tf::Pose> grip = Geometry::getPose("map","/r_gripper_tool_frame");
04753 
04754             geometry_msgs::PoseStamped camPS;
04755             tf::poseStampedTFToMsg(cam,camPS);
04756 
04757             geometry_msgs::PoseStamped gripPS;
04758             tf::poseStampedTFToMsg(grip,gripPS);
04759 
04760             sensor_msgs::PointCloud2 pc = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/kinect/cloud_throttled"));
04761 
04762             tf::Stamped<tf::Pose> net_stamped = Geometry::getPose("/r_gripper_tool_frame","/openni_rgb_optical_frame");
04763             tf::Transform net_transform;
04764             net_transform.setOrigin(net_stamped.getOrigin());
04765             net_transform.setRotation(net_stamped.getRotation());
04766 
04767             sensor_msgs::PointCloud2 pct; //in gripper frame
04768 
04769             pcl_ros::transformPointCloud("/r_gripper_tool_frame",net_transform,pc,pct);
04770             pct.header.frame_id = "/map";
04771 
04772             //xxx void pcl_ros::transformPointCloud     (       const std::string &     target_frame,           const tf::Transform &   net_transform,          const sensor_msgs::PointCloud2 &        in,             sensor_msgs::PointCloud2 &      out
04773 
04774             //pcl::PointCloud<pcl::PointXYZ> cloud;
04775 
04776             //pcl::fromROSMsg(pct,cloud);
04777 
04778             ros::Publisher pct_pub = node_handle.advertise<sensor_msgs::PointCloud2>( "/pct", 10 , true);
04779 
04780             pct_pub.publish(pct);
04781 
04782             ros::Rate rt(5.0);
04783 
04784             while (ros::ok())
04785             {
04786                 rt.sleep();
04787                 ros::spinOnce();
04788             }
04789 
04790 
04791             if (0)
04792             {
04793                 namespace ser = ros::serialization;
04794 
04795                 //std_msgs::UInt32 my_value;
04796                 //my_value.data = 5;
04797 
04798                 uint32_t serial_size = ros::serialization::serializationLength(pc);
04799                 boost::shared_array<uint8_t> buffer(new uint8_t[serial_size]);
04800 
04801                 ser::OStream stream(buffer.get(), serial_size);
04802                 ser::serialize(stream, pc);
04803 
04804                 ROS_INFO("RECEIVED CLOUD %i", serial_size);
04805 
04806                 // opening and closing a file
04807                 ofstream outfile;
04808 
04809                 outfile.open ("test.txt");
04810 
04811                 //ser::serialize(outfile,pc);
04812                 //for ... outfile << buffer; // doesnt work o
04813 
04814                 // >> i/o operations here <<
04815 
04816                 outfile.close();
04817 
04818                 // deserialize
04819                 //std_msgs::UInt32 my_value;
04820 
04821                 //uint32_t serial_size = ros::serialization::serializationLength(pc);
04822                 {
04823 
04824                     sensor_msgs::PointCloud2 pco;
04825 
04826                     //boost::shared_array<uint8_t> buffer(new uint8_t[serial_size]);
04827 
04828                     // Fill buffer with a serialized UInt32
04829                     ser::IStream stream(buffer.get(), serial_size);
04830                     //ser::deserialize(stream, my_value);
04831                     ros::serialization::Serializer<sensor_msgs::PointCloud2>::read(stream, pco);
04832 
04833                     ROS_INFO("data size : %zu %zu", pco.data.size(), pc.data.size());
04834                 }
04835             }
04836 
04837             return 0;
04838 
04839         }
04840     }
04841 
04842     if (atoi(argv[1]) == -604)
04843     {
04844 
04845         ros::Publisher pcm_pub_in = node_handle.advertise<sensor_msgs::PointCloud2>( "/pc_inlier", 10 , true);
04846         ros::Publisher pcm_pub_out = node_handle.advertise<sensor_msgs::PointCloud2>( "/pc_outlier", 10 , true);
04847 
04848 
04849         while (ros::ok())
04850         {
04851 
04852             //tf::Stamped<tf::Pose> cam = Geometry::getPose("/map","/openni_rgb_optical_frame");
04853             //tf::Stamped<tf::Pose> grip = Geometry::getPose("map","/r_gripper_tool_frame");
04854 
04855             //geometry_msgs::PoseStamped camPS;
04856             //tf::poseStampedTFToMsg(cam,camPS);
04857 
04858             //geometry_msgs::PoseStamped gripPS;
04859             //tf::poseStampedTFToMsg(grip,gripPS);
04860             boost::shared_ptr<const sensor_msgs::PointCloud2> pca;
04861             boost::shared_ptr<const sensor_msgs::PointCloud2> pcb;
04862 
04863             ros::Rate rt(5.0);
04864 
04865             pca = ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/pc");
04866 
04867             ROS_INFO("Spinning");
04868 
04869             while (ros::ok())
04870             {
04871 
04872                 pcb = pca;
04873                 pca = ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/pc");
04874 
04875                 ROS_INFO("got two clouds");
04876 
04877                 pcl::PointCloud<pcl::PointXYZRGB> clouda;
04878                 pcl::PointCloud<pcl::PointXYZRGB> cloudb;
04879 
04880                 pcl::PointCloud<pcl::PointXYZRGB> cloudinlier;
04881                 pcl::PointCloud<pcl::PointXYZRGB> cloudoutlier;
04882 
04883                 pcl::fromROSMsg(*pca,clouda);
04884                 pcl::fromROSMsg(*pcb,cloudb);
04885 
04886                 //pct_pub.publish(pct);
04887 
04888                 pcl::KdTree<pcl::PointXYZRGB>::Ptr tree;
04889                 tree.reset (new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
04890                 tree->setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZRGB> >(clouda));
04891 
04892                 // Allocate enough space to hold the results
04893                 // \note resize is irrelevant for a radiusSearch ().
04894                 std::vector<int> nn_indices (1);
04895                 std::vector<float> nn_sqr_dists (1);
04896 
04897                 double distance_threshold = 0.01;
04898                 for (size_t k = 0; k < cloudb.points.size(); ++k )
04899                 {
04900 
04901                     if (tree->radiusSearch (cloudb.points[k], distance_threshold, nn_indices,nn_sqr_dists, 1) != 0)
04902                     {
04903                         // there are neighbors! see nn_indices, nn_sqr_dists
04904                         cloudinlier.points.push_back(cloudb.points[k]);
04905                     }
04906                     else
04907                     {
04908                         cloudoutlier.points.push_back(cloudb.points[k]);
04909                     }
04910                 }
04911 
04912                 sensor_msgs::PointCloud2 outliermsg;
04913                 sensor_msgs::PointCloud2 inliermsg;
04914 
04915                 pcl::toROSMsg(cloudoutlier,outliermsg);
04916                 outliermsg.header = pcb->header;
04917                 pcm_pub_out.publish(outliermsg);
04918 
04919                 pcl::toROSMsg(cloudinlier,inliermsg);
04920                 inliermsg.header = pcb->header;
04921                 pcm_pub_in.publish(inliermsg);
04922 
04923                 rt.sleep();
04924                 ros::spinOnce();
04925             }
04926 
04927             return 0;
04928 
04929         }
04930     }
04931 
04932     if (atoi(argv[1]) == -605)
04933     {
04934 
04935 
04936         /*
04937         0.934298 -0.530159 0.981964  0.002485 0.005966 0.002250 0.999977
04938         0.924528 -0.533511 0.981347  0.002485 0.005966 0.002250 0.999977
04939         0.914757 -0.536862 0.980729  0.002485 0.005966 0.002250 0.999977
04940         0.904986 -0.540214 0.980112  0.002485 0.005966 0.002250 0.999977
04941         0.895216 -0.543565 0.979495  0.002485 0.005966 0.002250 0.999977
04942         */
04943 
04944         btTransform t[1000];
04945 
04946         t[0].setOrigin(btVector3(0.896, -0.185, 0.931));
04947         t[0].setRotation(btQuaternion(-0.007, -0.352, 0.003, 0.936));
04948         t[1].setOrigin(btVector3(0.896, -0.185, 0.92));
04949         t[1].setRotation(btQuaternion(-0.007, -0.352, 0.003, 0.936));
04950 
04951         {
04952             tf::Stamped<tf::Pose> secondlast;
04953             secondlast.frame_id_ = "/base_link";
04954             secondlast.setOrigin(t[0].getOrigin());
04955             secondlast.setRotation(t[0].getRotation());
04956             tf::Stamped<tf::Pose> last;
04957             last.frame_id_ = "/base_link";
04958             last.setOrigin(t[1].getOrigin());
04959             last.setRotation(t[1].getRotation());
04960             double dist = reachable_distance(secondlast,last);
04961             ROS_INFO("REACHABLE DISTANCE %f", dist);
04962 
04963         }
04964 
04965         ROS_INFO("START");
04966         bool reachable = true;
04967 
04968         double distance = 0;
04969         //calulate the distance we can further follow the extrapolated trajectory within the workspace bounds
04970         for (int k = 2; reachable && (k < 1000); ++k)
04971         {
04972             t[k] = t[k-1] * (t[0].inverseTimes(t[1]));
04973             tf::Stamped<tf::Pose> act;
04974             act.frame_id_ = "/base_link";
04975             act.setOrigin(t[k].getOrigin());
04976             act.setRotation(t[k].getRotation());
04977             //pubPose(act);
04978             printPose("interpolated", act);
04979             geometry_msgs::PoseStamped pose;
04980             double start_angles[7];
04981             double solution[7];
04982             tf::poseStampedTFToMsg(RobotArm::getInstance(0)->tool2wrist(act),pose);
04983             reachable &= RobotArm::getInstance(0)->run_ik(pose,start_angles,solution,"r_wrist_roll_link");
04984             if (reachable)
04985                 distance += (t[k].getOrigin() - t[k-1].getOrigin()).length();
04986             ROS_INFO(reachable ? "reachable %f" : "not reachable %f", distance);
04987         }
04988 
04989 
04990         //bool RobotArm::run_ik(geometry_msgs::PoseStamped pose, double start_angles[7],
04991         //            double solution[7], std::string link_name)
04992 
04993 
04994         ROS_INFO("END");
04995     }
04996 
04997 
04998     /*if (atoi(argv[1]) == -606)
04999     {
05000 
05001         mangle();
05002 
05003     }*/
05004 
05005     if (atoi(argv[1]) == -606)
05006     {
05007 
05008         mangle(argv[2]);
05009 
05010     }
05011 
05012     if (atoi(argv[1]) == -607)
05013     {
05014         btVector3 hint(atof(argv[2]),atof(argv[3]),atof(argv[4]));
05015 
05016 
05017         tf::Stamped<tf::Pose> handleHint;
05018 
05019         handleHint.setOrigin(hint);
05020         handleHint.frame_id_ = "/map";
05021 
05022         handleHint = Geometry::getPoseIn("/map", handleHint);
05023 
05024         if (0)
05025         {
05026             RobotArm::getInstance(atoi(argv[8]))->bring_into_reach(handleHint);
05027         }
05028 
05029         handleHint = Geometry::getPoseIn("/base_link", handleHint);
05030 
05031         ros::Time start = ros::Time::now();
05032 
05033         tf::Stamped<tf::Pose> handlePos = handleHint;
05034         if (atoi(argv[9]) == 0)
05035             handlePos.setRotation(btQuaternion(0,0,0,1));
05036         else
05037             handlePos.setRotation(btQuaternion(-0.7,0,0,0.7));
05038 
05039         handlePos.setOrigin(handlePos.getOrigin() + btVector3(0,0,10));
05040 
05041         while (((handlePos.getOrigin() - handleHint.getOrigin()).length() > 0.5) && (ros::Time::now() - start < ros::Duration(60)))
05042         {
05043             handlePos = Perception3d::getHandlePoseFromLaser(handleHint, 30);
05044         }
05045 
05046         printPose("pose in map", Geometry::getPoseIn("/map",handlePos));
05047 
05048         if ((handlePos.getOrigin() - handleHint.getOrigin()).length() == 0)
05049             ROS_INFO("FAIL");
05050         else
05051             ROS_INFO("SUCCESS: %f ", (handlePos.getOrigin() - handleHint.getOrigin()).length() );
05052 
05053     }
05054 
05055     if (atoi(argv[1]) == -608)
05056     {
05057 
05058         srand ( time(NULL) );
05059         ros::Publisher vis_pub;
05060         ros::NodeHandle nh_;
05061         vis_pub = nh_.advertise<visualization_msgs::Marker>( "find_base_pose_markers", 10000, true );
05062         int markerCnt =0;
05063 
05064         btVector3 hint(atof(argv[2]),atof(argv[3]),atof(argv[4]));
05065         //tf::Pose lastPose;
05066 
05067         tf::Stamped<tf::Pose> inFront;
05068         inFront.frame_id_ = "/map";
05069         inFront.setOrigin(btVector3(0.295, 0.387, 0.051));
05070         inFront.setRotation(btQuaternion(0.000, -0.001, 0.037, 0.999));
05071 
05072         RobotDriver::getInstance()->driveInMap(inFront);
05073 
05074         inFront = Geometry::getPoseIn("/base_link", inFront);
05075 
05076         std::vector<tf::Stamped<tf::Pose> > poses;
05077 
05078         for (double x = 0; x >= - .5; x -= 0.25)
05079             for (double y = -0.35; y <= 0.35; y += 0.35)
05080             {
05081 
05082             }
05083     }
05084 
05085     if (atoi(argv[1]) == -609)
05086     {
05087 
05088         srand ( time(NULL) );
05089         ros::Publisher vis_pub;
05090         ros::NodeHandle nh_;
05091         vis_pub = nh_.advertise<visualization_msgs::Marker>( "find_base_pose_markers", 10000, true );
05092         int markerCnt =0;
05093 
05094         btVector3 hint(atof(argv[2]),atof(argv[3]),atof(argv[4]));
05095         //tf::Pose lastPose;
05096 
05097         while (ros::ok())
05098         {
05099             double x = ((rand() % 10000) / 5000.0f) - 1;
05100             double y = ((rand() % 10000) / 5000.0f) - 1;
05101             double angle = ((rand() % 10000) / (1000.0f));
05102             btQuaternion quat(btVector3(0,0,1), angle);
05103 
05104             tf::Pose pose;
05105             pose.setOrigin(btVector3(hint.x() - x * 0.75  - 0.85 ,hint.y() + y * 1,0));
05106             pose.setRotation(quat);
05107 
05108             tf::Stamped<tf::Pose> current;
05109             RobotDriver::getInstance()->getRobotPose(current);
05110 
05111             double colr = 0;
05112             double colg = 1;
05113             double colb = 0;
05114             //if (0)
05115 
05116 
05117             btTransform relative = current.inverseTimes(pose);
05118             colr = 1;
05119             colg = 0;
05120             colb = 0;
05121             tf::Pose rel;
05122             rel.setOrigin(current.getOrigin());
05123             rel.setRotation(btQuaternion(btVector3(0,0,1),atan2( pose.getOrigin().y() - current.getOrigin().y(), pose.getOrigin().x() - current.getOrigin().x())));
05124             rel.setRotation(relative.getRotation());
05125             //if (0)
05126 
05127 
05128             tf::Pose handle;
05129             handle.setOrigin(hint);
05130             tf::Pose handleInNext = pose.inverseTimes(handle);
05131             double fangle = atan2(handleInNext.getOrigin().y(), handleInNext.getOrigin().x());
05132             {
05133                 visualization_msgs::Marker marker;
05134                 marker.header.frame_id = "/map";
05135                 marker.header.stamp = ros::Time::now();
05136                 marker.ns = "target base footprints";
05137                 marker.id = ++markerCnt;
05138                 marker.type = visualization_msgs::Marker::ARROW;
05139                 marker.action = visualization_msgs::Marker::ADD;
05140                 marker.pose.position.x = pose.getOrigin().x();
05141                 marker.pose.position.y = pose.getOrigin().y();
05142                 marker.pose.position.z = pose.getOrigin().z();
05143                 marker.pose.orientation.x =pose.getRotation().x();
05144                 marker.pose.orientation.y = pose.getRotation().y();
05145                 marker.pose.orientation.z = pose.getRotation().z();
05146                 marker.pose.orientation.w = pose.getRotation().w();
05147 
05148                 marker.scale.x = .25;
05149                 marker.scale.y = 0.15;
05150                 marker.scale.z = 0.15;
05151                 marker.color.a = 1;
05152                 if ((handleInNext.getOrigin().x() > 1) || (handleInNext.getOrigin().x() < 0) ||
05153                         (fabs(handleInNext.getOrigin().y()) > 0.5))
05154                     colr = 0;
05155                 if (fabs(fangle) > M_PI / 2.0f)
05156                     colr = 0;
05157                 marker.color.r = colr;
05158                 marker.color.g = colg;
05159                 marker.color.b = colb;
05160                 vis_pub.publish( marker );
05161                 ros::spinOnce();
05162             }
05163 
05164             ros::Duration(0.1).sleep();
05165 
05166             if (0)
05167             {
05168                 tf::Stamped<tf::Pose> relativeS;
05169                 relativeS.setOrigin(rel.getOrigin() + btVector3(0.01,0.01,0));
05170                 relativeS.setRotation(rel.getRotation());
05171                 relativeS.frame_id_ = "/map";
05172                 RobotDriver::getInstance()->driveInMap(relativeS,false);
05173 
05174                 tf::Stamped<tf::Pose> goalPose;
05175                 goalPose.frame_id_ = "/map";
05176                 goalPose.setOrigin(pose.getOrigin());
05177                 goalPose.setRotation(pose.getRotation());
05178 
05179                 bool free = RobotDriver::getInstance()->checkCollision(goalPose);
05180                 ROS_INFO("Free: %s", free ? "YES FREE" : "no.");
05181 
05182                 if (free)
05183                 {
05184 
05185                     RobotDriver::getInstance()->driveInMap(goalPose);
05186 
05187                     btVector3 hint(atof(argv[2]),atof(argv[3]),atof(argv[4]));
05188 
05189                     tf::Stamped<tf::Pose> handleHint;
05190 
05191                     handleHint.setOrigin(hint);
05192                     handleHint.frame_id_ = "/map";
05193 
05194                     handleHint = Geometry::getPoseIn("/map", handleHint);
05195 
05196                     if (0)
05197                     {
05198                         RobotArm::getInstance(atoi(argv[8]))->bring_into_reach(handleHint);
05199                     }
05200 
05201                     handleHint = Geometry::getPoseIn("/base_link", handleHint);
05202 
05203                     ros::Time start = ros::Time::now();
05204 
05205                     tf::Stamped<tf::Pose> handlePos = handleHint;
05206                     if (atoi(argv[9]) == 0)
05207                         handlePos.setRotation(btQuaternion(0,0,0,1));
05208                     else
05209                         handlePos.setRotation(btQuaternion(-0.7,0,0,0.7));
05210 
05211                     handlePos.setOrigin(handlePos.getOrigin() + btVector3(0,0,10));
05212 
05213                     //while (((handlePos.getOrigin() - handleHint.getOrigin()).length() > 0.5) && (ros::Time::now() - start < ros::Duration(60)))
05214                     //{
05215                     handlePos = Perception3d::getHandlePoseFromLaser(handleHint, 30);
05216                     //}
05217 
05218                     printPose("pose in map", Geometry::getPoseIn("/map",handlePos));
05219 
05220                     if (((handlePos.getOrigin() - handleHint.getOrigin()).length() > 0.2) || ((handlePos.getOrigin() - handleHint.getOrigin()).length() == 0.0))
05221                     {
05222                         ROS_INFO("FAIL");
05223                         {
05224                             visualization_msgs::Marker marker;
05225                             marker.header.frame_id = "/map";
05226                             marker.header.stamp = ros::Time::now();
05227                             marker.ns = "target base footprints";
05228                             marker.id = ++markerCnt;
05229                             marker.type = visualization_msgs::Marker::ARROW;
05230                             marker.action = visualization_msgs::Marker::ADD;
05231                             marker.pose.position.x = pose.getOrigin().x();
05232                             marker.pose.position.y = pose.getOrigin().y();
05233                             marker.pose.position.z = pose.getOrigin().z();
05234                             marker.pose.orientation.x = pose.getRotation().x();
05235                             marker.pose.orientation.y = pose.getRotation().y();
05236                             marker.pose.orientation.z = pose.getRotation().z();
05237                             marker.pose.orientation.w = pose.getRotation().w();
05238                             marker.scale.x = 0.15;
05239                             marker.scale.y = 0.55;
05240                             marker.scale.z = 0.55;
05241                             marker.color.a = 1;
05242                             marker.color.r = 1.0;
05243                             marker.color.g = 0.1;
05244                             marker.color.b = 0.1;
05245                             vis_pub.publish( marker );
05246                             ros::spinOnce();
05247                         }
05248                     }
05249                     else
05250                     {
05251                         ROS_INFO("SUCCESS: %f ", (handlePos.getOrigin() - handleHint.getOrigin()).length() );
05252                         {
05253                             visualization_msgs::Marker marker;
05254                             marker.header.frame_id = "/map";
05255                             marker.header.stamp = ros::Time::now();
05256                             marker.ns = "target base footprints";
05257                             marker.id = ++markerCnt;
05258                             marker.type = visualization_msgs::Marker::ARROW;
05259                             marker.action = visualization_msgs::Marker::ADD;
05260                             marker.pose.position.x = pose.getOrigin().x();
05261                             marker.pose.position.y = pose.getOrigin().y();
05262                             marker.pose.position.z = pose.getOrigin().z();
05263                             marker.pose.orientation.x = pose.getRotation().x();
05264                             marker.pose.orientation.y = pose.getRotation().y();
05265                             marker.pose.orientation.z = pose.getRotation().z();
05266                             marker.pose.orientation.w = pose.getRotation().w();
05267                             marker.scale.x = 0.15;
05268                             marker.scale.y = 0.55;
05269                             marker.scale.z = 0.55;
05270                             marker.color.a = 1;
05271                             marker.color.r = 0.1;
05272                             marker.color.g = 1.0;
05273                             marker.color.b = 0.1;
05274                             vis_pub.publish( marker );
05275                             ros::spinOnce();
05276                         }
05277 
05278                     }
05279 
05280                 }
05281                 else
05282                 {
05283                     {
05284                         visualization_msgs::Marker marker;
05285                         marker.header.frame_id = "/map";
05286                         marker.header.stamp = ros::Time::now();
05287                         marker.ns = "target base footprints";
05288                         marker.id = ++markerCnt;
05289                         marker.type = visualization_msgs::Marker::ARROW;
05290                         marker.action = visualization_msgs::Marker::ADD;
05291                         marker.pose.position.x = pose.getOrigin().x();
05292                         marker.pose.position.y = pose.getOrigin().y();
05293                         marker.pose.position.z = pose.getOrigin().z();
05294                         marker.pose.orientation.x = pose.getRotation().x();
05295                         marker.pose.orientation.y = pose.getRotation().y();
05296                         marker.pose.orientation.z = pose.getRotation().z();
05297                         marker.pose.orientation.w = pose.getRotation().w();
05298                         marker.scale.x = 0.15;
05299                         marker.scale.y = 0.55;
05300                         marker.scale.z = 0.55;
05301                         marker.color.a = 1;
05302                         marker.color.r = 0.1;
05303                         marker.color.g = 0.1;
05304                         marker.color.b = 0.1;
05305                         vis_pub.publish( marker );
05306                         ros::spinOnce();
05307                     }
05308                 }
05309 
05310             }
05311 
05312 
05313         }
05314     }
05315 
05316     //[ INFO] [1315570666.905808386]: AXIS OF ROTATION : 0.146734 -0.017116 0.989028, angle 0.020118
05317 
05318 
05319     /*if (atoi(argv[1]) == -607)
05320     {
05321 
05322         tf::Stamped<tf::Pose> cl_to_tool = RobotArm::getPose("/r_gripper_tool_frame","/openni_rgb_optical_frame");
05323         tf::Stamped<tf::Pose> cl_to_base = RobotArm::getPose("base_link", "/openni_rgb_optical_frame");
05324         tf::Stamped<tf::Pose> base_to_tool = RobotArm::getPose("/r_gripper_tool_frame", "/base_link");
05325         printPose("cl_to_tool", cl_to_tool);
05326         printPose("cl_to_base", cl_to_base);
05327         printPose("base_to_tool", base_to_tool);
05328 
05329         {
05330             btTransform inverted = cl_to_base.inverse() * cl_to_tool;
05331             tf::Stamped<tf::Pose> invt;
05332             invt.setOrigin(inverted.getOrigin());
05333             invt.setRotation(inverted.getRotation());
05334             printPose("base_tvia inv", inverted);
05335         }
05336 
05337         {
05338             btTransform inverted = cl_to_base* cl_to_tool.inverse();
05339             tf::Stamped<tf::Pose> invt;
05340             invt.setOrigin(inverted.getOrigin());
05341             invt.setRotation(inverted.getRotation());
05342             printPose("base_tvia inv", inverted);
05343         }
05344 
05345         {
05346             btTransform inverted = cl_to_tool * cl_to_base.inverse();
05347             tf::Stamped<tf::Pose> invt;
05348             invt.setOrigin(inverted.getOrigin());
05349             invt.setRotation(inverted.getRotation());
05350             printPose("base_tvia inv", inverted);
05351         }
05352 
05353     }*/
05354 
05355     //test ik service
05356     if (atoi(argv[1]) == -610)
05357     {
05358         //bool move_ik
05359         double x,y,z,ox,oy,oz,ow;
05360         x = atof(argv[3]);
05361         y = atof(argv[4]);
05362         z = atof(argv[5]);
05363         ox = atof(argv[6]);
05364         oy = atof(argv[7]);
05365         oz = atof(argv[8]);
05366         ow = atof(argv[9]);
05367 
05368 
05369         geometry_msgs::PoseStamped stamped_pose;
05370         stamped_pose.header.frame_id = "map";
05371         stamped_pose.header.stamp = ros::Time::now();
05372         stamped_pose.pose.position.x=x;
05373         stamped_pose.pose.position.y=y;
05374         stamped_pose.pose.position.z=z;
05375         stamped_pose.pose.orientation.x=ox;
05376         stamped_pose.pose.orientation.y=oy;
05377         stamped_pose.pose.orientation.z=oz;
05378         stamped_pose.pose.orientation.w=ow;
05379 
05380         //tf::Stamped<tf::Pose> act = RobotArm::getInstance(0)->getToolPose("base_link");
05381 
05382 
05383         //tf::poseStampedTFToMsg(act,stamped_pose);
05384 
05385         double stA[7];
05386         double stAs[7];
05387         RobotArm::getInstance(atoi(argv[2]))->getJointState(stA);
05388         int ret = RobotArm::getInstance(atoi(argv[2]))->run_ik(stamped_pose,stA,stAs,"r_wrist_roll_link");
05389 
05390 
05391         //stA[0] = std::numeric_limits<double>::quiet_NaN();
05392 
05393 
05394         ROS_INFO("RET : %i", ret);
05395         for (int k = 0; k < 7; ++k)
05396         {
05397             ROS_INFO("k %i: %f  =  %f", k, stA[k], stAs[k]);
05398         }
05399     }
05400 
05401     //test approach
05402     if (atoi(argv[1]) == -611)
05403     {
05404 
05405         tf::Stamped<tf::Pose> start, end;
05406         geometry_msgs::PoseStamped stamped_pose;
05407         double x,y,z,ox,oy,oz,ow;
05408 
05409         x = atof(argv[3]);
05410         y = atof(argv[4]);
05411         z = atof(argv[5]);
05412         ox = atof(argv[6]);
05413         oy = atof(argv[7]);
05414         oz = atof(argv[8]);
05415         ow = atof(argv[9]);
05416         stamped_pose.header.frame_id = "map";
05417         stamped_pose.header.stamp = ros::Time::now();
05418         stamped_pose.pose.position.x=x;
05419         stamped_pose.pose.position.y=y;
05420         stamped_pose.pose.position.z=z;
05421         stamped_pose.pose.orientation.x=ox;
05422         stamped_pose.pose.orientation.y=oy;
05423         stamped_pose.pose.orientation.z=oz;
05424         stamped_pose.pose.orientation.w=ow;
05425         tf::poseStampedMsgToTF(stamped_pose, start);
05426 
05427         x = atof(argv[10]);
05428         y = atof(argv[11]);
05429         z = atof(argv[12]);
05430         ox = atof(argv[13]);
05431         oy = atof(argv[14]);
05432         oz = atof(argv[15]);
05433         ow = atof(argv[16]);
05434         stamped_pose.pose.position.x=x;
05435         stamped_pose.pose.position.y=y;
05436         stamped_pose.pose.position.z=z;
05437         stamped_pose.pose.orientation.x=ox;
05438         stamped_pose.pose.orientation.y=oy;
05439         stamped_pose.pose.orientation.z=oz;
05440         stamped_pose.pose.orientation.w=ow;
05441         tf::poseStampedMsgToTF(stamped_pose, end);
05442 
05443         RobotArm::getInstance(atoi(argv[2]))->move_toolframe_ik_pose(start);
05444         Approach *apr = new Approach();
05445         apr->init(atoi(argv[2]),start, end, Approach::inside);
05446         double distA = (apr->increment(0,1));
05447 
05448         tf::Stamped<tf::Pose> endpos = RobotArm::getInstance(atoi(argv[2]))->getToolPose("map");
05449         ROS_INFO("dist %f to end %f", distA, (endpos.getOrigin() - end.getOrigin()).length() );
05450     }
05451 
05452     //output some coords in fridge_link frame
05453     if (atoi(argv[1]) == -612)
05454     {
05455         tf::Stamped<tf::Pose> act;
05456         act.frame_id_ = "/map";
05457         act.stamp_ = ros::Time::now();
05458         act.setOrigin(btVector3(0.305, -0.515,0));
05459         act.setRotation(btQuaternion(0,0,-0.348, 0.938));
05460 
05461         act = Geometry::getPoseIn("/ias_kitchen/fridge_link", act);
05462 
05463         printPose("base", act);
05464 
05465 
05466         double pts[][7] =
05467         {
05468             {0.478704, -1.0355, 1.18101, 0.767433, 0.639987, 0.022135, 0.0311955},
05469             {0.489086, -0.984206, 1.17956, 0.797904, 0.601535, 0.01726, 0.0347398},
05470             {0.494529, -0.937741, 1.1803, 0.830545, 0.555891, 0.0110758, 0.0325103},
05471             {0.504333, -0.909376, 1.18066, 0.849808, 0.526105, 0.00709967, 0.03147},
05472             {0.507886, -0.884252, 1.17954, 0.8814, 0.471274, 0.00699274, 0.0313926},
05473             {0.516993, -0.854729, 1.18006, 0.903026, 0.428457, 0.00859376, 0.0299171},
05474             {0.527833, -0.832331, 1.1803, 0.920176, 0.390256, 0.0125722, 0.0286066},
05475             {0.541463, -0.80644, 1.18, 0.931353, 0.362808, 0.0186723, 0.0245782},
05476             {0.571712, -0.760535, 1.17887, 0.936451, 0.349496, 0.024334, 0.017896},
05477             {0.608236, -0.715618, 1.17839, 0.944274, 0.327791, 0.0273483, 0.0123364},
05478             {0.647457, -0.676296, 1.17812, 0.954053, 0.298037, 0.0302956, 0.00623379},
05479             {0.690692, -0.638766, 1.17999, 0.964469, 0.262043, 0.0336022, 0.00195834},
05480             {0.734141, -0.609302, 1.18042, 0.974717, 0.220844, 0.0339708, -0.00102721},
05481             {0.781735, -0.583995, 1.17916, 0.983083, 0.180164, 0.0327274, -0.00426907},
05482             {0.828575, -0.564397, 1.17937, 0.990023, 0.137179, 0.0315954, -0.00617472},
05483             {0.870116, -0.550422, 1.17831, 0.995336, 0.0920069, 0.0283872, -0.00586025},
05484             {0.921693, -0.544899, 1.17853, 0.998734, 0.0415909, 0.0273629, -0.00714236},
05485             {0.971471, -0.549669, 1.17854, 0.998732, 0.0416648, 0.0273237, -0.00716123}
05486         };
05487 
05488         for (int k = 0; k < 72 - 55; ++k)
05489         {
05490             act.setOrigin(btVector3(pts[k][0],pts[k][1],1.35));
05491             act.setRotation(btQuaternion(pts[k][3],pts[k][4],pts[k][5],pts[k][6]));
05492             act.frame_id_ = "/map";
05493             act = Geometry::getPoseIn("/ias_kitchen/fridge_link", act);
05494             printPose("traj", act);
05495         }
05496     }
05497 
05498 // open  drawer under oven
05499     if (atoi(argv[1]) == -613)
05500     {
05501         //- Translation: [-0.137, 2.135, 0.050]
05502 //- Rotation: in Quaternion [0.001, 0.003, -0.032, 0.999]
05503         //          in RPY [0.001, 0.006, -0.064]
05504         //bin/ias_drawer_executive -3 1 .77 2.35 .84 .707 0 0 .707
05505         //bin/ias_drawer_executive -3 1 .33 2.35 .84 .707 0 0 .707
05506         //bin/ias_drawer_executive -3 1 .77 2.35 .84 .707 0 0 .707
05507 
05508 
05509     }
05510 
05511     if (atoi(argv[1]) == -714)
05512     {
05513 
05514         tf::Stamped<tf::Pose> left, right;
05515         geometry_msgs::PoseStamped stamped_pose;
05516         double x,y,z,ox,oy,oz,ow;
05517 
05518         x = atof(argv[3]);
05519         y = atof(argv[4]);
05520         z = atof(argv[5]);
05521         ox = atof(argv[6]);
05522         oy = atof(argv[7]);
05523         oz = atof(argv[8]);
05524         ow = atof(argv[9]);
05525         stamped_pose.header.frame_id = "map";
05526         stamped_pose.header.stamp = ros::Time::now();
05527         stamped_pose.pose.position.x=x;
05528         stamped_pose.pose.position.y=y;
05529         stamped_pose.pose.position.z=z;
05530         stamped_pose.pose.orientation.x=ox;
05531         stamped_pose.pose.orientation.y=oy;
05532         stamped_pose.pose.orientation.z=oz;
05533         stamped_pose.pose.orientation.w=ow;
05534         tf::poseStampedMsgToTF(stamped_pose, left);
05535         printPose("left", left);
05536 
05537         x = atof(argv[10]);
05538         y = atof(argv[11]);
05539         z = atof(argv[12]);
05540         ox = atof(argv[13]);
05541         oy = atof(argv[14]);
05542         oz = atof(argv[15]);
05543         ow = atof(argv[16]);
05544         stamped_pose.header.frame_id = "map";
05545         stamped_pose.header.stamp = ros::Time::now();
05546         stamped_pose.pose.position.x=x;
05547         stamped_pose.pose.position.y=y;
05548         stamped_pose.pose.position.z=z;
05549         stamped_pose.pose.orientation.x=ox;
05550         stamped_pose.pose.orientation.y=oy;
05551         stamped_pose.pose.orientation.z=oz;
05552         stamped_pose.pose.orientation.w=ow;
05553         tf::poseStampedMsgToTF(stamped_pose, right);
05554         printPose("left", right);
05555 
05556         findBothArms(left, right);
05557     }
05558 
05559 
05560     if (atoi(argv[1]) == -715)
05561     {
05562 
05563         RobotArm::getInstance(0)->raise_elbow = true;
05564         RobotArm::getInstance(1)->raise_elbow = true;
05565 
05566         tf::Stamped<tf::Pose> left, right;
05567         geometry_msgs::PoseStamped stamped_pose;
05568         double x,y,z,ox,oy,oz,ow;
05569 
05570         x = atof(argv[3]);
05571         y = atof(argv[4]);
05572         z = atof(argv[5]);
05573         ox = atof(argv[6]);
05574         oy = atof(argv[7]);
05575         oz = atof(argv[8]);
05576         ow = atof(argv[9]);
05577         stamped_pose.header.frame_id = "map";
05578         stamped_pose.header.stamp = ros::Time::now();
05579         stamped_pose.pose.position.x=x;
05580         stamped_pose.pose.position.y=y;
05581         stamped_pose.pose.position.z=z;
05582         stamped_pose.pose.orientation.x=ox;
05583         stamped_pose.pose.orientation.y=oy;
05584         stamped_pose.pose.orientation.z=oz;
05585         stamped_pose.pose.orientation.w=ow;
05586         tf::poseStampedMsgToTF(stamped_pose, left);
05587         printPose("left", left);
05588 
05589         x = atof(argv[10]);
05590         y = atof(argv[11]);
05591         z = atof(argv[12]);
05592         ox = atof(argv[13]);
05593         oy = atof(argv[14]);
05594         oz = atof(argv[15]);
05595         ow = atof(argv[16]);
05596         stamped_pose.header.frame_id = "map";
05597         stamped_pose.header.stamp = ros::Time::now();
05598         stamped_pose.pose.position.x=x;
05599         stamped_pose.pose.position.y=y;
05600         stamped_pose.pose.position.z=z;
05601         stamped_pose.pose.orientation.x=ox;
05602         stamped_pose.pose.orientation.y=oy;
05603         stamped_pose.pose.orientation.z=oz;
05604         stamped_pose.pose.orientation.w=ow;
05605         tf::poseStampedMsgToTF(stamped_pose, right);
05606         printPose("left", right);
05607 
05608         RobotArm::moveBothArms(left, right);
05609     }
05610 
05611 
05612 
05613 }
05614 
05615 /*
05616 #include <pcl/kdtree/kdtree_flann.h>
05617 
05618   // Dummy point clouds
05619   PointCloud<PointXYZ>::Ptr cloud1 (new PointCloud<PointXYZ> ());
05620   PointCloud<PointXYZ>::Ptr cloud2 (new PointCloud<PointXYZ> ());
05621 
05622   // KD-Tree
05623   KdTree<PointXYZ>::Ptr tree;
05624   tree.reset (new KdTreeFLANN<PointXYZ>);
05625   tree->setInputCloud (cloud1);
05626 
05627   // Allocate enough space to hold the results
05628   // \note resize is irrelevant for a radiusSearch ().
05629   std::vector<int> nn_indices (1);
05630   std::vector<double> nn_sqr_dists (1);
05631 
05632   // Search
05633   iterate over cloud2->points
05634   {
05635     // tree->*Search returns the number of found points
05636 
05637     if (tree->nearestKSearch (p, 1, nn_indices, nn_sqr_dists) != 0)
05638     {
05639       // there are neighbors! see nn_indices, nn_sqr_dists
05640     }
05641 
05642     // alternatively:
05643 
05644     if (tree->radiusSearch (p, distance_threshold, nn_indices,
05645 nn_sqr_dists, 1) != 0)
05646     {
05647       // there are neighbors! see nn_indices, nn_sqr_dists
05648     }
05649   }
05650 */
05651 
05652 /*
05653 t time 1318337516.221
05654 - Translation: [0.844, 2.022, 1.402]
05655 - Rotation: in Quaternion [1.000, 0.023, -0.015, -0.011]
05656             in RPY [-3.120, 0.029, 0.046]
05657 ^CAt time 1318337516.720
05658 - Translation: [0.844, 2.022, 1.402]
05659 - Rotation: in Quaternion [1.000, 0.023, -0.015, -0.010]
05660             in RPY [-3.120, 0.030, 0.046]
05661 ruehr@pr2b:~/sshsandbox/ias_drawer_executive$ rosrun tf tf_echo map r_gripper_tool_frame
05662 At time 1318337545.784
05663 - Translation: [0.832, 2.030, 1.411]
05664 - Rotation: in Quaternion [-0.695, -0.013, 0.013, 0.719]
05665             in RPY [-1.537, -0.000, 0.036]
05666 ^CAt time 1318337546.149
05667 - Translation: [0.832, 2.030, 1.410]
05668 - Rotation: in Quaternion [-0.695, -0.013, 0.013, 0.719]
05669             in RPY [-1.537, 0.000, 0.036]
05670 */
 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:57