collision_proximity: collision_proximity: collision_proximity_speed

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
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) ); // initialize random seed
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<btVector3> > 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){//in_prox_collision && !ode_in_collision) {
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         //cps->visualizeVoxelizedLinks(collision_models->getGroupLinkUnion());
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       //cps->visualizeVoxelizedLinks(collision_models->getGroupLinkUnion());
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<btVector3> > 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 //ROS_INFO_STREAM("Setup prox " << tot_prox_setup.toSec()/(TEST_NUM*1.0) << " ode " << tot_ode_setup.toSec()/(TEST_NUM*1.0));
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 }
collision_proximity_speed_test.cpp
