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
00037 #include <ros/ros.h>
00038 #include <tf/transform_listener.h>
00039 #include <planning_environment/monitors/planning_monitor.h>
00040 #include <collision_proximity/collision_proximity_space.h>
00041 #include <kinematics_msgs/GetKinematicSolverInfo.h>
00042 #include <arm_navigation_msgs/GetPlanningScene.h>
00043 #include <arm_navigation_msgs/GetRobotState.h>
00044 #include <planning_environment/models/model_utils.h>
00045
00046 static const std::string planning_scene_name = "/environment_server/get_planning_scene";
00047 static const std::string robot_state_name = "/environment_server/get_robot_state";
00048 static const std::string ARM_QUERY_NAME = "/pr2_right_arm_kinematics/get_ik_solver_info";
00049
00050 static const unsigned int TEST_NUM = 10000;
00051
00052 double gen_rand(double min, double max)
00053 {
00054 int rand_num = rand()%100+1;
00055 double result = min + (double)((max-min)*rand_num)/101.0;
00056 return result;
00057 }
00058
00059 int main(int argc, char** argv)
00060 {
00061 ros::init(argc, argv, "collision_proximity_test");
00062
00063 ros::NodeHandle nh;
00064
00065 std::string robot_description_name = nh.resolveName("robot_description", true);
00066
00067 srand ( time(NULL) );
00068 ros::service::waitForService(ARM_QUERY_NAME);
00069
00070 ros::ServiceClient query_client = nh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>(ARM_QUERY_NAME);
00071
00072 kinematics_msgs::GetKinematicSolverInfo::Request request;
00073 kinematics_msgs::GetKinematicSolverInfo::Response response;
00074
00075 query_client.call(request, response);
00076 int num_joints;
00077 std::vector<double> min_limits, max_limits;
00078 num_joints = (int) response.kinematic_solver_info.joint_names.size();
00079 std::vector<std::string> joint_state_names = response.kinematic_solver_info.joint_names;
00080 std::map<std::string, double> joint_state_map;
00081 for(int i=0; i< num_joints; i++)
00082 {
00083 joint_state_map[joint_state_names[i]] = 0.0;
00084 min_limits.push_back(response.kinematic_solver_info.limits[i].min_position);
00085 max_limits.push_back(response.kinematic_solver_info.limits[i].max_position);
00086 }
00087
00088 std::vector<std::vector<double> > valid_joint_states;
00089 valid_joint_states.resize(TEST_NUM);
00090 for(unsigned int i = 0; i < TEST_NUM; i++) {
00091 std::vector<double> jv(7,0.0);
00092 for(unsigned int j=0; j < min_limits.size(); j++)
00093 {
00094 jv[j] = gen_rand(std::max(min_limits[j],-M_PI),std::min(max_limits[j],M_PI));
00095 }
00096 valid_joint_states[i] = jv;
00097 }
00098
00099 collision_proximity::CollisionProximitySpace* cps = new collision_proximity::CollisionProximitySpace(robot_description_name);
00100 ros::ServiceClient planning_scene_client = nh.serviceClient<arm_navigation_msgs::GetPlanningScene>(planning_scene_name, true);
00101 ros::service::waitForService(planning_scene_name);
00102
00103 arm_navigation_msgs::GetPlanningScene::Request ps_req;
00104 arm_navigation_msgs::GetPlanningScene::Response ps_res;
00105
00106 arm_navigation_msgs::CollisionObject obj1;
00107 obj1.header.stamp = ros::Time::now();
00108 obj1.header.frame_id = "odom_combined";
00109 obj1.id = "obj1";
00110 obj1.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00111 obj1.shapes.resize(1);
00112 obj1.shapes[0].type = arm_navigation_msgs::Shape::BOX;
00113 obj1.shapes[0].dimensions.resize(3);
00114 obj1.shapes[0].dimensions[0] = .1;
00115 obj1.shapes[0].dimensions[1] = .1;
00116 obj1.shapes[0].dimensions[2] = .75;
00117 obj1.poses.resize(1);
00118 obj1.poses[0].position.x = .6;
00119 obj1.poses[0].position.y = -.6;
00120 obj1.poses[0].position.z = .375;
00121 obj1.poses[0].orientation.w = 1.0;
00122
00123 arm_navigation_msgs::AttachedCollisionObject att_obj;
00124 att_obj.object = obj1;
00125 att_obj.object.header.stamp = ros::Time::now();
00126 att_obj.object.header.frame_id = "r_gripper_palm_link";
00127 att_obj.link_name = "r_gripper_palm_link";
00128 att_obj.touch_links.push_back("r_gripper_palm_link");
00129 att_obj.touch_links.push_back("r_gripper_r_finger_link");
00130 att_obj.touch_links.push_back("r_gripper_l_finger_link");
00131 att_obj.touch_links.push_back("r_gripper_r_finger_tip_link");
00132 att_obj.touch_links.push_back("r_gripper_l_finger_tip_link");
00133 att_obj.touch_links.push_back("r_wrist_roll_link");
00134 att_obj.touch_links.push_back("r_wrist_flex_link");
00135 att_obj.touch_links.push_back("r_forearm_link");
00136 att_obj.touch_links.push_back("r_gripper_motor_accelerometer_link");
00137 att_obj.object.id = "obj2";
00138 att_obj.object.shapes[0].type = arm_navigation_msgs::Shape::CYLINDER;
00139 att_obj.object.shapes[0].dimensions.resize(2);
00140 att_obj.object.shapes[0].dimensions[0] = .025;
00141 att_obj.object.shapes[0].dimensions[1] = .5;
00142 att_obj.object.poses.resize(1);
00143 att_obj.object.poses[0].position.x = .12;
00144 att_obj.object.poses[0].position.y = 0.0;
00145 att_obj.object.poses[0].position.z = 0.0;
00146 att_obj.object.poses[0].orientation.w = 1.0;
00147
00148 ps_req.collision_object_diffs.push_back(obj1);
00149 ps_req.attached_collision_object_diffs.push_back(att_obj);
00150
00151 arm_navigation_msgs::GetRobotState::Request rob_state_req;
00152 arm_navigation_msgs::GetRobotState::Response rob_state_res;
00153
00154 ros::Rate slow_wait(1.0);
00155 while(1) {
00156 if(!nh.ok()) {
00157 ros::shutdown();
00158 exit(0);
00159 }
00160 planning_scene_client.call(ps_req,ps_res);
00161 ROS_INFO_STREAM("Num coll " << ps_res.all_collision_objects.size() << " att " << ps_res.all_attached_collision_objects.size());
00162 if(ps_res.all_collision_objects.size() > 0
00163 && ps_res.all_attached_collision_objects.size() > 0) break;
00164 slow_wait.sleep();
00165 }
00166
00167 ROS_INFO("After test");
00168
00169 ros::ServiceClient robot_state_service = nh.serviceClient<arm_navigation_msgs::GetRobotState>(robot_state_name, true);
00170 ros::service::waitForService(robot_state_name);
00171
00172 planning_models::KinematicState* state = cps->setupForGroupQueries("right_arm_and_end_effector",
00173 ps_res.complete_robot_state,
00174 ps_res.allowed_collision_matrix,
00175 ps_res.transformed_allowed_contacts,
00176 ps_res.all_link_padding,
00177 ps_res.all_collision_objects,
00178 ps_res.all_attached_collision_objects,
00179 ps_res.unmasked_collision_map);
00180
00181 ros::WallDuration tot_ode, tot_prox;
00182 ros::WallDuration min_ode(1000.0,1000.0);
00183 ros::WallDuration min_prox(1000.0, 1000.0);
00184 ros::WallDuration max_ode;
00185 ros::WallDuration max_prox;
00186
00187 std::vector<double> link_distances;
00188 std::vector<std::vector<double> > distances;
00189 std::vector<std::vector<tf::Vector3> > gradients;
00190 std::vector<std::string> link_names;
00191 std::vector<std::string> attached_body_names;
00192 std::vector<collision_proximity::CollisionType> collisions;
00193 unsigned int prox_num_in_collision = 0;
00194 unsigned int ode_num_in_collision = 0;
00195
00196 ros::WallTime n1, n2;
00197
00198 for(unsigned int i = 0; i < TEST_NUM; i++) {
00199 for(unsigned int j = 0; j < joint_state_names.size(); j++) {
00200 joint_state_map[joint_state_names[j]] = valid_joint_states[i][j];
00201 }
00202 state->setKinematicState(joint_state_map);
00203 n1 = ros::WallTime::now();
00204 cps->setCurrentGroupState(*state);
00205 bool in_prox_collision = cps->isStateInCollision();
00206 n2 = ros::WallTime::now();
00207 if(in_prox_collision) {
00208 prox_num_in_collision++;
00209 }
00210 ros::WallDuration prox_dur(n2-n1);
00211 if(prox_dur > max_prox) {
00212 max_prox = prox_dur;
00213 } else if (prox_dur < min_prox) {
00214 min_prox = prox_dur;
00215 }
00216 tot_prox += prox_dur;
00217 n1 = ros::WallTime::now();
00218 bool ode_in_collision = cps->getCollisionModels()->isKinematicStateInCollision(*state);
00219 n2 = ros::WallTime::now();
00220 if(ode_in_collision) {
00221 ode_num_in_collision++;
00222 }
00223 if(0){
00224 ros::Rate r(1.0);
00225 while(nh.ok()) {
00226 cps->getStateCollisions(link_names, attached_body_names, in_prox_collision, collisions);
00227 ROS_INFO("Prox not ode");
00228 cps->visualizeDistanceField();
00229 cps->visualizeCollisions(link_names, attached_body_names, collisions);
00230 cps->visualizeConvexMeshes(cps->getCollisionModels()->getGroupLinkUnion());
00231 std::vector<std::string> objs = link_names;
00232 objs.insert(objs.end(), attached_body_names.begin(), attached_body_names.end());
00233 cps->visualizeObjectSpheres(objs);
00234
00235 r.sleep();
00236 }
00237 exit(0);
00238 }
00239 if(0 && !in_prox_collision && ode_in_collision) {
00240 ros::Rate r(1.0);
00241 while(nh.ok()) {
00242 ROS_INFO("Ode not prox");
00243 cps->visualizeDistanceField();
00244 cps->getStateCollisions(link_names, attached_body_names, in_prox_collision, collisions);
00245 cps->visualizeCollisions(link_names, attached_body_names, collisions);
00246 cps->visualizeConvexMeshes(cps->getCollisionModels()->getGroupLinkUnion());
00247 std::vector<std::string> objs = link_names;
00248 objs.insert(objs.end(), attached_body_names.begin(), attached_body_names.end());
00249 cps->visualizeObjectSpheres(objs);
00250 r.sleep();
00251 }
00252
00253 std::vector<collision_space::EnvironmentModel::AllowedContact> allowed_contacts;
00254 std::vector<collision_space::EnvironmentModel::Contact> contact;
00255 cps->getCollisionModels()->getCollisionSpace()->getCollisionContacts(allowed_contacts, contact, 10);
00256 for(unsigned int i = 0; i < contact.size(); i++) {
00257 std::string name1;
00258 std::string name2;
00259 if(contact[i].link1 != NULL) {
00260 if(contact[i].link1_attached_body_index != 0) {
00261 name1 = contact[i].link1->getAttachedBodyModels()[contact[i].link1_attached_body_index-1]->getName();
00262 } else {
00263 name1 = contact[i].link1->getName();
00264 }
00265 }
00266 if(contact[i].link2 != NULL) {
00267 if(contact[i].link2_attached_body_index != 0) {
00268 name2 = contact[i].link2->getAttachedBodyModels()[contact[i].link2_attached_body_index-1]->getName();
00269 } else {
00270 name2 = contact[i].link2->getName();
00271 }
00272 } else if (!contact[i].object_name.empty()) {
00273 name2 = contact[i].object_name;
00274 }
00275 ROS_INFO_STREAM("Contact " << i << " between " << name1 << " and " << name2);
00276 }
00277 exit(0);
00278 if(0) {
00279 std::vector<double> prox_link_distances;
00280 std::vector<std::vector<double> > prox_distances;
00281 std::vector<std::vector<tf::Vector3> > prox_gradients;
00282 std::vector<std::string> prox_link_names;
00283 std::vector<std::string> prox_attached_body_names;
00284 cps->getStateGradients(prox_link_names, prox_attached_body_names, prox_link_distances, prox_distances, prox_gradients);
00285 ROS_INFO_STREAM("Link size " << prox_link_names.size());
00286 for(unsigned int i = 0; i < prox_link_names.size(); i++) {
00287 ROS_INFO_STREAM("Link " << prox_link_names[i] << " closest distance " << prox_link_distances[i]);
00288 }
00289 for(unsigned int i = 0; i < prox_attached_body_names.size(); i++) {
00290 ROS_INFO_STREAM("Attached body names " << prox_attached_body_names[i] << " closest distance " << prox_link_distances[i+prox_link_names.size()]);
00291 }
00292 exit(0);
00293 }
00294 }
00295 ros::WallDuration ode_dur(n2-n1);
00296 if(ode_dur > max_ode) {
00297 max_ode = ode_dur;
00298 } else if (ode_dur < min_ode) {
00299 min_ode = ode_dur;
00300 }
00301 tot_ode += ode_dur;
00302
00303 }
00304
00305 ROS_INFO_STREAM("Av prox time " << (tot_prox.toSec()/(TEST_NUM*1.0)) << " min " << min_prox.toSec() << " max " << max_prox.toSec()
00306 << " percent in coll " << (prox_num_in_collision*1.0)/(TEST_NUM*1.0));
00307 ROS_INFO_STREAM("Av ode time " << (tot_ode.toSec()/(TEST_NUM*1.0)) << " min " << min_ode.toSec() << " max " << max_ode.toSec()
00308 << " percent in coll " << (ode_num_in_collision*1.0)/(TEST_NUM*1.0));
00309
00310 ros::shutdown();
00311
00312 delete cps;
00313
00314 }