00001 #include "rail_grasping/graspObject.h"
00002
00003 using namespace std;
00004
00005 graspObject::graspObject() :
00006 acPickup("jaco_arm/manipulation/pickup"),
00007 acGrasp("jaco_arm/manipulation/grasp"),
00008 acJointTrajectory("jaco_arm/joint_velocity_controller/trajectory"),
00009 acMoveArm("carl_moveit_wrapper/move_to_pose")
00010 {
00011 ROS_INFO("Starting rail grasping...");
00012 graspTransform.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
00013 graspTransform.setRotation(tf::Quaternion(0, 0, 0, 1));
00014
00015 cartesianCommandPub = n.advertise<wpi_jaco_msgs::CartesianCommand>("jaco_arm/cartesian_cmd", 1);
00016
00017 armJointSubscriber = n.subscribe("jaco_arm/joint_states", 1, &graspObject::armJointStatesCallback, this);
00018
00019 IKClient = n.serviceClient<carl_moveit::CallIK>("carl_moveit_wrapper/call_ik");
00020 cartesianPositionClient = n.serviceClient<wpi_jaco_msgs::GetCartesianPosition>("jaco_arm/get_cartesian_position");
00021 cartesianPathClient = n.serviceClient<carl_moveit::CartesianPath>("carl_moveit_wrapper/cartesian_path");
00022
00023 jointNamesSet = false;
00024 armJointPos.resize(NUM_JACO_JOINTS);
00025 armJointNames.resize(NUM_JACO_JOINTS);
00026
00027 while (!acPickup.waitForServer(ros::Duration(5.0)) &&
00028 !acGrasp.waitForServer(ros::Duration(5.0)) &&
00029 !acJointTrajectory.waitForServer(ros::Duration(5.0)))
00030 {
00031 ROS_INFO("Waiting for grasp, pickup, and arm trajectory action servers...");
00032 }
00033
00034 requestGraspServer = n.advertiseService("rail_grasping/request_grasp", &graspObject::requestGrasp, this);
00035 requestReleaseServer = n.advertiseService("rail_grasping/request_release", &graspObject::requestRelease, this);
00036
00037 ROS_INFO("Rail grasping started.");
00038 }
00039
00040 void graspObject::armJointStatesCallback(const sensor_msgs::JointState &msg)
00041 {
00042 for (unsigned int i = 0; i < NUM_JACO_JOINTS; i++)
00043 {
00044 armJointPos[i] = msg.position[i];
00045 }
00046
00047 if (!jointNamesSet)
00048 {
00049 for (unsigned int i = 0; i < NUM_JACO_JOINTS; i++)
00050 {
00051 armJointNames[i] = msg.name[i];
00052 }
00053 jointNamesSet = true;
00054 }
00055 }
00056
00057 bool graspObject::requestRelease(rail_grasping::RequestGrasp::Request & req, rail_grasping::RequestGrasp::Response & res)
00058 {
00059 ROS_INFO("Received new release request...");
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00089
00091
00092
00093
00094
00095
00096 geometry_msgs::Pose releasePose = req.graspPose;
00097 releasePose.position.z += .05;
00098
00099
00100 bool success = false;
00101 int counter = 0;
00102 while (!success)
00103 {
00104 ROS_INFO("Moving to approach angle...");
00105 carl_moveit::MoveToPoseGoal movePoseGoal;
00106 movePoseGoal.pose = releasePose;
00107 switch (counter)
00108 {
00109 case 0:
00110 movePoseGoal.pose.position.x += .005;
00111 break;
00112 case 1:
00113 movePoseGoal.pose.position.x -= .005;
00114 break;
00115 case 2:
00116 movePoseGoal.pose.position.y += .005;
00117 break;
00118 case 3:
00119 movePoseGoal.pose.position.y -= .005;
00120 break;
00121 case 4:
00122 movePoseGoal.pose.position.z += .005;
00123 break;
00124 case 5:
00125 movePoseGoal.pose.position.z -= .005;
00126 break;
00127 case 6:
00128 movePoseGoal.pose.position.x += .005;
00129 movePoseGoal.pose.position.y += .005;
00130 break;
00131 case 7:
00132 movePoseGoal.pose.position.x += .005;
00133 movePoseGoal.pose.position.y -= .005;
00134 break;
00135 case 8:
00136 movePoseGoal.pose.position.x -= .005;
00137 movePoseGoal.pose.position.y += .005;
00138 break;
00139 case 9:
00140 movePoseGoal.pose.position.x -= .005;
00141 movePoseGoal.pose.position.y -= .005;
00142 break;
00143 }
00144 acMoveArm.sendGoal(movePoseGoal);
00145 ROS_INFO("Approach angle arm move initiated.");
00146 acMoveArm.waitForResult(ros::Duration(20.0));
00147 carl_moveit::MoveToPoseResultConstPtr movePoseResult = acMoveArm.getResult();
00148
00149 counter++;
00150
00151 res.earlyFailureDetected = !movePoseResult->success;
00152 if (!res.earlyFailureDetected)
00153 {
00154 success = true;
00155 res.result = true;
00156 }
00157 else if (res.earlyFailureDetected && counter > 9)
00158 {
00159 ROS_INFO("Detected failure on moving to approach angle.");
00160 res.result = false;
00161 return false;
00162 }
00163 }
00164
00166 wpi_jaco_msgs::GetCartesianPosition::Request ikReq;
00167 wpi_jaco_msgs::GetCartesianPosition::Response ikRes;
00168 cartesianPositionClient.call(ikReq, ikRes);
00169
00170 wpi_jaco_msgs::CartesianCommand cartesianCmd;
00171 cartesianCmd.position = true;
00172 cartesianCmd.armCommand = true;
00173 cartesianCmd.fingerCommand = false;
00174 cartesianCmd.repeat = false;
00175 cartesianCmd.arm = ikRes.pos;
00176 cartesianCmd.arm.linear.z -= .05;
00177 cartesianCommandPub.publish(cartesianCmd);
00178
00179 ros::Duration(5.0).sleep();
00181
00182
00183 ROS_INFO("Fully opening gripper...");
00184 wpi_jaco_msgs::ExecuteGraspGoal openGripperGoal;
00185 openGripperGoal.closeGripper = false;
00186 openGripperGoal.limitFingerVelocity = false;
00187 acGrasp.sendGoal(openGripperGoal);
00188 acGrasp.waitForResult(ros::Duration(5.0));
00189 ROS_INFO("Gripper opened.");
00190
00191
00192 ROS_INFO("Lifting hand...");
00193 wpi_jaco_msgs::ExecutePickupGoal pickupGoal;
00194 pickupGoal.limitFingerVelocity = false;
00195 pickupGoal.setLiftVelocity = false;
00196 acPickup.sendGoal(pickupGoal);
00197 acPickup.waitForResult(ros::Duration(10));
00198 ROS_INFO("hand lifting complete.");
00199 }
00200
00201 bool graspObject::requestGrasp(rail_grasping::RequestGrasp::Request & req, rail_grasping::RequestGrasp::Response & res)
00202 {
00203 ROS_INFO("Received new grasp request...");
00204
00205 graspTransform.setOrigin(tf::Vector3(req.graspPose.position.x, req.graspPose.position.y, req.graspPose.position.z));
00206 graspTransform.setRotation(tf::Quaternion(req.graspPose.orientation.x, req.graspPose.orientation.y, req.graspPose.orientation.z, req.graspPose.orientation.w));
00207 tfBroadcaster.sendTransform(tf::StampedTransform(graspTransform, ros::Time::now(), "base_footprint", "grasp_frame"));
00208
00209 ros::Duration(1).sleep();
00210
00211 bool earlyFailureDetected;
00212 res.result = executeGrasp(&earlyFailureDetected);
00213 res.earlyFailureDetected = earlyFailureDetected;
00214
00215 return true;
00216 }
00217
00218 bool graspObject::executeGrasp(bool *earlyFailureFlag)
00219 {
00220 ROS_INFO("\nGrasp execution started");
00221
00222
00223 ROS_INFO("Fully opening gripper...");
00224 wpi_jaco_msgs::ExecuteGraspGoal openGripperGoal;
00225 openGripperGoal.closeGripper = false;
00226 openGripperGoal.limitFingerVelocity = false;
00227 acGrasp.sendGoal(openGripperGoal);
00228 acGrasp.waitForResult(ros::Duration(5.0));
00229 ROS_INFO("Gripper opened.");
00230
00231
00232
00233 geometry_msgs::PoseStamped approachPose;
00234 geometry_msgs::PoseStamped poseOut;
00235
00236 approachPose.header.frame_id = "grasp_frame";
00237 approachPose.pose.position.x = 0.0;
00238 approachPose.pose.position.y = 0.0;
00239 approachPose.pose.position.z = 0.7;
00240 approachPose.pose.position.z = 0.0;
00241 approachPose.pose.orientation.x = 0.0;
00242 approachPose.pose.orientation.y = 0.0;
00243 approachPose.pose.orientation.z = 0.0;
00244 approachPose.pose.orientation.w = 1.0;
00245
00246
00247 approachPose.pose.position.z += .19;
00248
00249 tfListener.transformPose("base_footprint", approachPose, poseOut);
00250
00251
00252 ROS_INFO("Moving to approach angle...");
00253 carl_moveit::MoveToPoseGoal movePoseGoal;
00254 movePoseGoal.pose = poseOut.pose;
00255 acMoveArm.sendGoal(movePoseGoal);
00256 ROS_INFO("Approach angle arm move initiated.");
00257 acMoveArm.waitForResult(ros::Duration(20.0));
00258 carl_moveit::MoveToPoseResultConstPtr movePoseResult = acMoveArm.getResult();
00259
00260 *earlyFailureFlag = !movePoseResult->success;
00261 if (*earlyFailureFlag)
00262 {
00263 ROS_INFO("Detected failure on moving to approach angle.");
00264 return false;
00265 }
00266
00267
00268 carl_moveit::CartesianPath pathSrvObj;
00269 geometry_msgs::PoseStamped pickupPose;
00270 geometry_msgs::PoseStamped pickupPoseOut;
00271 pickupPoseOut.header.frame_id = "base_footprint";
00272
00273 pickupPose.header.frame_id = "grasp_frame";
00274 pickupPose.pose.position.x = 0.0;
00275 pickupPose.pose.position.y = 0.0;
00276 pickupPose.pose.position.z = 0.0;
00277 pickupPose.pose.orientation.x = 0.0;
00278 pickupPose.pose.orientation.y = 0.0;
00279 pickupPose.pose.orientation.z = 0.0;
00280 pickupPose.pose.orientation.w = 1.0;
00281
00282
00283 pickupPose.pose.position.z += .19;
00284
00285 tfListener.transformPose("base_footprint", pickupPose, pickupPoseOut);
00286
00287 tf::StampedTransform currentHandTransform;
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301 pathSrvObj.request.waypoints.push_back(pickupPoseOut.pose);
00302 pathSrvObj.request.avoidCollisions = false;
00303
00304 if (!cartesianPathClient.call(pathSrvObj))
00305 {
00306 ROS_INFO("Failed to call cartesian path planner.");
00307 *earlyFailureFlag = true;
00308 return false;
00309 }
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354 ROS_INFO("Closing gripper...");
00355 wpi_jaco_msgs::ExecuteGraspGoal closeGripperGoal;
00356 closeGripperGoal.closeGripper = true;
00357 closeGripperGoal.limitFingerVelocity = false;
00358 acGrasp.sendGoal(closeGripperGoal);
00359 acGrasp.waitForResult(ros::Duration(5.0));
00360 ROS_INFO("Gripper closed.");
00361
00362
00363
00364
00365
00366 ROS_INFO("Lifting object...");
00367
00368
00369
00370
00371
00372
00373
00374
00375
00377 carl_moveit::CartesianPathRequest pickupPathReq;
00378 carl_moveit::CartesianPathResponse pickupPathRes;
00379 tfListener.waitForTransform("jaco_link_hand", "base_footprint", ros::Time::now(), ros::Duration(1.0));
00380 tfListener.lookupTransform("base_footprint", "jaco_link_hand", ros::Time(0), currentHandTransform);
00381 geometry_msgs::Pose pickupPoseWaypoint;
00382 pickupPoseWaypoint.position.x = currentHandTransform.getOrigin().x();
00383 pickupPoseWaypoint.position.y = currentHandTransform.getOrigin().y();
00384 pickupPoseWaypoint.position.z = currentHandTransform.getOrigin().z() + .085;
00385 pickupPoseWaypoint.orientation.x = currentHandTransform.getRotation().x();
00386 pickupPoseWaypoint.orientation.y = currentHandTransform.getRotation().y();
00387 pickupPoseWaypoint.orientation.z = currentHandTransform.getRotation().z();
00388 pickupPoseWaypoint.orientation.w = currentHandTransform.getRotation().w();
00389 pickupPathReq.waypoints.push_back(pickupPoseWaypoint);
00390 pickupPathReq.avoidCollisions = false;
00391
00392 if (!cartesianPathClient.call(pickupPathReq, pickupPathRes))
00393 {
00394 ROS_INFO("Failed to call cartesian path planner.");
00395 return false;
00396 }
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428 return true;
00429 }
00430
00431 bool graspObject::moveArmToPose(geometry_msgs::PoseStamped poseGoal)
00432 {
00433
00434 carl_moveit::CallIK::Request ikReq;
00435 carl_moveit::CallIK::Response ikRes;
00436 ikReq.pose = poseGoal.pose;
00437 IKClient.call(ikReq, ikRes);
00438
00439 if (ikRes.success = false)
00440 {
00441 ROS_INFO("IK Server returned false");
00442 return false;
00443 }
00444
00445
00446
00447 if (!jointNamesSet)
00448 {
00449 ROS_INFO("Joint data from the arm has not been received");
00450 return false;
00451 }
00452
00453 trajectory_msgs::JointTrajectoryPoint startPoint;
00454 trajectory_msgs::JointTrajectoryPoint endPoint;
00455 startPoint.positions = armJointPos;
00456 endPoint.positions = ikRes.jointPositions;
00457 control_msgs::FollowJointTrajectoryGoal trajGoal;
00458 trajGoal.trajectory.joint_names = armJointNames;
00459 trajGoal.trajectory.points.clear();
00460 trajGoal.trajectory.points.push_back(startPoint);
00461 trajGoal.trajectory.points.push_back(endPoint);
00462
00463
00464
00465
00466 ROS_INFO("Sending trajectory goal directly to arm");
00467
00468 acJointTrajectory.sendGoal(trajGoal);
00469 acJointTrajectory.waitForResult(ros::Duration(10.0));
00470
00471
00472 return true;
00473 }
00474
00475 void graspObject::publishGraspFrame()
00476 {
00477 tfBroadcaster.sendTransform(tf::StampedTransform(graspTransform, ros::Time::now(), "base_footprint", "grasp_frame"));
00478 }
00479
00480 int main(int argc, char **argv)
00481 {
00482 ros::init(argc, argv, "rail_grasping");
00483
00484 graspObject go;
00485
00486 ros::Rate loop_rate(30);
00487 while (ros::ok())
00488 {
00489 ros::spinOnce();
00490 go.publishGraspFrame();
00491 loop_rate.sleep();
00492 }
00493
00494 return 0;
00495 }