ik_tester_fast.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 *  Copyright (c) 2009, Willow Garage, Inc.
00004 *  All rights reserved.
00005 *
00006 *  Redistribution and use in source and binary forms, with or without
00007 *  modification, are permitted provided that the following conditions
00008 *  are met:
00009 *
00010 *   * Redistributions of source code must retain the above copyright
00011 *     notice, this list of conditions and the following disclaimer.
00012 *   * Redistributions in binary form must reproduce the above
00013 *     copyright notice, this list of conditions and the following
00014 *     disclaimer in the documentation and/or other materials provided
00015 *     with the distribution.
00016 *   * Neither the name of the Willow Garage nor the names of its
00017 *     contributors may be used to endorse or promote products derived
00018 *     from this software without specific prior written permission.
00019 *
00020 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
00031 *  POSSIBILITY OF SUCH DAMAGE.
00032 *********************************************************************/
00033 
00034 // Author(s): Kaijen Hsiao (code adapted from Gil's fast grasp tester)
00035 
00036 #include "object_manipulator/tools/hand_description.h"
00037 #include "object_manipulator/tools/vector_tools.h"
00038 #include "object_manipulator/tools/mechanism_interface.h"
00039 #include "object_manipulator/tools/ik_tester_fast.h"
00040 
00041 using arm_navigation_msgs::ArmNavigationErrorCodes;
00042 
00043 namespace object_manipulator {
00044 
00045   IKTesterFast::IKTesterFast(planning_environment::CollisionModels* cm,
00046                                    const std::string& plugin_name) 
00047   : redundancy_(2),
00048     cm_(cm),
00049     state_(NULL),
00050     kinematics_loader_("kinematics_base","kinematics::KinematicsBase")
00051 {  
00052   ros::NodeHandle nh;
00053 
00054   vis_marker_publisher_ = nh.advertise<visualization_msgs::Marker> ("ik_tester_fast", 128);
00055   vis_marker_array_publisher_ = nh.advertise<visualization_msgs::MarkerArray> ("ik_tester_fast_array", 128);
00056 
00057   const std::map<std::string, planning_models::KinematicModel::GroupConfig>& group_config_map = 
00058     getCollisionModels()->getKinematicModel()->getJointModelGroupConfigMap();
00059 
00060   for(std::map<std::string, planning_models::KinematicModel::GroupConfig>::const_iterator it = group_config_map.begin();
00061       it != group_config_map.end();
00062       it++) {
00063     kinematics::KinematicsBase* kinematics_solver = NULL;
00064     try
00065     {
00066       kinematics_solver = kinematics_loader_.createClassInstance(plugin_name);
00067     }
00068     catch(pluginlib::PluginlibException& ex)
00069     {
00070       ROS_ERROR("The plugin failed to load. Error1: %s", ex.what());    //handle the class failing to load
00071       return;
00072     }
00073     if(!it->second.base_link_.empty() && !it->second.tip_link_.empty()) {
00074       ik_solver_map_[it->first] = new arm_kinematics_constraint_aware::ArmKinematicsSolverConstraintAware(kinematics_solver,
00075                                                                                                           getCollisionModels(),
00076                                                                                                           it->first);
00077       ik_solver_map_[it->first]->setSearchDiscretization(.025);
00078     }
00079   }
00080 }
00081 
00082 IKTesterFast::~IKTesterFast()
00083 {
00084   for(std::map<std::string, arm_kinematics_constraint_aware::ArmKinematicsSolverConstraintAware*>::iterator it = ik_solver_map_.begin();
00085       it != ik_solver_map_.end();
00086       it++) {
00087     delete it->second;
00088   }
00089 }
00090 
00091 planning_environment::CollisionModels* IKTesterFast::getCollisionModels() {
00092   if(cm_ == NULL) {
00093     return &mechInterface().getCollisionModels();
00094   } else {
00095     return cm_;
00096   }
00097 }
00098 
00099 planning_models::KinematicState* IKTesterFast::getPlanningSceneState() {
00100   if(state_ == NULL) {
00101     if(mechInterface().getPlanningSceneState() == NULL)
00102     {
00103       ROS_ERROR("Planning scene was NULL!  Did you forget to set it somewhere?  Getting new planning scene");
00104       const arm_navigation_msgs::OrderedCollisionOperations collision_operations;
00105       const std::vector<arm_navigation_msgs::LinkPadding> link_padding;
00106       mechInterface().getPlanningScene(collision_operations, link_padding);
00107     }                                  
00108     return mechInterface().getPlanningSceneState();
00109   } 
00110   else {
00111     return state_;
00112   }
00113 }
00114 
00115 
00116 void IKTesterFast::getGroupLinks(const std::string& group_name,
00117                                     std::vector<std::string>& group_links)
00118 {
00119   group_links.clear();
00120   const planning_models::KinematicModel::JointModelGroup* jmg = 
00121     getCollisionModels()->getKinematicModel()->getModelGroup(group_name);
00122   if(jmg == NULL) return;
00123   group_links = jmg->getGroupLinkNames();
00124 }
00125 
00126 void IKTesterFast::getGroupJoints(const std::string& group_name,
00127                                     std::vector<std::string>& group_joints)
00128 {
00129   if(ik_solver_map_.find(group_name) == ik_solver_map_.end()) {
00130     ROS_ERROR_STREAM("No group for solver " << group_name);
00131     return;
00132   }
00133   group_joints = ik_solver_map_[group_name]->getJointNames();
00134 }
00135 
00136 
00137 void IKTesterFast::testIKSet(std::string arm_name, const std::vector<geometry_msgs::PoseStamped> &test_poses,
00138                              bool return_on_first_hit, std::vector<sensor_msgs::JointState> &solutions_arr,
00139                              std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> &error_codes)
00140 {
00141   ros::WallTime start = ros::WallTime::now();
00142 
00143   error_codes.resize(test_poses.size());
00144   solutions_arr.resize(test_poses.size());
00145   std::map<int, int> outcome_count;
00146 
00147   state_ = NULL;
00148   planning_environment::CollisionModels* cm = getCollisionModels();
00149   planning_models::KinematicState* state = getPlanningSceneState();
00150 
00151   std::map<std::string, double> planning_scene_state_values;
00152   state->getKinematicStateValues(planning_scene_state_values);
00153 
00154   std::vector<std::string> end_effector_links, arm_links; 
00155   getGroupLinks(handDescription().gripperCollisionName(arm_name), end_effector_links);
00156   getGroupLinks(handDescription().armGroup(arm_name), arm_links);
00157 
00158   collision_space::EnvironmentModel::AllowedCollisionMatrix original_acm = cm->getCurrentAllowedCollisionMatrix();
00159   cm->disableCollisionsForNonUpdatedLinks(arm_name);
00160   collision_space::EnvironmentModel::AllowedCollisionMatrix group_disable_acm = cm->getCurrentAllowedCollisionMatrix();
00161   collision_space::EnvironmentModel::AllowedCollisionMatrix group_all_arm_disable_acm = group_disable_acm;
00162 
00163   //turning off collisions for the arm associated with this end effector for group_all_arm_disable_acm
00164   for(unsigned int i = 0; i < arm_links.size(); i++) {
00165     group_all_arm_disable_acm.changeEntry(arm_links[i], true);
00166   }
00167 
00168   std_msgs::Header world_header;
00169   world_header.frame_id = cm->getWorldFrameId();
00170 
00171   //Check IK for each pose in turn
00172   for(unsigned int i = 0; i < test_poses.size(); i++) {
00173 
00174     //set kinematic state back to original
00175     state->setKinematicState(planning_scene_state_values);
00176 
00177     //first check to see if the gripper itself is in collision
00178     geometry_msgs::PoseStamped world_pose_stamped;
00179     if(!cm->convertPoseGivenWorldTransform(*state,
00180                                            world_header.frame_id,
00181                                            test_poses[i].header,
00182                                            test_poses[i].pose,
00183                                            world_pose_stamped)) {
00184       ROS_WARN_STREAM("Can't convert into frame " << world_header.frame_id);
00185       continue;
00186     }
00187     
00188     tf::Transform tf_pose;
00189     tf::poseMsgToTF(world_pose_stamped.pose, tf_pose);
00190 
00191     //only check the gripper for collisions, not the arm
00192     cm->setAlteredAllowedCollisionMatrix(group_all_arm_disable_acm);
00193 
00194     if(!state->updateKinematicStateWithLinkAt(handDescription().gripperFrame(arm_name), tf_pose))
00195     {
00196       ROS_ERROR("ik_tester_fast: updateKinematicStateWithLinkAt failed!");
00197     }
00198 
00199     if(cm->isKinematicStateInCollision(*state)) {
00200       ROS_DEBUG("Kinematic state in collision!");
00201       outcome_count[arm_navigation_msgs::ArmNavigationErrorCodes::KINEMATICS_STATE_IN_COLLISION]++;
00202       error_codes[i].val = arm_navigation_msgs::ArmNavigationErrorCodes::KINEMATICS_STATE_IN_COLLISION;
00203       continue;
00204     } 
00205     
00206     //now check collision-aware ik for pose
00207     //go back to checking the entire arm
00208     cm->setAlteredAllowedCollisionMatrix(group_disable_acm);
00209     //cm->setAlteredAllowedCollisionMatrix(original_acm);
00210     state->setKinematicState(planning_scene_state_values);
00211 
00212     geometry_msgs::PoseStamped base_link_gripper_pose;
00213     cm->convertPoseGivenWorldTransform(*state,
00214                                        ik_solver_map_[arm_name]->getBaseName(),
00215                                        world_header,
00216                                        world_pose_stamped.pose,
00217                                        base_link_gripper_pose);
00218 
00219     arm_navigation_msgs::Constraints emp;
00220     sensor_msgs::JointState solution;
00221     arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00222     ROS_DEBUG_STREAM("x y z " << base_link_gripper_pose.pose.position.x << " " 
00223                      << base_link_gripper_pose.pose.position.y << " " 
00224                      << base_link_gripper_pose.pose.position.z);
00225     if(ik_solver_map_[arm_name]->findConstraintAwareSolution(base_link_gripper_pose.pose,
00226                                                               emp,
00227                                                               state,
00228                                                               solution,
00229                                                               error_code,
00230                                                               false)) {
00231       solutions_arr[i] = solution;
00232     }
00233     //didn't find a solution 
00234     else 
00235     {
00236       ROS_DEBUG("Pose out of reach or in collision");
00237     }
00238 
00239     //put the collision matrix back
00240     cm->setAlteredAllowedCollisionMatrix(original_acm);
00241 
00242     outcome_count[error_code.val]++;
00243     error_codes[i].val = error_code.val;
00244     if(return_on_first_hit && error_code.val == arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS) break;
00245   }
00246 
00247   ROS_INFO_STREAM("Took " << (ros::WallTime::now()-start).toSec());
00248 
00249   //print out how many of each outcome we found
00250   for(std::map<int, int>::iterator it = outcome_count.begin();
00251       it != outcome_count.end();
00252       it++) {
00253     ROS_INFO_STREAM("Outcome " << it->first << " count " << it->second);
00254   }
00255 }
00256 
00257 
00258 } //namespace object_manipulator


object_manipulator
Author(s): Matei Ciocarlie and Kaijen Hsiao
autogenerated on Thu Jan 2 2014 11:39:04