00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include <ros/ros.h>
00019 #include <iostream>
00020 #include <fstream>
00021 #include <math.h>
00022
00023 #include <cob_pick_place_action/cob_pick_place_action.h>
00024 #include "geometric_shapes/shapes.h"
00025 #include "geometric_shapes/shape_messages.h"
00026 #include "geometric_shapes/mesh_operations.h"
00027 #include "geometric_shapes/shape_operations.h"
00028
00029 #include <algorithm>
00030
00031 void CobPickPlaceActionServer::initialize()
00032 {
00033 pub_co = nh_.advertise<moveit_msgs::CollisionObject>("collision_object", 10);
00034 pub_ao = nh_.advertise<moveit_msgs::AttachedCollisionObject>("attached_collision_object", 10);
00035
00036
00037 map_classid_to_classname[11]="sauerkraut";
00038 map_classid_to_classname[13]="fruittea";
00039 map_classid_to_classname[16]="orangemarmelade";
00040 map_classid_to_classname[18]="yellowsaltcube";
00041 map_classid_to_classname[27]="hotpot";
00042 map_classid_to_classname[30]="bluesaltcube";
00043 map_classid_to_classname[31]="yellowsaltcylinder";
00044 map_classid_to_classname[44]="knaeckebrot";
00045 map_classid_to_classname[46]="liviosunfloweroil";
00046 map_classid_to_classname[50]="instantsoup";
00047 map_classid_to_classname[52]="hotpot2";
00048 map_classid_to_classname[57]="mueslibars";
00049 map_classid_to_classname[65]="fruitdrink";
00050 map_classid_to_classname[99]="ruskwholemeal";
00051 map_classid_to_classname[101]="koalacandy";
00052 map_classid_to_classname[103]="instanttomatosoup";
00053 map_classid_to_classname[106]="bakingsoda";
00054 map_classid_to_classname[107]="sweetener";
00055 map_classid_to_classname[109]="chocoicing";
00056 map_classid_to_classname[119]="tomatoherbsauce";
00057 map_classid_to_classname[122]="peanuts";
00058 map_classid_to_classname[126]="herbsalt";
00059 map_classid_to_classname[128]="bathdetergent";
00060
00061
00062 map_classid_to_classname[5001]="pringles";
00063
00064
00065 static const std::string COB_PICKUP_ACTION_NAME = "cob_pick_action";
00066 as_pick.reset(new actionlib::SimpleActionServer<cob_pick_place_action::CobPickAction>(nh_, COB_PICKUP_ACTION_NAME, boost::bind(&CobPickPlaceActionServer::pick_goal_cb, this, _1), false));
00067 as_pick->start();
00068
00069 static const std::string COB_PLACE_ACTION_NAME = "cob_place_action";
00070 as_place.reset(new actionlib::SimpleActionServer<cob_pick_place_action::CobPlaceAction>(nh_, COB_PLACE_ACTION_NAME, boost::bind(&CobPickPlaceActionServer::place_goal_cb, this, _1), false));
00071 as_place->start();
00072
00073 last_grasp_valid = false;
00074 last_object_name.clear();
00075
00076
00077 static const std::string QUERY_GRASPS_OR_ACTION_NAME = "query_grasps";
00078 ac_grasps_or.reset(new actionlib::SimpleActionClient<cob_grasp_generation::QueryGraspsAction>(nh_, QUERY_GRASPS_OR_ACTION_NAME, true));
00079 ROS_INFO("Waiting for action server \"%s\" to start...", QUERY_GRASPS_OR_ACTION_NAME.c_str());
00080 ac_grasps_or->waitForServer();
00081 ROS_INFO("Action server \"%s\" started.", QUERY_GRASPS_OR_ACTION_NAME.c_str());
00082 }
00083
00084 void CobPickPlaceActionServer::run()
00085 {
00086 ROS_INFO("cob_pick_action...spinning");
00087 ros::spin();
00088 }
00089
00090 void CobPickPlaceActionServer::pick_goal_cb(const cob_pick_place_action::CobPickGoalConstPtr &goal)
00091 {
00092 ROS_INFO("PickGoalCB: Received new goal: Trying to pick %s", goal->object_name.c_str());
00093 cob_pick_place_action::CobPickResult result;
00094 std::string response;
00095 bool success = false;
00096
00097 ROS_DEBUG_STREAM(*(goal.get()));
00098
00099 if(goal->gripper_type.empty())
00100 {
00101 ROS_ERROR("Pick failed: No gripper_type specified!");
00102 result.success.data=false;
00103 response="Pick failed: No gripper_type specified!";
00104 as_pick->setAborted(result, response);
00105 last_grasp_valid = false;
00106 last_object_name.clear();
00107 return;
00108 }
00109
00111 std::vector<moveit_msgs::Grasp> grasps;
00112 if(goal->grasp_database=="KIT")
00113 {
00114 ROS_INFO("Using KIT grasp table");
00115 if(goal->grasp_id!=0)
00116 {
00117 ROS_INFO("Using specific grasp_id: %d", goal->grasp_id);
00118 fillSingleGraspKIT(goal->object_class, goal->gripper_type, goal->grasp_id, goal->object_pose, grasps);
00119 }
00120 else
00121 {
00122 ROS_INFO("Using all grasps");
00123 fillAllGraspsKIT(goal->object_class, goal->gripper_type, goal->object_pose, grasps);
00124 }
00125 }
00126 else if(goal->grasp_database=="OpenRAVE")
00127 {
00128 ROS_INFO("Using OpenRAVE grasp table");
00129 fillGraspsOR(goal->object_class, goal->gripper_type, goal->gripper_side, goal->grasp_id, goal->object_pose, grasps);
00130 }
00131 else if(goal->grasp_database=="ALL")
00132 {
00133 ROS_INFO("Using all available databases");
00134 std::vector<moveit_msgs::Grasp> grasps_OR, grasps_KIT;
00135 fillAllGraspsKIT(goal->object_class, goal->gripper_type, goal->object_pose, grasps_KIT);
00136 fillGraspsOR(goal->object_class, goal->gripper_type, goal->gripper_side, goal->grasp_id, goal->object_pose, grasps_OR);
00137
00138 grasps = grasps_KIT;
00139 std::vector<moveit_msgs::Grasp>::iterator it = grasps.end();
00140 grasps.insert(it, grasps_OR.begin(), grasps_OR.end());
00141 }
00142 else
00143 {
00144 ROS_ERROR("Grasp_Database %s not supported! Please use \"KIT\" or \"OpenRAVE\" or \"ALL\" instead", goal->grasp_database.c_str());
00145 result.success.data=false;
00146 response="Pick failed: Grasp Database not supported!";
00147 as_pick->setAborted(result, response);
00148 last_grasp_valid = false;
00149 last_object_name.clear();
00150 return;
00151 }
00152
00153 if(!grasps.empty())
00154 {
00155 ROS_INFO("PickGoalCB: Found %lu grasps for this object", grasps.size());
00156 for(unsigned int i=0; i<grasps.size(); i++)
00157 {
00158 ROS_DEBUG_STREAM("Grasp "<< i << ": " << grasps[i]);
00159 }
00160 }
00161 else
00162 {
00163 ROS_ERROR("No grasps found for object %s in database %s using gripper_type %s", goal->object_name.c_str(), goal->grasp_database.c_str(), goal->gripper_type.c_str());
00164 result.success.data=false;
00165 response="Pick failed: No grasps found!";
00166 as_pick->setAborted(result, response);
00167 last_grasp_valid = false;
00168 last_object_name.clear();
00169 return;
00170 }
00171
00173 insertObject(goal->object_name, goal->object_class, goal->object_pose);
00174
00175 if(!(goal->support_surface.empty()))
00176 {
00177 ROS_INFO("Setting SupportSurface to %s", goal->support_surface.c_str());
00178 group.setSupportSurfaceName(goal->support_surface);
00179 }
00180
00182 group.setPlanningTime(300.0);
00183 success = group.pick(goal->object_name, grasps);
00184
00185 if(success)
00186 {
00187 ROS_INFO("Pick successfull!");
00188 result.success.data=true;
00189 response="Pick successfull!";
00190 as_pick->setSucceeded(result, response);
00191 last_grasp_valid = true;
00192 last_object_name = goal->object_name;
00193 }
00194 else
00195 {
00196 ROS_ERROR("Pick failed: Could not plan!");
00197 result.success.data=false;
00198 response="Pick failed: Could not plan!";
00199 as_pick->setAborted(result, response);
00200 last_grasp_valid = false;
00201 last_object_name.clear();
00202 }
00203 }
00204
00205 void CobPickPlaceActionServer::place_goal_cb(const cob_pick_place_action::CobPlaceGoalConstPtr &goal)
00206 {
00207 ROS_INFO("PlaceCB: Received new goal: Trying to Place %s", goal->object_name.c_str());
00208 cob_pick_place_action::CobPlaceResult result;
00209 std::string response;
00210 bool success = false;
00211
00212 if(!last_grasp_valid || last_object_name.empty() || last_object_name != goal->object_name)
00213 {
00214 ROS_ERROR("Object %s cannot be placed", goal->object_name.c_str());
00215 result.success.data = false;
00216 response = "Object has not been picked before";
00217 as_place->setAborted(result, response);
00218 last_grasp_valid = false;
00219 last_object_name.clear();
00220 return;
00221 }
00222
00223 if(goal->destinations.empty())
00224 {
00225 ROS_ERROR("Object %s cannot be placed", goal->object_name.c_str());
00226 result.success.data = false;
00227 response = "No destinations given";
00228 as_place->setAborted(result, response);
00229 last_grasp_valid = false;
00230 last_object_name.clear();
00231 return;
00232 }
00233
00234 std::vector<moveit_msgs::PlaceLocation> locations;
00235 trajectory_msgs::JointTrajectory pre_grasp_posture;
00236 pre_grasp_posture.header.stamp = ros::Time::now();
00237 pre_grasp_posture.joint_names.resize(7);
00238 pre_grasp_posture.joint_names[0] = "sdh_knuckle_joint";
00239 pre_grasp_posture.joint_names[1] = "sdh_finger_12_joint";
00240 pre_grasp_posture.joint_names[2] = "sdh_finger_13_joint";
00241 pre_grasp_posture.joint_names[3] = "sdh_finger_22_joint";
00242 pre_grasp_posture.joint_names[4] = "sdh_finger_23_joint";
00243 pre_grasp_posture.joint_names[5] = "sdh_thumb_2_joint";
00244 pre_grasp_posture.joint_names[6] = "sdh_thumb_3_joint";
00245
00246 trajectory_msgs::JointTrajectoryPoint point;
00247 point.positions.assign(7,0.0);
00248 point.velocities.assign(7,0.0);
00249 point.accelerations.assign(7,0.0);
00250 point.effort.assign(7,0.0);
00251 point.time_from_start = ros::Duration(3.0);
00252
00253 point.positions[0] = 0.0;
00254 point.positions[1] = -0.9854;
00255 point.positions[2] = 0.9472;
00256 point.positions[3] = -0.9854;
00257 point.positions[4] = 0.9472;
00258 point.positions[5] = -0.9854;
00259 point.positions[6] = 0.9472;
00260 pre_grasp_posture.points.push_back(point);
00261
00262 for(unsigned int i=0; i<goal->destinations.size(); i++)
00263 {
00264 moveit_msgs::PlaceLocation place_location;
00265
00266 place_location.id = "Last_"+goal->object_name+"_grasp";
00267 place_location.post_place_posture = pre_grasp_posture;
00268 place_location.place_pose = goal->destinations[i];
00269 place_location.pre_place_approach.direction.header.frame_id = "/base_footprint";
00270 place_location.pre_place_approach.direction.vector.z = -1.0;
00271 place_location.pre_place_approach.min_distance = 0.1;
00272 place_location.pre_place_approach.desired_distance = 0.15;
00273 place_location.post_place_retreat.direction.header.frame_id = "/base_footprint";
00274 place_location.post_place_retreat.direction.vector.z = 1.0;
00275 place_location.post_place_retreat.min_distance = 0.1;
00276 place_location.post_place_retreat.desired_distance = 0.15;
00277
00278 locations.push_back(place_location);
00279 }
00280
00281 if(!(goal->support_surface.empty()))
00282 {
00283 ROS_INFO("Setting SupportSurface to %s", goal->support_surface.c_str());
00284 group.setSupportSurfaceName(goal->support_surface);
00285 }
00286 group.setPlanningTime(300.0);
00287
00288 success = group.place(goal->object_name, locations);
00289
00291 if(success)
00292 {
00293 result.success.data=true;
00294 response="PLACE SUCCEEDED";
00295 as_place->setSucceeded(result, response);
00296 last_grasp_valid = false;
00297 last_object_name.clear();
00298 }
00299 else
00300 {
00301 result.success.data=false;
00302 response="PLACE FAILED";
00303 as_place->setAborted(result, response);
00304 last_grasp_valid = false;
00305 last_object_name.clear();
00306 }
00307 }
00308
00309 void CobPickPlaceActionServer::insertObject(std::string object_name, unsigned int object_class, geometry_msgs::PoseStamped object_pose)
00310 {
00311 ROS_INFO("Adding object to MoveIt! environment..");
00312
00313 moveit_msgs::CollisionObject co;
00314 co.header.stamp = object_pose.header.stamp;
00315 co.header.frame_id = object_pose.header.frame_id;
00316
00317
00318 co.id = object_name;
00319 co.operation = co.REMOVE;
00320 pub_co.publish(co);
00321
00322
00323 co.id = object_name;
00324 co.operation = co.ADD;
00325
00326 std::string mesh_name = map_classid_to_classname[object_class];
00327 std::transform(mesh_name.begin(), mesh_name.end(), mesh_name.begin(), ::tolower);
00328
00329 boost::scoped_ptr<shapes::Mesh> mesh;
00330 mesh.reset(shapes::createMeshFromResource("package://cob_grasp_generation/files/meshes/"+mesh_name+".stl"));
00331 shapes::ShapeMsg shape_msg;
00332 shapes::constructMsgFromShape(mesh.get(), shape_msg);
00333 co.meshes.push_back(boost::get<shape_msgs::Mesh>(shape_msg));
00334 co.mesh_poses.push_back(object_pose.pose);
00335 pub_co.publish(co);
00336
00337
00338 tf::Transform transform;
00339 transform.setOrigin( tf::Vector3(object_pose.pose.position.x, object_pose.pose.position.y, object_pose.pose.position.z) );
00340 transform.setRotation( tf::Quaternion(object_pose.pose.orientation.x, object_pose.pose.orientation.y, object_pose.pose.orientation.z, object_pose.pose.orientation.w) );
00341 tf_broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), object_pose.header.frame_id, object_name));
00342
00343
00344 ros::Duration(1.0).sleep();
00345 }
00346
00347
00348
00349 void CobPickPlaceActionServer::fillAllGraspsKIT(unsigned int objectClassId, std::string gripper_type, geometry_msgs::PoseStamped object_pose, std::vector<moveit_msgs::Grasp> &grasps)
00350 {
00351 grasps.clear();
00352 Grasp *current_grasp = NULL;
00353
00355 std::string path = ros::package::getPath("cob_grasp_generation")+std::string("/files/")+gripper_type+std::string("_grasptable_kit.txt");
00356 GraspTableIniFile = const_cast<char*>(path.c_str());
00357 m_GraspTable = new GraspTable();
00358 int error = m_GraspTable->Init(GraspTableIniFile);
00359 if(error<0)
00360 {
00361 ROS_ERROR("Failed to initialize GraspTables");
00362 return;
00363 }
00364
00365 m_GraspTable->ResetReadPtr(objectClassId);
00366 unsigned int grasp_index = 0;
00367
00368 current_grasp = m_GraspTable->GetNextGrasp(objectClassId);
00369
00370 while(current_grasp)
00371 {
00372 convertGraspKIT(current_grasp, object_pose, grasps);
00373
00374 current_grasp = m_GraspTable->GetNextGrasp(objectClassId);
00375 grasp_index++;
00376 }
00377 }
00378
00379 void CobPickPlaceActionServer::fillSingleGraspKIT(unsigned int objectClassId, std::string gripper_type, unsigned int grasp_id, geometry_msgs::PoseStamped object_pose, std::vector<moveit_msgs::Grasp> &grasps)
00380 {
00381 grasps.clear();
00382 Grasp *current_grasp = NULL;
00383
00385 std::string path = ros::package::getPath("cob_grasp_generation")+std::string("/files/")+gripper_type+std::string("_grasptable_kit.txt");
00386 GraspTableIniFile = const_cast<char*>(path.c_str());
00387 m_GraspTable = new GraspTable();
00388 int error = m_GraspTable->Init(GraspTableIniFile);
00389 if(error<0)
00390 {
00391 ROS_ERROR("Failed to initialize GraspTables");
00392 return;
00393 }
00394
00395 current_grasp = m_GraspTable->GetGrasp(objectClassId, grasp_id);
00396
00397 if(current_grasp)
00398 {
00399 ROS_INFO("GraspIndex %d found",grasp_id);
00400
00401 convertGraspKIT(current_grasp, object_pose, grasps);
00402 }
00403 else
00404 ROS_ERROR("Grasp %d NOT found", grasp_id);
00405 }
00406
00407 void CobPickPlaceActionServer::convertGraspKIT(Grasp* current_grasp, geometry_msgs::PoseStamped object_pose, std::vector<moveit_msgs::Grasp> &grasps)
00408 {
00409 bool debug = true;
00410 moveit_msgs::Grasp g;
00411
00412
00413 std::vector<double> current_hand_pre_config = current_grasp->GetHandPreGraspConfig();
00414 sensor_msgs::JointState pre_grasp_posture;
00415 pre_grasp_posture.position.clear();
00416 for (unsigned int i=0; i<current_hand_pre_config.size(); i++)
00417 {
00418 pre_grasp_posture.position.push_back(current_hand_pre_config[i]);
00419 }
00420 g.pre_grasp_posture = MapHandConfiguration(pre_grasp_posture);
00421
00422
00423 std::vector<double> current_hand_config = current_grasp->GetHandOptimalGraspConfig();
00424 sensor_msgs::JointState grasp_posture;
00425 grasp_posture.position.clear();
00426 for (unsigned int i=0; i<current_hand_config.size(); i++)
00427 {
00428 grasp_posture.position.push_back(current_hand_config[i]);
00429 }
00430 g.grasp_posture= MapHandConfiguration(grasp_posture);
00431
00432
00434
00435
00436 std::vector<double> current_grasp_pose = current_grasp->GetTCPGraspPose();
00437 tf::Transform transform_grasp_O_from_SDH = tf::Transform(
00438 tf::createQuaternionFromRPY(current_grasp_pose[3], current_grasp_pose[4], current_grasp_pose[5] ),
00439 0.001*tf::Vector3(current_grasp_pose[0],current_grasp_pose[1],current_grasp_pose[2]));
00440
00441 if (debug)
00442 {
00443 geometry_msgs::Transform msg_grasp_O_from_SDH;
00444 tf::transformTFToMsg(transform_grasp_O_from_SDH, msg_grasp_O_from_SDH);
00445 ROS_DEBUG_STREAM("msg_grasp_O_from_SDH:" << msg_grasp_O_from_SDH);
00446 }
00447
00448
00449 tf::Transform transform_HEADER_from_O = tf::Transform(
00450 tf::Quaternion(object_pose.pose.orientation.x, object_pose.pose.orientation.y, object_pose.pose.orientation.z, object_pose.pose.orientation.w),
00451 tf::Vector3(object_pose.pose.position.x, object_pose.pose.position.y, object_pose.pose.position.z));
00452
00453 if (debug)
00454 {
00455 geometry_msgs::Transform msg_HEADER_from_O;
00456 tf::transformTFToMsg(transform_HEADER_from_O, msg_HEADER_from_O);
00457 ROS_DEBUG_STREAM("msg_HEADER_from_O:" << msg_HEADER_from_O);
00458 }
00459
00460
00461 tf::Transform transform_grasp_FOOTPRINT_from_ARM7 = transformPose(transform_grasp_O_from_SDH, transform_HEADER_from_O, object_pose.header.frame_id);
00462
00463 if (debug)
00464 {
00465 geometry_msgs::Transform msg_grasp_FOOTPRINT_from_ARM7;
00466 tf::transformTFToMsg(transform_grasp_FOOTPRINT_from_ARM7, msg_grasp_FOOTPRINT_from_ARM7);
00467 ROS_DEBUG_STREAM("msg_grasp_FOOTPRINT_from_ARM7:" << msg_grasp_FOOTPRINT_from_ARM7);
00468 }
00469
00470
00471 geometry_msgs::Transform msg_transform_grasp_FOOTPRINT_from_ARM7;
00472 tf::transformTFToMsg(transform_grasp_FOOTPRINT_from_ARM7, msg_transform_grasp_FOOTPRINT_from_ARM7);
00473 geometry_msgs::PoseStamped msg_pose_grasp_FOOTPRINT_from_ARM7;
00474 msg_pose_grasp_FOOTPRINT_from_ARM7.header.stamp = ros::Time::now();
00475 msg_pose_grasp_FOOTPRINT_from_ARM7.header.frame_id = "/base_footprint";
00476 msg_pose_grasp_FOOTPRINT_from_ARM7.pose.position.x = msg_transform_grasp_FOOTPRINT_from_ARM7.translation.x;
00477 msg_pose_grasp_FOOTPRINT_from_ARM7.pose.position.y = msg_transform_grasp_FOOTPRINT_from_ARM7.translation.y;
00478 msg_pose_grasp_FOOTPRINT_from_ARM7.pose.position.z = msg_transform_grasp_FOOTPRINT_from_ARM7.translation.z;
00479 msg_pose_grasp_FOOTPRINT_from_ARM7.pose.orientation = msg_transform_grasp_FOOTPRINT_from_ARM7.rotation;
00480
00481 if (debug)
00482 {
00483 ROS_DEBUG_STREAM("msg_pose_grasp_FOOTPRINT_from_ARM7:" << msg_pose_grasp_FOOTPRINT_from_ARM7);
00484 }
00485
00486 g.grasp_pose = msg_pose_grasp_FOOTPRINT_from_ARM7;
00487
00488
00489
00490 std::vector<double> current_pre_grasp_pose = current_grasp->GetTCPPreGraspPose();
00491 tf::Transform transform_pre_O_from_SDH = tf::Transform(
00492 tf::createQuaternionFromRPY(current_pre_grasp_pose[3], current_pre_grasp_pose[4], current_pre_grasp_pose[5] ),
00493 0.001*tf::Vector3(current_pre_grasp_pose[0],current_pre_grasp_pose[1],current_pre_grasp_pose[2]));
00494
00495 if (debug)
00496 {
00497 geometry_msgs::Transform msg_pre_O_from_SDH;
00498 tf::transformTFToMsg(transform_pre_O_from_SDH, msg_pre_O_from_SDH);
00499 ROS_DEBUG_STREAM("msg_pre_O_from_SDH:" << msg_pre_O_from_SDH);
00500 }
00501
00502
00503 tf::Transform transform_pre_FOOTPRINT_from_ARM7 = transformPose(transform_pre_O_from_SDH, transform_HEADER_from_O, object_pose.header.frame_id);
00504
00505 if (debug)
00506 {
00507 geometry_msgs::Transform msg_pre_FOOTPRINT_from_ARM7;
00508 tf::transformTFToMsg(transform_pre_FOOTPRINT_from_ARM7, msg_pre_FOOTPRINT_from_ARM7);
00509 ROS_DEBUG_STREAM("msg_pre_FOOTPRINT_from_ARM7:" << msg_pre_FOOTPRINT_from_ARM7);
00510 }
00511
00512
00513 geometry_msgs::Transform msg_transform_pre_FOOTPRINT_from_ARM7;
00514 tf::transformTFToMsg(transform_pre_FOOTPRINT_from_ARM7, msg_transform_pre_FOOTPRINT_from_ARM7);
00515 geometry_msgs::PoseStamped msg_pose_pre_FOOTPRINT_from_ARM7;
00516 msg_pose_pre_FOOTPRINT_from_ARM7.header.stamp = ros::Time::now();
00517 msg_pose_pre_FOOTPRINT_from_ARM7.header.frame_id = "/base_footprint";
00518 msg_pose_pre_FOOTPRINT_from_ARM7.pose.position.x = msg_transform_pre_FOOTPRINT_from_ARM7.translation.x;
00519 msg_pose_pre_FOOTPRINT_from_ARM7.pose.position.y = msg_transform_pre_FOOTPRINT_from_ARM7.translation.y;
00520 msg_pose_pre_FOOTPRINT_from_ARM7.pose.position.z = msg_transform_pre_FOOTPRINT_from_ARM7.translation.z;
00521 msg_pose_pre_FOOTPRINT_from_ARM7.pose.orientation = msg_transform_pre_FOOTPRINT_from_ARM7.rotation;
00522 if (debug)
00523 {
00524 ROS_DEBUG_STREAM("msg_pose_pre_FOOTPRINT_from_ARM7:" << msg_pose_pre_FOOTPRINT_from_ARM7);
00525 }
00526
00527 g.pre_grasp_approach = calculateApproachDirection(msg_pose_grasp_FOOTPRINT_from_ARM7.pose, msg_pose_pre_FOOTPRINT_from_ARM7.pose);
00528
00529
00530 g.post_grasp_retreat.direction.header.frame_id = g.pre_grasp_approach.direction.header.frame_id;
00531 g.post_grasp_retreat.direction.vector.x = -g.pre_grasp_approach.direction.vector.x;
00532 g.post_grasp_retreat.direction.vector.y = -g.pre_grasp_approach.direction.vector.y;
00533 g.post_grasp_retreat.direction.vector.z = -g.pre_grasp_approach.direction.vector.z + 0.5;
00534
00535 g.post_grasp_retreat.min_distance = g.pre_grasp_approach.min_distance;
00536 g.post_grasp_retreat.desired_distance = g.pre_grasp_approach.desired_distance;
00537
00538 g.post_place_retreat = g.post_grasp_retreat;
00539
00540 grasps.push_back(g);
00541 }
00542
00543
00544
00545
00546 void CobPickPlaceActionServer::fillGraspsOR(unsigned int objectClassId, std::string gripper_type, std::string gripper_side, unsigned int grasp_id, geometry_msgs::PoseStamped object_pose, std::vector<moveit_msgs::Grasp> &grasps)
00547 {
00548 bool finished_before_timeout;
00549 grasps.clear();
00550
00551
00552 if(map_classid_to_classname.find(objectClassId) == map_classid_to_classname.end())
00553 {
00554 ROS_ERROR("Unable to resolve class_name for class_id %d", objectClassId);
00555 return;
00556 }
00557
00558
00559 cob_grasp_generation::QueryGraspsGoal goal_query_grasps;
00560 goal_query_grasps.object_name = map_classid_to_classname.find(objectClassId)->second;
00561 goal_query_grasps.gripper_type = gripper_type;
00562 goal_query_grasps.gripper_side = gripper_side;
00563 goal_query_grasps.grasp_id = grasp_id;
00564 goal_query_grasps.num_grasps = 0;
00565 goal_query_grasps.threshold = 0;
00566
00567 ac_grasps_or->sendGoal(goal_query_grasps);
00568
00569 ROS_INFO("Querying grasps...");
00570 finished_before_timeout = ac_grasps_or->waitForResult(ros::Duration(70.0));
00571 actionlib::SimpleClientGoalState state_grasps_or = ac_grasps_or->getState();
00572 boost::shared_ptr<const cob_grasp_generation::QueryGraspsResult> result_query_grasps = ac_grasps_or->getResult();
00573 if (finished_before_timeout)
00574 {
00575 ROS_INFO("Action finished: %s",state_grasps_or.toString().c_str());
00576
00577 ROS_INFO("Found %lu grasps for this object", result_query_grasps.get()->grasp_list.size());
00578 for(unsigned int i=0; i<result_query_grasps.get()->grasp_list.size(); i++)
00579 {
00580 ROS_DEBUG_STREAM("Grasp "<< i << ": " << result_query_grasps.get()->grasp_list[i]);
00581 }
00582
00583 for(unsigned int i=0; i<result_query_grasps.get()->grasp_list.size(); i++)
00584 {
00585 moveit_msgs::Grasp current_grasp;
00586
00587 current_grasp.grasp_posture = result_query_grasps.get()->grasp_list[i].grasp_posture;
00588
00589
00590
00591
00592
00593
00594 current_grasp.pre_grasp_posture = result_query_grasps.get()->grasp_list[i].pre_grasp_posture;
00595
00596
00598
00599
00600 geometry_msgs::Pose current_grasp_pose = result_query_grasps.get()->grasp_list[i].grasp_pose.pose;
00601 tf::Transform transform_grasp_O_from_SDH = tf::Transform(
00602 tf::Quaternion(current_grasp_pose.orientation.x, current_grasp_pose.orientation.y, current_grasp_pose.orientation.z, current_grasp_pose.orientation.w),
00603 tf::Vector3(current_grasp_pose.position.x, current_grasp_pose.position.y, current_grasp_pose.position.z));
00604
00605
00606 geometry_msgs::Transform msg_grasp_O_from_SDH;
00607 tf::transformTFToMsg(transform_grasp_O_from_SDH, msg_grasp_O_from_SDH);
00608 ROS_DEBUG_STREAM("msg_grasp_O_from_SDH:" << msg_grasp_O_from_SDH);
00609
00610
00611 tf::Transform transform_HEADER_from_O = tf::Transform(
00612 tf::Quaternion(object_pose.pose.orientation.x, object_pose.pose.orientation.y, object_pose.pose.orientation.z, object_pose.pose.orientation.w),
00613 tf::Vector3(object_pose.pose.position.x, object_pose.pose.position.y, object_pose.pose.position.z));
00614
00615 geometry_msgs::Transform msg_HEADER_from_O;
00616 tf::transformTFToMsg(transform_HEADER_from_O, msg_HEADER_from_O);
00617 ROS_DEBUG_STREAM("msg_HEADER_from_O:" << msg_HEADER_from_O);
00618
00619
00620 tf::Transform transform_grasp_FOOTPRINT_from_ARM7 = transformPose(transform_grasp_O_from_SDH, transform_HEADER_from_O, object_pose.header.frame_id);
00621
00622 geometry_msgs::Transform msg_grasp_FOOTPRINT_from_ARM7;
00623 tf::transformTFToMsg(transform_grasp_FOOTPRINT_from_ARM7, msg_grasp_FOOTPRINT_from_ARM7);
00624 ROS_DEBUG_STREAM("msg_grasp_FOOTPRINT_from_ARM7:" << msg_grasp_FOOTPRINT_from_ARM7);
00625
00626
00627 geometry_msgs::Transform msg_transform_grasp_FOOTPRINT_from_ARM7;
00628 tf::transformTFToMsg(transform_grasp_FOOTPRINT_from_ARM7, msg_transform_grasp_FOOTPRINT_from_ARM7);
00629 geometry_msgs::PoseStamped msg_pose_grasp_FOOTPRINT_from_ARM7;
00630 msg_pose_grasp_FOOTPRINT_from_ARM7.header.stamp = ros::Time::now();
00631 msg_pose_grasp_FOOTPRINT_from_ARM7.header.frame_id = "/base_footprint";
00632 msg_pose_grasp_FOOTPRINT_from_ARM7.pose.position.x = msg_transform_grasp_FOOTPRINT_from_ARM7.translation.x;
00633 msg_pose_grasp_FOOTPRINT_from_ARM7.pose.position.y = msg_transform_grasp_FOOTPRINT_from_ARM7.translation.y;
00634 msg_pose_grasp_FOOTPRINT_from_ARM7.pose.position.z = msg_transform_grasp_FOOTPRINT_from_ARM7.translation.z;
00635 msg_pose_grasp_FOOTPRINT_from_ARM7.pose.orientation = msg_transform_grasp_FOOTPRINT_from_ARM7.rotation;
00636 ROS_DEBUG_STREAM("msg_pose_grasp_FOOTPRINT_from_ARM7:" << msg_pose_grasp_FOOTPRINT_from_ARM7);
00637
00638 current_grasp.grasp_pose = msg_pose_grasp_FOOTPRINT_from_ARM7;
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649 ROS_INFO_STREAM("EndeffectorLink: " << group.getEndEffectorLink());
00650 current_grasp.pre_grasp_approach.direction.header.frame_id = group.getEndEffectorLink();
00651 current_grasp.pre_grasp_approach.direction.vector.x = 0.0;
00652 current_grasp.pre_grasp_approach.direction.vector.y = 0.0;
00653 current_grasp.pre_grasp_approach.direction.vector.z = 1.0;
00654 current_grasp.pre_grasp_approach.min_distance = 0.18;
00655 current_grasp.pre_grasp_approach.desired_distance = 0.28;
00656
00657
00658
00659
00660
00661
00662
00663
00664
00665 current_grasp.post_grasp_retreat.direction.header.frame_id = "/base_footprint";
00666 current_grasp.post_grasp_retreat.direction.vector.x = 0.0;
00667 current_grasp.post_grasp_retreat.direction.vector.y = 0.0;
00668 current_grasp.post_grasp_retreat.direction.vector.z = 1.0;
00669 current_grasp.post_grasp_retreat.min_distance = 0.05;
00670 current_grasp.post_grasp_retreat.desired_distance = 0.1;
00671
00672
00673
00674
00675
00676
00677
00678
00679 current_grasp.post_place_retreat = current_grasp.post_grasp_retreat;
00680
00681 grasps.push_back(current_grasp);
00682 }
00683
00684 }
00685 else
00686 ROS_ERROR("Grasps not queried within timeout");
00687 }
00688
00689
00690
00691
00692
00693
00694 trajectory_msgs::JointTrajectory CobPickPlaceActionServer::MapHandConfiguration(sensor_msgs::JointState table_config)
00695 {
00696 trajectory_msgs::JointTrajectory grasp_configuration;
00697
00698 trajectory_msgs::JointTrajectoryPoint point;
00699 point.positions.assign(7,0.0);
00700 point.velocities.assign(7,0.0);
00701 point.accelerations.assign(7,0.0);
00702 point.effort.assign(7,0.0);
00703 point.time_from_start = ros::Duration(3.0);
00704
00705 point.positions[0] = table_config.position[0];
00706 point.positions[1] = table_config.position[2];
00707 point.positions[2] = table_config.position[3];
00708 point.positions[3] = table_config.position[5];
00709 point.positions[4] = table_config.position[6];
00710 point.positions[5] = table_config.position[11];
00711 point.positions[6] = table_config.position[12];
00712 grasp_configuration.points.push_back(point);
00713
00714 grasp_configuration.joint_names.resize(7);
00715 grasp_configuration.joint_names[0]=("sdh_knuckle_joint");
00716 grasp_configuration.joint_names[1]=("sdh_thumb_2_joint");
00717 grasp_configuration.joint_names[2]=("sdh_thumb_3_joint");
00718 grasp_configuration.joint_names[3]=("sdh_finger_12_joint");
00719 grasp_configuration.joint_names[4]=("sdh_finger_13_joint");
00720 grasp_configuration.joint_names[5]=("sdh_finger_22_joint");
00721 grasp_configuration.joint_names[6]=("sdh_finger_23_joint");
00722
00723
00724 for(unsigned int i=0; i<grasp_configuration.points[0].positions.size();i++)
00725 {
00726 if(grasp_configuration.points[0].positions[i]>1.57079){grasp_configuration.points[0].positions[i]=1.57079;}
00727 if(grasp_configuration.points[0].positions[i]<-1.57079){grasp_configuration.points[0].positions[i]=-1.57079;}
00728 }
00729 return grasp_configuration;
00730 }
00731
00732 tf::Transform CobPickPlaceActionServer::transformPose(tf::Transform transform_O_from_SDH, tf::Transform transform_HEADER_from_O, std::string object_frame_id)
00733 {
00734 bool debug = false;
00735
00736
00737 tf::StampedTransform transform_SDH_from_ARM7;
00738
00739 bool transform_available = false;
00740 while(!transform_available)
00741 {
00742 try{
00744
00745 tf_listener_.lookupTransform("/gripper_left_palm_link", group.getEndEffectorLink(), ros::Time(0), transform_SDH_from_ARM7);
00746 transform_available = true;
00747 }
00748 catch (tf::TransformException ex){
00749 ROS_WARN("Waiting for transform...(%s)",ex.what());
00750 ros::Duration(0.1).sleep();
00751 }
00752 }
00753
00754
00755 geometry_msgs::TransformStamped msg_SDH_from_ARM7;
00756 tf::transformStampedTFToMsg(transform_SDH_from_ARM7, msg_SDH_from_ARM7);
00757
00758 if (debug)
00759 ROS_DEBUG_STREAM("msg_SDH_from_ARM7:" << msg_SDH_from_ARM7);
00760
00761
00762 tf::Transform transform_O_from_ARM7 = transform_O_from_SDH * transform_SDH_from_ARM7;
00763
00764
00765 tf::StampedTransform transform_FOOTPRINT_from_HEADER;
00766 try
00767 {
00768 ros::Time now = ros::Time::now();
00769 tf_listener_.waitForTransform("/base_footprint", object_frame_id, now, ros::Duration(10.0));
00770 tf_listener_.lookupTransform("/base_footprint", object_frame_id, now, transform_FOOTPRINT_from_HEADER);
00771 }
00772 catch (tf::TransformException ex)
00773 {
00774 ROS_ERROR("%s",ex.what());
00775 }
00776
00777 geometry_msgs::TransformStamped msg_FOOTPRINT_from_HEADER;
00778 tf::transformStampedTFToMsg(transform_FOOTPRINT_from_HEADER, msg_FOOTPRINT_from_HEADER);
00779
00780 if (debug)
00781 ROS_DEBUG_STREAM("msg_FOOTPRINT_from_HEADER:" << msg_FOOTPRINT_from_HEADER);
00782
00783
00784 tf::Transform transform_FOOTPRINT_from_O = transform_FOOTPRINT_from_HEADER * transform_HEADER_from_O;
00785
00786
00787 tf::Transform transform_FOOTPRINT_from_ARM7 = transform_FOOTPRINT_from_O * transform_O_from_ARM7;
00788
00789 return transform_FOOTPRINT_from_ARM7;
00790 }
00791
00792 moveit_msgs::GripperTranslation CobPickPlaceActionServer::calculateApproachDirection(geometry_msgs::Pose msg_pose_grasp_FOOTPRINT_from_ARM7, geometry_msgs::Pose msg_pose_pre_FOOTPRINT_from_ARM7)
00793 {
00794 double finger_length = 0.18;
00795 moveit_msgs::GripperTranslation approach;
00796 approach.direction.header.frame_id = "/base_footprint";
00797
00798
00799
00800 double distance = sqrt(
00801 pow((msg_pose_grasp_FOOTPRINT_from_ARM7.position.x-msg_pose_pre_FOOTPRINT_from_ARM7.position.x),2) +
00802 pow((msg_pose_grasp_FOOTPRINT_from_ARM7.position.y-msg_pose_pre_FOOTPRINT_from_ARM7.position.y),2) +
00803 pow((msg_pose_grasp_FOOTPRINT_from_ARM7.position.z-msg_pose_pre_FOOTPRINT_from_ARM7.position.z),2));
00804
00805 ROS_DEBUG("Distance between pre-grasp and grasp: %f", distance);
00806
00807 approach.direction.vector.x = (msg_pose_grasp_FOOTPRINT_from_ARM7.position.x-msg_pose_pre_FOOTPRINT_from_ARM7.position.x)/distance;
00808 approach.direction.vector.y = (msg_pose_grasp_FOOTPRINT_from_ARM7.position.y-msg_pose_pre_FOOTPRINT_from_ARM7.position.y)/distance;
00809 approach.direction.vector.z = (msg_pose_grasp_FOOTPRINT_from_ARM7.position.z-msg_pose_pre_FOOTPRINT_from_ARM7.position.z)/distance;
00810
00811
00812 if(distance < finger_length)
00813 {
00814 approach.min_distance = finger_length;
00815 }
00816 else
00817 {
00818 approach.min_distance = distance;
00819 }
00820 approach.desired_distance = approach.min_distance + 0.1;
00821
00822 return approach;
00823 }
00824
00825
00826
00827
00828
00829
00830
00831
00832
00833
00834
00835
00836
00837
00838
00839 int main(int argc, char **argv)
00840 {
00841 ros::init (argc, argv, "cob_pick_place_action_server_node");
00842 if( argc != 2 )
00843 {
00844 ROS_ERROR("No manipulator specified!");
00845 return -1;
00846 }
00847 else
00848 {
00849 ROS_INFO("Starting PickPlace ActionServer for group %s", argv[1]);
00850 }
00851
00852 CobPickPlaceActionServer *cob_pick_place_action_server = new CobPickPlaceActionServer(std::string(argv[1]));
00853
00854 cob_pick_place_action_server->initialize();
00855 cob_pick_place_action_server->run();
00856
00857 return 0;
00858 }