00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
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
00049 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperEventDetectorAction> PlaceClient;
00050
00051 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> GripperClient;
00052
00053
00054
00055
00056 class RobotArm
00057 {
00058 private:
00059
00060
00061 TrajClient* traj_client_r_;
00062 TrajClient* traj_client_l_;
00063
00064 public:
00066 RobotArm()
00067 {
00068
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
00073 while(!traj_client_l_->waitForServer(ros::Duration(5.0))){
00074 ROS_INFO("Waiting for the joint_trajectory_action server");
00075 }
00076
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
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
00105 pr2_controllers_msgs::JointTrajectoryGoal goal;
00106
00107
00108
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
00131 goal.trajectory.points.resize(1);
00132
00133
00134
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
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
00151 goal.trajectory.points[ind].time_from_start = ros::Duration(duration);
00152
00153
00154 return goal;
00155 }
00156
00157
00159
00160
00161
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
00174 Gripper(){
00175
00176
00177
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
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
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
00217 void open(bool left, bool right){
00218 pr2_controllers_msgs::Pr2GripperCommandGoal open;
00219 open.command.position = 0.09;
00220 open.command.max_effort = -1.0;
00221
00222 if(left)
00223 gripper_client_l_->sendGoal(open);
00224 if(right)
00225 gripper_client_r_->sendGoal(open);
00226 }
00227
00228
00229 void close(bool left, bool right,bool wait_for_result){
00230 pr2_controllers_msgs::Pr2GripperCommandGoal close;
00231 close.command.position = 0.002;
00232 close.command.max_effort = -1.0;
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
00250 void slap(bool left, bool right){
00251 pr2_gripper_sensor_msgs::PR2GripperEventDetectorGoal place_goal;
00252 place_goal.command.trigger_conditions = 4;
00253 place_goal.command.acceleration_trigger_magnitude = 6.0;
00254 place_goal.command.slip_trigger_magnitude = 0.008;
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
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
00291 gripper.close(left,right,true);
00292
00293 while(ros::ok())
00294 {
00295 float pre_five_r []= {-1.4204039529994779, 0.64014688694872024, 0.64722849826846929, -1.9254939970572147, 30.931365914354672, -0.52166442520665446, -16.642483924162743 };
00296 float pre_five_l []= {1.544791237364544, 0.028669297805660576, -0.061946460502633194, -1.9322034349027488, -0.96915535302752587, -0.22688029069077575, -3.5411348633607669};
00297 if(left)
00298 arm.startTrajectory(arm.arm_trajectoryPoint(pre_five_l,2.0,false),false);
00299 if(right)
00300 arm.startTrajectory(arm.arm_trajectoryPoint(pre_five_r,2.0,true),true);
00301 sleep(2.0);
00302
00303 float five_r []= {-0.43912122996219438, -0.26865637196243625, -0.06073806015457861, -1.0597651429837187, 30.217764249732564, -0.098888248598895112, -17.139399300620212 };
00304 float five_l []= {0.34795130919909756, -0.33483508677418122, 0.21450526015905091, -0.87161320330702452, -0.55300340950519367, -0.51138511922202357, -3.1737890509598912};
00305 if(left)
00306 arm.startTrajectory(arm.arm_trajectoryPoint(five_l,1.25,false),false);
00307 if(right)
00308 arm.startTrajectory(arm.arm_trajectoryPoint(five_r,1.25,true),true);
00309
00310
00311 gripper.slap(left,right);
00312
00313
00314
00315 while(!gripper.slapDone(left,right) && ros::ok())
00316 {
00317 ros::Duration(0.005).sleep();
00318 }
00319
00320 float post_five_r []= {-1.2271486279403441, 0.98994689398564029, 0.27937452657595019, -1.9855738422813785, 31.095246711685615, -0.75469820126008202, -17.206098475132887 };
00321 float post_five_l []= {1.458651261930636, 1.0444005395208478, 0.081571109098415029, -2.1115743463069396, -1.3390296269894097, -0.16823026639652239, -3.5006715676681437};
00322 if(left)
00323 arm.startTrajectory(arm.arm_trajectoryPoint(post_five_l,1.35,false),false);
00324 if(right)
00325 arm.startTrajectory(arm.arm_trajectoryPoint(post_five_r,1.35,true),true);
00326
00327 sleep(2.0);
00328 }
00329 return 0;
00330 }