high_five.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *  \author Joe Romano
00036  *********************************************************************/
00037 //@author  Joe Romano
00038 //@email   joeromano@gmail.com
00039 //@brief   high_five.cpp - pr2 gives you props yo
00040 
00041 #include <ros/ros.h>
00042 #include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
00043 #include <pr2_gripper_sensor_msgs/PR2GripperEventDetectorAction.h>
00044 #include <actionlib/client/simple_action_client.h>
00045 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00046 
00047 typedef actionlib::SimpleActionClient< pr2_controllers_msgs::JointTrajectoryAction > TrajClient;
00048 // Our Action interface type, provided as a typedef for convenience                   
00049 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperEventDetectorAction> PlaceClient;
00050 // Our Action interface type, provided as a typedef for convenience                   
00051 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> GripperClient;
00052 // Our Action interface type, provided as a typedef for convenience                   
00053 
00054 
00055 
00056 class RobotArm
00057 {
00058 private:
00059   // Action client for the joint trajectory action 
00060   // used to trigger the arm movement action
00061   TrajClient* traj_client_r_;
00062   TrajClient* traj_client_l_;
00063 
00064 public:
00066   RobotArm() 
00067   {
00068     // tell the action client that we want to spin a thread by default
00069     traj_client_r_ = new TrajClient("r_arm_controller/joint_trajectory_action", true);
00070     traj_client_l_ = new TrajClient("l_arm_controller/joint_trajectory_action", true);
00071 
00072     // wait for action server to come up
00073     while(!traj_client_l_->waitForServer(ros::Duration(5.0))){
00074       ROS_INFO("Waiting for the joint_trajectory_action server");
00075     }
00076     // wait for action server to come up
00077     while(!traj_client_r_->waitForServer(ros::Duration(5.0))){
00078       ROS_INFO("Waiting for the joint_trajectory_action server");
00079     }
00080   }
00081 
00083   ~RobotArm()
00084   {
00085     delete traj_client_r_;
00086     delete traj_client_l_;
00087   }
00088 
00090   void startTrajectory(pr2_controllers_msgs::JointTrajectoryGoal goal,bool right_arm)
00091   {
00092     //Start the trjaectory immediately
00093     goal.trajectory.header.stamp = ros::Time::now();
00094 
00095     if(right_arm)
00096       traj_client_r_->sendGoal(goal);
00097     else
00098       traj_client_l_->sendGoal(goal);
00099   }
00100 
00102   pr2_controllers_msgs::JointTrajectoryGoal arm_trajectoryPoint(float* angles, float duration, bool right_arm)
00103   {
00104     //our goal variable
00105     pr2_controllers_msgs::JointTrajectoryGoal goal;
00106 
00107     // First, the joint names, which apply to all waypoints
00108     //starts at 17
00109     if(right_arm)
00110     {
00111       goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
00112       goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
00113       goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
00114       goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
00115       goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
00116       goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
00117       goal.trajectory.joint_names.push_back("r_wrist_roll_joint");
00118     }
00119     else
00120     {
00121       goal.trajectory.joint_names.push_back("l_shoulder_pan_joint");
00122       goal.trajectory.joint_names.push_back("l_shoulder_lift_joint");
00123       goal.trajectory.joint_names.push_back("l_upper_arm_roll_joint");
00124       goal.trajectory.joint_names.push_back("l_elbow_flex_joint");
00125       goal.trajectory.joint_names.push_back("l_forearm_roll_joint");
00126       goal.trajectory.joint_names.push_back("l_wrist_flex_joint");
00127       goal.trajectory.joint_names.push_back("l_wrist_roll_joint");
00128     }
00129 
00130     // We will have N waypoints in this goal trajectory
00131     goal.trajectory.points.resize(1);
00132 
00133     // First trajectory point
00134     // Positions
00135     int ind = 0;
00136     goal.trajectory.points[ind].positions.resize(7);
00137     goal.trajectory.points[ind].positions[0] =  angles[0];
00138     goal.trajectory.points[ind].positions[1] =  angles[1];
00139     goal.trajectory.points[ind].positions[2] =  angles[2];
00140     goal.trajectory.points[ind].positions[3] =  angles[3];
00141     goal.trajectory.points[ind].positions[4] =  angles[4];
00142     goal.trajectory.points[ind].positions[5] =  angles[5];
00143     goal.trajectory.points[ind].positions[6] =  angles[6];
00144     // Velocities
00145     goal.trajectory.points[ind].velocities.resize(7);
00146     for (size_t j = 0; j < 7; ++j)
00147     {
00148       goal.trajectory.points[ind].velocities[j] = 0.0;
00149     }
00150     // set time we want this trajectory to be reached at
00151     goal.trajectory.points[ind].time_from_start = ros::Duration(duration);
00152 
00153     //we are done; return the goal
00154     return goal;
00155   }
00156 
00157 
00159   //actionlib::SimpleClientGoalState getState()
00160   //{
00161   //  return traj_client_->getState();
00162   //}
00163 };
00164 
00165 
00166 class Gripper{
00167 private:
00168   GripperClient* gripper_client_r_, *gripper_client_l_;  
00169   PlaceClient* place_client_r_, *place_client_l_;
00170 
00171 
00172 public:
00173   //Action client initialization
00174   Gripper(){
00175 
00176     //Initialize the client for the Action interface to the gripper controller
00177     //and tell the action client that we want to spin a thread by default
00178     gripper_client_l_  = new GripperClient("l_gripper_sensor_controller/gripper_action",true);
00179     place_client_l_  = new PlaceClient("l_gripper_sensor_controller/event_detector",true);
00180     gripper_client_r_  = new GripperClient("r_gripper_sensor_controller/gripper_action",true);
00181     place_client_r_  = new PlaceClient("r_gripper_sensor_controller/event_detector",true);
00182         
00183     //wait for the gripper action server to come up 
00184     while(!gripper_client_r_->waitForServer(ros::Duration(5.0))){
00185       ROS_INFO("Waiting for the r_gripper_sensor_controller/gripper_action action server to come up");
00186     }
00187 
00188     while(!place_client_r_->waitForServer(ros::Duration(5.0))){
00189       ROS_INFO("Waiting for the r_gripper_sensor_controller/event_detector action server to come up");
00190     }    
00191 
00192     //wait for the gripper action server to come up 
00193     while(!gripper_client_l_->waitForServer(ros::Duration(5.0))){
00194       ROS_INFO("Waiting for the l_gripper_sensor_controller/gripper_action action server to come up");
00195     }
00196 
00197     while(!place_client_l_->waitForServer(ros::Duration(5.0))){
00198       ROS_INFO("Waiting for the l_gripper_sensor_controller/event_detector action server to come up");
00199     }    
00200 
00201 
00202   }
00203 
00204   ~Gripper(){
00205     delete gripper_client_l_;
00206     delete gripper_client_r_;
00207     delete place_client_l_;
00208     delete place_client_r_;
00209 
00210 
00211   }
00212 
00213   
00214 
00215 
00216   //Open the gripper
00217   void open(bool left, bool right){
00218     pr2_controllers_msgs::Pr2GripperCommandGoal open;
00219     open.command.position = 0.09;    // position open (9 cm)
00220     open.command.max_effort = -1.0;  // Do not limit effort (negative)
00221     
00222     if(left)
00223       gripper_client_l_->sendGoal(open);
00224     if(right)
00225       gripper_client_r_->sendGoal(open);
00226   }
00227 
00228   //Close the gripper
00229   void close(bool left, bool right,bool wait_for_result){
00230     pr2_controllers_msgs::Pr2GripperCommandGoal close;
00231     close.command.position = 0.002;    // position open (9 cm)
00232     close.command.max_effort = -1.0;  // Do not limit effort (negative)
00233     
00234     if(left)
00235     {
00236       gripper_client_l_->sendGoal(close);
00237       if(wait_for_result)
00238         gripper_client_l_->waitForResult();
00239     }
00240     if(right)
00241     {
00242       gripper_client_r_->sendGoal(close);
00243       if(wait_for_result)
00244         gripper_client_r_->waitForResult();
00245     }
00246   }
00247 
00248 
00249   //move into place mode to drop an object
00250   void slap(bool left, bool right){
00251     pr2_gripper_sensor_msgs::PR2GripperEventDetectorGoal place_goal;
00252     place_goal.command.trigger_conditions = 4;  // use just acceleration as our contact signal
00253     place_goal.command.acceleration_trigger_magnitude = 6.0;  // m/^2
00254     place_goal.command.slip_trigger_magnitude = 0.008;        // slip gain
00255 
00256     if(left)
00257       place_client_l_->sendGoal(place_goal);
00258     if(right)
00259       place_client_r_->sendGoal(place_goal);
00260   }
00261 
00263   bool slapDone(bool left, bool right)
00264   {
00265     if(left && right)
00266       return place_client_l_->getState().isDone() && place_client_r_->getState().isDone();
00267     else if(left)
00268       return place_client_l_->getState().isDone();
00269     else if(right)
00270       return place_client_r_->getState().isDone();
00271     return false;
00272   }  
00273 };
00274 
00275 int main(int argc, char** argv){
00276   ros::init(argc, argv, "lift_test");
00277 
00278   // figure out what kind of five we want
00279   bool left = false;
00280   bool right = true;
00281   if(argc > 1)
00282   {
00283     left =  (strcmp(argv[1],"left") == 0) || (strcmp(argv[1],"double") == 0);
00284     right = (strcmp(argv[1],"right") == 0) || (strcmp(argv[1],"double") == 0);
00285   }
00286 
00287   RobotArm arm;
00288   Gripper gripper;
00289 
00290   gripper.close(left,right,true);
00291   
00292     float pre_five_r []= {-1.4204039529994779, 0.64014688694872024, 0.64722849826846929, -1.9254939970572147, 30.931365914354672, -0.52166442520665446, -16.642483924162743 };
00293     float pre_five_l []= {1.544791237364544, 0.028669297805660576, -0.061946460502633194, -1.9322034349027488, -0.96915535302752587, -0.22688029069077575, -3.5411348633607669};
00294     if(left)
00295       arm.startTrajectory(arm.arm_trajectoryPoint(pre_five_l,2.0,false),false);
00296     if(right)
00297       arm.startTrajectory(arm.arm_trajectoryPoint(pre_five_r,2.0,true),true);
00298     sleep(2.0);
00299 
00300     float five_r []= {-0.43912122996219438, -0.26865637196243625, -0.06073806015457861, -1.0597651429837187, 30.217764249732564, -0.098888248598895112, -17.139399300620212 };
00301     float five_l []= {0.34795130919909756, -0.33483508677418122, 0.21450526015905091, -0.87161320330702452, -0.55300340950519367, -0.51138511922202357, -3.1737890509598912};
00302     if(left)
00303       arm.startTrajectory(arm.arm_trajectoryPoint(five_l,1.25,false),false);
00304     if(right)
00305       arm.startTrajectory(arm.arm_trajectoryPoint(five_r,1.25,true),true);
00306 
00307     // now start looking for a slap during the move
00308     gripper.slap(left,right);
00309 
00310 
00311     //wait for a slap
00312     while(!gripper.slapDone(left,right) && ros::ok())
00313     {
00314       ros::Duration(0.005).sleep();
00315     }
00316 
00317     float post_five_r []= {-1.2271486279403441, 0.98994689398564029, 0.27937452657595019, -1.9855738422813785, 31.095246711685615, -0.75469820126008202, -17.206098475132887 };
00318     float post_five_l []= {1.458651261930636, 1.0444005395208478, 0.081571109098415029, -2.1115743463069396, -1.3390296269894097, -0.16823026639652239, -3.5006715676681437};
00319     if(left)
00320       arm.startTrajectory(arm.arm_trajectoryPoint(post_five_l,1.35,false),false);
00321     if(right)
00322       arm.startTrajectory(arm.arm_trajectoryPoint(post_five_r,1.35,true),true);
00323 
00324   return 0;
00325 }


pr2_props
Author(s): Joe Romano
autogenerated on Fri Aug 28 2015 12:13:27