Approach.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 #include <boost/thread.hpp>
00031 
00032 
00033 #include <ias_drawer_executive/OperateHandleController.h>
00034 #include <ias_drawer_executive/RobotArm.h>
00035 #include <ias_drawer_executive/Geometry.h>
00036 #include <ias_drawer_executive/Gripper.h>
00037 #include <ias_drawer_executive/Pressure.h>
00038 #include <ias_drawer_executive/Poses.h>
00039 #include <ias_drawer_executive/AverageTF.h>
00040 #include <ias_drawer_executive/Torso.h>
00041 #include <ias_drawer_executive/Approach.h>
00042 
00043 #include <cop_client_cpp/cop_client.h>
00044 
00045 
00046 void Approach::init(int side, tf::Stamped<tf::Pose> approachStart, tf::Stamped<tf::Pose> plateCenter, Approach::SensorArea sensors)
00047 {
00048     sensors_ = sensors;
00049 
00050     ros::service::call((side_==0) ? "/r_gripper_sensor_controller/update_zeros" : "/l_gripper_sensor_controller/update_zeros", serv);
00051 
00052     side_ = side;
00053     arm = RobotArm::getInstance(side);
00054     pressure = Pressure::getInstance(side);
00055 
00056     touched = false;
00057     firstTouch = 0;
00058 
00059     approachStart.stamp_=ros::Time();
00060     plateCenter.stamp_=ros::Time();
00061 
00062     startPose = Geometry::getPoseIn("map",approachStart);
00063 
00064     plateCenter=Geometry::getPoseIn("map",plateCenter);
00065     //plateCenter.setOrigin(plateCenter.getOrigin() + btVector3(0,side ? -.15 : .15,0.0f));
00066     diff = plateCenter.getOrigin() - startPose.getOrigin(); // we define a line (pMa, diff) in map frame here that is used for the approach
00067     //like that: p.setOrigin(pMa.getOrigin() + diff * ap);
00068     ROS_INFO("DIFF %f %f %f", diff.x(), diff.y(), diff.z());
00069     // approach
00070     arm->time_to_target = 10;
00071 
00072     pressureDiff = 1000;
00073 }
00074 
00075 double Approach::increment(double st, double ap)
00076 {
00077 
00078     tf::Stamped<tf::Pose> p = startPose;
00079     p.setOrigin(startPose.getOrigin() + diff * ap);
00080 
00081     tf::Stamped<tf::Pose> curr = startPose;
00082     curr.setOrigin(startPose.getOrigin() + diff * st);
00083 
00084     //arm->moveElbowOutOfWay(curr);
00085 
00086     ROS_INFO("START SIDE %i , %f %f %f, %f %f %f %f",side_,startPose.getOrigin().x(),startPose.getOrigin().y(),startPose.getOrigin().z(),
00087              startPose.getRotation().x(),startPose.getRotation().y(),startPose.getRotation().z(),startPose.getRotation().w());
00088     ROS_INFO("GOAL SIDE %i , %f %f %f, %f %f %f %f",side_,p.getOrigin().x(),p.getOrigin().y(),p.getOrigin().z(),
00089              p.getRotation().x(),p.getRotation().y(),p.getRotation().z(),p.getRotation().w());
00090 
00091     p = Geometry::getPoseIn("base_link",p);
00092 
00093     //arm->time_to_target = 1;
00094 
00095     //arm->move_toolframe_ik(startPose.getOrigin().x(),startPose.getOrigin().y(),startPose.getOrigin().z(),
00096     //   startPose.getRotation().x(),startPose.getRotation().y(),startPose.getRotation().z(),startPose.getRotation().w());
00097 
00098     arm->time_to_target = 10;
00099 
00100     arm->retries = 3;
00101 
00102     boost::thread t1(&RobotArm::move_toolframe_ik,arm,p.getOrigin().x(),p.getOrigin().y(),p.getOrigin().z(),
00103                      p.getRotation().x(),p.getRotation().y(),p.getRotation().z(),p.getRotation().w());
00104 
00105     ros::Rate rate(40.0);
00106     double pl, pr;
00107     //pressure->getInside(pl,pr,false);
00108     switch (sensors_)
00109     {
00110     case Approach::inside :
00111         pressure->getInside(pl,pr, false);
00112         break;
00113     case Approach::front :
00114         pressure->getFront(pl,pr, false);
00115         break;
00116     default:
00117         ROS_ERROR("Approach::init got an unknown value %i for sensors", sensors_);
00118     }
00119 
00120     while (ros::ok() && arm->getActionClient()->getState() !=  actionlib::SimpleClientGoalState::ACTIVE)
00121     {
00122         ros::spinOnce();
00123         rate.sleep();
00124     }
00125 
00126     double tl,tr;
00127 
00128     switch (sensors_)
00129     {
00130     case Approach::inside :
00131         pressure->getInsideTouched(tl,tr);
00132         break;
00133     case Approach::front :
00134         pressure->getFrontTouched(tl,tr);
00135         break;
00136     default:
00137         ROS_ERROR("Approach::init got an unknown value %i for sensors", sensors_);
00138     }
00139 
00140 
00141     std::vector<double> distances;
00142 
00143     double sumavgs = 0;
00144     double numavgs = 0;
00145 
00146     bool touched = false;
00147     int cart_err_hit = 0;
00148     while (ros::ok()  && (arm->getActionClient()->getState() ==  actionlib::SimpleClientGoalState::ACTIVE))
00149     {
00150         double cl, cr;
00151         //pressure->getInside(cl,cr,false);
00152         switch (sensors_)
00153         {
00154         case Approach::inside :
00155             pressure->getInsideTouched(cl,cr);
00156             break;
00157         case Approach::front :
00158             pressure->getFrontTouched(cl,cr);
00159             break;
00160         default:
00161             ROS_ERROR("Approach::init got an unknown value %i for sensors", sensors_);
00162         }
00163         // ROS_INFO("STATUS %s", arm->getActionClient()->getState().toString().c_str());
00164         ROS_INFO("TOUCHES %i %f", side_,  (cl + cr) - (tl + tr));
00165         btVector3 cartErr = arm->cartError();
00166 
00167 
00168         btVector3 direction = (plateCenter.getOrigin() - startPose.getOrigin()).normalize();
00169         double projected = cartErr.dot(direction);
00170 
00171         distances.push_back(projected);
00172 
00173         double avg = 0;
00174 
00175         if (distances.size() > 5) {
00176             double distsum = 0;
00177             for (int k = distances.size() - 6; k < distances.size(); ++k) {
00178                 distsum += distances[k];
00179             }
00180             distsum /= 5;
00181             avg = distsum;
00182             sumavgs += fabs(avg);
00183             numavgs += 1;
00184         }
00185 
00186         double relative = 0;
00187         if (numavgs > 2) {
00188             double sumnum = sumavgs / numavgs;
00189             relative = avg / sumnum;
00190             ROS_ERROR("%f / \t %f = \t %f", avg ,sumnum, avg / sumnum);
00191         }
00192 
00193 
00194         ROS_INFO("CArt error : magn %f \t %f %f %f proj : \t %f \t avg: %f ", cartErr.length(), cartErr.x(), cartErr.y(), cartErr.z(), projected, avg);
00195 
00196         if (relative < -3)
00197           cart_err_hit++;
00198 
00199         if ((cl + cr > tl + tr + 5) || (cart_err_hit > 3)) // || cartErr.length() > 0.001)
00200         {
00201             ROS_INFO("SIDE %i TOUCHED press %i", side_, pressure->side_);
00202             ROS_INFO("TOUCHES %i %f", side_,  (cl + cr) - (tl + tr));
00203             arm->getActionClient()->cancelGoal();
00204             touched = true;
00205             break;
00206         }
00207 
00208         ros::spinOnce();
00209         rate.sleep();
00210     }
00211 
00212     tf::Stamped<tf::Pose> actPose;
00213     arm->getToolPose(actPose,"map");
00214 
00215     double actDiff = btVector3(startPose.getOrigin() - actPose.getOrigin()).length();
00216     ROS_INFO("DISTANCE FROM START %f" , actDiff);
00217 
00218     arm->retries = 0;
00219 
00220     if (touched)
00221         return actDiff;
00222     else
00223         return 0;
00224 
00225 }
00226 
00227 void Approach::move_to(double ap)
00228 {
00229 
00230     tf::Stamped<tf::Pose> p = startPose;
00231     p.setOrigin(startPose.getOrigin() + diff * ap);
00232 
00233     arm->time_to_target = 1;
00234 
00235     arm->universal_move_toolframe_ik(p.getOrigin().x(),p.getOrigin().y(),p.getOrigin().z(),
00236                                      p.getRotation().x(),p.getRotation().y(),p.getRotation().z(),p.getRotation().w(),"map");
00237 }
00238 
00239 
00240 bool Approach::finish()
00241 {
00242     arm->time_to_target = 1;
00243 
00244     //ros::service::call((side_==0) ? "/r_reactive_grasp/compliant_close" : "/l_reactive_grasp/compliant_close", serv);
00245 
00246     //ros::service::call("/r_reactive_grasp/compliant_close" , serv);
00247     Gripper::getInstance(side_)->closeCompliant();
00248 
00249     Gripper::getInstance(side_)->close();
00250     return false;
00251 }
00252 
00253 void Lift::init(int side)
00254 {
00255     side_ = side;
00256     arm = RobotArm::getInstance(side);
00257 
00258     tf::Stamped<tf::Pose> actPose;
00259     const char fixed_frame[] = "torso_lift_link";
00260     arm->getToolPose(actPose,fixed_frame);
00261 
00262     // for the case that we change the elbow angle, reposition the tool at current pose with given elbow angle
00263     //for popcorn: arm->universal_move_toolframe_ik_pose(actPose);
00264     //Pressure *pressure = Pressure::getInstance(side);
00265     //pressure->reset();
00266 
00267     plateCenter= actPose;
00268     plateCenter.setOrigin(plateCenter.getOrigin() + btVector3(0,0,1));
00269     diff = plateCenter.getOrigin() - actPose.getOrigin(); // we define a line (pMa, diff) in map frame here that is used for the approach
00270     diffToRob = btVector3(-1,0,0);
00271     //like that: p.setOrigin(pMa.getOrigin() + diff * ap);
00272     startPose = actPose;
00273     // approach
00274     // arm->time_to_target = 0.25;
00275     arm->time_to_target = 1;
00276 }
00277 
00278 //void Lift::increment(double up, double back)
00279 void Lift::increment(double x, double y, double z)
00280 {
00281     ros::spinOnce();
00282     tf::Stamped<tf::Pose> p = startPose;
00283     //p.setOrigin(startPose.getOrigin() + diff * z + diffToRob * back);
00284     p.setOrigin(startPose.getOrigin() + btVector3(x,y,z));
00285     //arm->universal_move_toolframe_ik_pose(p);
00286     arm->move_toolframe_ik_pose(p);
00287 }
00288 


ias_drawer_executive
Author(s): Thomas Ruehr
autogenerated on Mon Oct 6 2014 08:59:20