Gripper.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 
00031 #include <ias_drawer_executive/Gripper.h>
00032 #include <ias_drawer_executive/Geometry.h>
00033 
00034 #include <tf/transform_listener.h>
00035 #include <string>
00036 #include <boost/thread.hpp>
00037 
00038 Gripper::Gripper(int side){
00039 
00040     side_ = side;
00041 
00042     std::string sideletter((side_ == 0) ? "r" : "l");
00043     //Initialize the client for the Action interface to the gripper controller
00044     //and tell the action client that we want to spin a thread by default
00045     //gripper_client_ = new GripperClient(side ? "l_gripper_controller/gripper_action" : "r_gripper_controller/gripper_action", true);
00046 
00047     gripper_client_ = new GripperClient(side ? "l_gripper_controller/gripper_action" : "r_gripper_controller/gripper_action", true);
00048     //    grab_ = new GrabAC("/"+ sideletter + "_gripper_sensor_controller/grab", true);
00049     //  contact_ = new FindContactAC("/"+ sideletter + "_gripper_sensor_controller/find_contact", true);
00050 
00051     //wait for the gripper action server to come up
00052     while(ros::ok() && !gripper_client_->waitForServer(ros::Duration(5.0))){
00053       ROS_INFO((side_ == 0) ? "Waiting for the r_gripper_controller/gripper_action action server to come up":  "Waiting for the l_gripper_controller/gripper_action action server to come up");
00054     }
00055   }
00056 
00057 Gripper::~Gripper(){
00058     delete gripper_client_;
00059 }
00060 
00061 double Gripper::getAmountOpen(){
00062     tf::Stamped<tf::Pose> trans;
00063     if (side_ == 0)
00064        trans = Geometry::getRelativeTransform("/r_gripper_l_finger_tip_link", "/r_gripper_r_finger_tip_link");
00065     else
00066        trans = Geometry::getRelativeTransform("/l_gripper_l_finger_tip_link", "/l_gripper_r_finger_tip_link");
00067 
00068     return trans.getOrigin().length() - .029245 ; // offset of links to surface' - 0.032162;
00069 }
00070 
00071 //Open the gripper
00072 void Gripper::openThreaded(double amount)
00073 {
00074     boost::thread(&Gripper::open, this, amount);
00075 }
00076 
00077 void Gripper::open(double amount){
00078     pr2_controllers_msgs::Pr2GripperCommandGoal open;
00079     //open.command.position = 0.085 * amount;
00080     open.command.position = amount;
00081     open.command.max_effort = -1.0;  // Do not limit effort (negative)
00082 
00083     gripper_client_->sendGoal(open);
00084     gripper_client_->waitForResult();
00085     if(gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00086       ROS_INFO("The gripper opened!");
00087     else
00088       ROS_INFO("The gripper failed to open.");
00089   }
00090 
00091 //Close the gripper
00092 void Gripper::closeThreaded(double amount)
00093 {
00094     boost::thread(&Gripper::close, this, amount);
00095 }
00096 
00097 
00098 void Gripper::close(double amount){
00099     pr2_controllers_msgs::Pr2GripperCommandGoal squeeze;
00100     //squeeze.command.position = 0.085 - (amount * 0.085);
00101     squeeze.command.position = amount;
00102     squeeze.command.max_effort = 150.0;  // Close hard
00103 
00104     gripper_client_->sendGoal(squeeze);
00105     gripper_client_->waitForResult();
00106     if(gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00107       ROS_INFO("The gripper closed!");
00108     else
00109       ROS_INFO("The gripper failed to close.");
00110   }
00111 
00112     //Close the gripper
00113 void Gripper::closeHard(double amount){
00114     pr2_controllers_msgs::Pr2GripperCommandGoal squeeze;
00115     //squeeze.command.position = 0.085 - (amount * 0.085);
00116     squeeze.command.position = amount;
00117     squeeze.command.max_effort = -1.0;  // Close hard = do not limit force
00118 
00119     gripper_client_->sendGoal(squeeze);
00120     gripper_client_->waitForResult();
00121     if(gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00122       ROS_INFO("The gripper closed!");
00123     else
00124       ROS_INFO("The gripper failed to close.");
00125   }
00126 
00127 Gripper *Gripper::instance[] = {0,0};
00128 
00129 Gripper* Gripper::getInstance(int side) {
00130     if (!instance[side]) {
00131         instance[side] = new Gripper(side);
00132     }
00133     return instance[side];
00134 }
00135 
00136 
00137 void Gripper::updatePressureZero(){
00138   //ros::service::call((side_==0) ? "/r_gripper_sensor_controller/update_zeros" : "/l_gripper_sensor_controller/update_zeros", serv);
00139 }
00140 
00141 void Gripper::closeCompliant(double gain){
00142   ros::service::call((side_==0) ? "/r_reactive_grasp/compliant_close" : "/l_reactive_grasp/compliant_close", serv);
00143   //typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperGrabAction>    GrabAC;
00144   //GrabAC grab("/"+ side + "_gripper_sensor_controller/grab", true);
00145 
00146   /*while(!contact_->waitForServer())
00147     {
00148       ROS_INFO("Waiting for the /%s_gripper_sensor_controller/findcontact to come up", ((side_ == 0) ? "r" : "l"));
00149     }
00150   pr2_gripper_sensor_msgs::PR2GripperFindContactGoal fc_goal;
00151   fc_goal.command.contact_conditions = 0; // 0 both 1 left 2 right 3 either
00152   fc_goal.command.zero_fingertip_sensors = true;
00153   contact_->sendGoal(fc_goal);
00154   contact_->waitForResult(ros::Duration(5.0));
00155   if (contact_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00156     ROS_INFO("/%s_gripper_sensor_controller/findcontact SUCCEEDED", ((side_ == 0) ? "r" : "l"));
00157   else
00158     ROS_ERROR("/%s_gripper_sensor_controller/findcontact FAILED", ((side_ == 0) ? "r" : "l"));*/
00159 
00160   /*while(!grab_->waitForServer())
00161     {
00162       ROS_INFO("Waiting for the /%s_gripper_sensor_controller/grab to come up", ((side_ == 0) ? "r" : "l"));
00163     }
00164   pr2_gripper_sensor_msgs::PR2GripperGrabGoal grab_goal;
00165   grab_goal.command.hardness_gain=gain;
00166   ROS_INFO("sending grab goal");
00167   grab_->sendGoal(grab_goal);
00168   grab_->waitForResult(ros::Duration(5.0));
00169   if (grab_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00170     ROS_INFO("/%s_gripper_sensor_controller/grab SUCCEEDED", ((side_ == 0) ? "r" : "l"));
00171   else
00172     ROS_ERROR("/%s_gripper_sensor_controller/grab FAILED", ((side_ == 0) ? "r" : "l"));*/
00173   //close();
00174 }
00175 
 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