Go to the documentation of this file.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 #include <ros/ros.h>
00041 #include <geometry_msgs/PoseArray.h>
00042 #include <sensor_msgs/JointState.h>
00043 #include <Eigen/Core>
00044 #include <Eigen/Geometry>
00045
00046
00047 #include <moveit_msgs/MoveGroupAction.h>
00048 #include <moveit_msgs/DisplayTrajectory.h>
00049 #include <moveit_msgs/RobotState.h>
00050 #include <moveit/kinematic_constraints/utils.h>
00051 #include <moveit/robot_state/robot_state.h>
00052 #include <moveit/robot_state/conversions.h>
00053 #include <moveit/robot_model_loader/robot_model_loader.h>
00054
00055
00056 #include <visualization_msgs/Marker.h>
00057 #include <visualization_msgs/MarkerArray.h>
00058
00059
00060 #include <moveit_simple_grasps/simple_grasps.h>
00061 #include <moveit_simple_grasps/grasp_filter.h>
00062 #include <moveit_visual_tools/visual_tools.h>
00063
00064
00065 #include <moveit_simple_grasps/grasp_data.h>
00066 #include <moveit_simple_grasps/custom_environment2.h>
00067
00068 namespace moveit_simple_grasps
00069 {
00070
00071
00072 static const double TABLE_HEIGHT = .92;
00073 static const double TABLE_WIDTH = .85;
00074 static const double TABLE_DEPTH = .47;
00075 static const double TABLE_X = 0.66;
00076 static const double TABLE_Y = 0;
00077 static const double TABLE_Z = -0.9/2+0.01;
00078
00079 static const double BLOCK_SIZE = 0.04;
00080
00081 class GraspGeneratorTest
00082 {
00083 private:
00084
00085 ros::NodeHandle nh_;
00086
00087
00088 moveit_simple_grasps::SimpleGraspsPtr simple_grasps_;
00089
00090
00091 moveit_visual_tools::VisualToolsPtr visual_tools_;
00092
00093
00094 moveit_simple_grasps::GraspFilterPtr grasp_filter_;
00095
00096
00097 moveit_simple_grasps::GraspData grasp_data_;
00098
00099
00100 planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
00101
00102
00103 std::string arm_;
00104 std::string ee_group_name_;
00105 std::string planning_group_name_;
00106
00107 public:
00108
00109
00110 GraspGeneratorTest(int num_tests)
00111 : nh_("~")
00112 {
00113
00114 nh_.param("arm", arm_, std::string("left"));
00115 nh_.param("ee_group_name", ee_group_name_, std::string(arm_ + "_hand"));
00116 planning_group_name_ = arm_ + "_arm";
00117
00118 ROS_INFO_STREAM_NAMED("test","Arm: " << arm_);
00119 ROS_INFO_STREAM_NAMED("test","End Effector: " << ee_group_name_);
00120 ROS_INFO_STREAM_NAMED("test","Planning Group: " << planning_group_name_);
00121
00122
00123
00124 if (!grasp_data_.loadRobotGraspData(nh_, ee_group_name_))
00125 ros::shutdown();
00126
00127
00128
00129 planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor("robot_description"));
00130
00131
00132
00133 visual_tools_.reset(new moveit_visual_tools::VisualTools(grasp_data_.base_link_, "/end_effector_marker", planning_scene_monitor_));
00134 visual_tools_->setLifetime(40.0);
00135 visual_tools_->setMuted(false);
00136 visual_tools_->loadEEMarker(grasp_data_.ee_group_, planning_group_name_);
00137 visual_tools_->setFloorToBaseHeight(-0.9);
00138
00139
00140
00141
00142
00143 visual_tools_->publishCollisionTable(TABLE_X, TABLE_Y, 0, TABLE_WIDTH, TABLE_HEIGHT, TABLE_DEPTH, "table");
00144
00145
00146
00147 simple_grasps_.reset( new moveit_simple_grasps::SimpleGrasps(visual_tools_) );
00148
00149
00150
00151 robot_state::RobotState robot_state = planning_scene_monitor_->getPlanningScene()->getCurrentState();
00152 grasp_filter_.reset(new moveit_simple_grasps::GraspFilter(robot_state, visual_tools_) );
00153
00154
00155
00156 geometry_msgs::Pose object_pose;
00157 std::vector<moveit_msgs::Grasp> possible_grasps;
00158 std::vector<trajectory_msgs::JointTrajectoryPoint> ik_solutions;
00159
00160
00161 for (int i = 0; i < num_tests; ++i)
00162 {
00163 ROS_INFO_STREAM_NAMED("test","Adding random object " << i+1 << " of " << num_tests);
00164
00165
00166 if (num_tests == 1)
00167 generateTestObject(object_pose);
00168 else
00169 generateRandomObject(object_pose);
00170
00171
00172 visual_tools_->publishBlock(object_pose, moveit_visual_tools::BLUE, BLOCK_SIZE);
00173
00174 possible_grasps.clear();
00175 ik_solutions.clear();
00176
00177
00178 simple_grasps_->generateBlockGrasps( object_pose, grasp_data_, possible_grasps);
00179
00180
00181 bool filter_pregrasps = true;
00182 grasp_filter_->filterGrasps(possible_grasps, ik_solutions, filter_pregrasps, grasp_data_.ee_parent_link_, planning_group_name_);
00183
00184
00185 visual_tools_->publishAnimatedGrasps(possible_grasps, grasp_data_.ee_parent_link_);
00186 visual_tools_->publishIKSolutions(ik_solutions, planning_group_name_, 0.25);
00187
00188
00189 if(!ros::ok())
00190 break;
00191 }
00192
00193
00194 }
00195
00196 void generateTestObject(geometry_msgs::Pose& object_pose)
00197 {
00198
00199 geometry_msgs::Pose start_object_pose;
00200 geometry_msgs::Pose end_object_pose;
00201
00202 start_object_pose.position.x = 0.8;
00203 start_object_pose.position.y = -0.5;
00204 start_object_pose.position.z = 0.02;
00205
00206 end_object_pose.position.x = 0.25;
00207 end_object_pose.position.y = 0.15;
00208 end_object_pose.position.z = 0.02;
00209
00210
00211 double angle = M_PI / 1.5;
00212 Eigen::Quaterniond quat(Eigen::AngleAxis<double>(double(angle), Eigen::Vector3d::UnitZ()));
00213 start_object_pose.orientation.x = quat.x();
00214 start_object_pose.orientation.y = quat.y();
00215 start_object_pose.orientation.z = quat.z();
00216 start_object_pose.orientation.w = quat.w();
00217
00218 angle = M_PI / 1.1;
00219 quat = Eigen::Quaterniond(Eigen::AngleAxis<double>(double(angle), Eigen::Vector3d::UnitZ()));
00220 end_object_pose.orientation.x = quat.x();
00221 end_object_pose.orientation.y = quat.y();
00222 end_object_pose.orientation.z = quat.z();
00223 end_object_pose.orientation.w = quat.w();
00224
00225
00226 object_pose = start_object_pose;
00227 }
00228
00229 void generateRandomObject(geometry_msgs::Pose& object_pose)
00230 {
00231
00232 object_pose.position.x = visual_tools_->dRand(0.7,TABLE_DEPTH);
00233 object_pose.position.y = visual_tools_->dRand(-TABLE_WIDTH/2,-0.1);
00234 object_pose.position.z = TABLE_Z + TABLE_HEIGHT / 2.0 + BLOCK_SIZE / 2.0;
00235
00236
00237 double angle = M_PI * visual_tools_->dRand(0.1,1);
00238 Eigen::Quaterniond quat(Eigen::AngleAxis<double>(double(angle), Eigen::Vector3d::UnitZ()));
00239 object_pose.orientation.x = quat.x();
00240 object_pose.orientation.y = quat.y();
00241 object_pose.orientation.z = quat.z();
00242 object_pose.orientation.w = quat.w();
00243 }
00244
00245 };
00246
00247 }
00248
00249
00250 int main(int argc, char *argv[])
00251 {
00252 int num_tests = 5;
00253
00254 ros::init(argc, argv, "grasp_generator_test");
00255
00256
00257 ros::AsyncSpinner spinner(2);
00258 spinner.start();
00259
00260
00261 bool verbose = false;
00262 if (argc > 1)
00263 {
00264 for (std::size_t i = 0; i < argc; ++i)
00265 {
00266 if (strcmp(argv[i], "--verbose") == 0)
00267 {
00268 ROS_INFO_STREAM_NAMED("main","Running in VERBOSE mode (much slower)");
00269 verbose = true;
00270 }
00271 }
00272 }
00273
00274
00275 srand(ros::Time::now().toSec());
00276
00277
00278 ros::Time start_time;
00279 start_time = ros::Time::now();
00280
00281
00282 moveit_simple_grasps::GraspGeneratorTest tester(num_tests);
00283
00284
00285 double duration = (ros::Time::now() - start_time).toNSec() * 1e-6;
00286 ROS_INFO_STREAM_NAMED("","Total time: " << duration);
00287 std::cout << duration << "\t" << num_tests << std::endl;
00288
00289 ros::Duration(1.0).sleep();
00290
00291 return 0;
00292 }