$search
00001 /* 00002 * Copyright (c) 2008, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 /* 00031 * Author: Sachin Chitta 00032 */ 00033 00034 #include <pr2_arm_kinematics_constraint_aware/pr2_arm_kinematics_constraint_aware.h> 00035 #include <pr2_arm_kinematics/pr2_arm_kinematics_utils.h> 00036 #include <geometry_msgs/PoseStamped.h> 00037 #include <kdl_parser/kdl_parser.hpp> 00038 #include <tf_conversions/tf_kdl.h> 00039 #include "ros/ros.h" 00040 #include <algorithm> 00041 #include <numeric> 00042 00043 #include <sensor_msgs/JointState.h> 00044 #include <kinematics_msgs/utils.h> 00045 #include <visualization_msgs/Marker.h> 00046 #include <visualization_msgs/MarkerArray.h> 00047 #include <planning_environment/models/model_utils.h> 00048 00049 using namespace KDL; 00050 using namespace tf; 00051 using namespace std; 00052 using namespace ros; 00053 00054 namespace pr2_arm_kinematics { 00055 00056 static const std::string IK_WITH_COLLISION_SERVICE = "get_constraint_aware_ik"; 00057 static const double IK_DEFAULT_TIMEOUT = 10.0; 00058 00059 PR2ArmIKConstraintAware::PR2ArmIKConstraintAware(): PR2ArmKinematics(false) 00060 { 00061 node_handle_.param<bool>("visualize_solution",visualize_solution_,true); 00062 node_handle_.param<std::string>("group", group_, std::string()); 00063 ROS_DEBUG("Advertising services"); 00064 00065 if(!isActive()) 00066 { 00067 ROS_ERROR("Could not load pr2_arm_kinematics_constraint_aware server"); 00068 } 00069 else 00070 { 00071 ROS_INFO("Loading pr2_arm_kinematics_constraint_aware server"); 00072 } 00073 collision_models_interface_ = new planning_environment::CollisionModelsInterface("robot_description"); 00074 if(group_.empty()) { 00075 ROS_WARN("Must specify planning group in configuration"); 00076 } 00077 const planning_models::KinematicModel::JointModelGroup* joint_model_group = collision_models_interface_->getKinematicModel()->getModelGroup(group_); 00078 if(joint_model_group == NULL) { 00079 ROS_WARN_STREAM("No joint group " << group_); 00080 } 00081 arm_links_ = joint_model_group->getGroupLinkNames(); 00082 00083 const planning_models::KinematicModel::LinkModel* end_effector_link = collision_models_interface_->getKinematicModel()->getLinkModel(ik_solver_info_.link_names.back()); 00084 end_effector_collision_links_ = collision_models_interface_->getKinematicModel()->getChildLinkModelNames(end_effector_link); 00085 00086 vis_marker_publisher_ = root_handle_.advertise<visualization_msgs::Marker>("kinematics_collisions", 128); 00087 vis_marker_array_publisher_ = root_handle_.advertise<visualization_msgs::MarkerArray>("kinematics_collisions_array", 128); 00088 00089 advertiseIK(); 00090 } 00091 00092 void PR2ArmIKConstraintAware::advertiseIK() 00093 { 00094 ik_collision_service_ = node_handle_.advertiseService(IK_WITH_COLLISION_SERVICE,&PR2ArmIKConstraintAware::getConstraintAwarePositionIK,this); 00095 display_trajectory_publisher_ = root_handle_.advertise<arm_navigation_msgs::DisplayTrajectory>("ik_solution_display", 1); 00096 } 00097 00098 bool PR2ArmIKConstraintAware::isReady(arm_navigation_msgs::ArmNavigationErrorCodes &error_code) 00099 { 00100 if(!active_) 00101 { 00102 ROS_ERROR("IK service is not ready"); 00103 return false; 00104 } 00105 if(!collision_models_interface_->isPlanningSceneSet()) { 00106 ROS_INFO("Planning scene not set"); 00107 error_code.val = error_code.COLLISION_CHECKING_UNAVAILABLE; 00108 return false; 00109 } 00110 error_code.val = error_code.SUCCESS; 00111 return true; 00112 } 00113 00114 bool PR2ArmIKConstraintAware::transformPose(const std::string& des_frame, 00115 const geometry_msgs::PoseStamped& pose_in, 00116 geometry_msgs::PoseStamped& pose_out) 00117 { 00118 if(!collision_models_interface_->convertPoseGivenWorldTransform(*collision_models_interface_->getPlanningSceneState(), 00119 des_frame, 00120 pose_in.header, 00121 pose_in.pose, 00122 pose_out)) { 00123 ROS_WARN_STREAM("Problem transforming pose"); 00124 return false; 00125 } 00126 return true; 00127 } 00128 00129 00130 int PR2ArmIKConstraintAware::CartToJntSearch(const KDL::JntArray& q_in, 00131 const KDL::Frame& p_in, 00132 KDL::JntArray &q_out, 00133 const double &timeout, 00134 arm_navigation_msgs::ArmNavigationErrorCodes& error_code) 00135 { 00136 if(!isReady(error_code)) 00137 return -1; 00138 bool ik_valid = (pr2_arm_ik_solver_->CartToJntSearch(q_in, 00139 p_in, 00140 q_out, 00141 timeout, 00142 error_code, 00143 boost::bind(&PR2ArmIKConstraintAware::initialPoseCheck, this, _1, _2, _3), 00144 boost::bind(&PR2ArmIKConstraintAware::collisionCheck, this, _1, _2, _3)) >= 0); 00145 00146 return ik_valid; 00147 } 00148 00149 bool PR2ArmIKConstraintAware::getPositionIK(kinematics_msgs::GetPositionIK::Request &request, 00150 kinematics_msgs::GetPositionIK::Response &response) 00151 { 00152 if(!isReady(response.error_code)) 00153 return true; 00154 00155 if(!checkIKService(request,response,ik_solver_info_)) 00156 return true; 00157 00158 collision_models_interface_->resetToStartState(*collision_models_interface_->getPlanningSceneState()); 00159 00160 geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped; 00161 00162 planning_environment::setRobotStateAndComputeTransforms(request.ik_request.robot_state, *collision_models_interface_->getPlanningSceneState()); 00163 00164 if(!collision_models_interface_->convertPoseGivenWorldTransform(*collision_models_interface_->getPlanningSceneState(), 00165 root_name_, 00166 pose_msg_in.header, 00167 pose_msg_in.pose, 00168 request.ik_request.pose_stamped)) { 00169 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE; 00170 return true; 00171 } 00172 return getPositionIKHelper(request, response); 00173 } 00174 00175 bool PR2ArmIKConstraintAware::getConstraintAwarePositionIK(kinematics_msgs::GetConstraintAwarePositionIK::Request &request_in, 00176 kinematics_msgs::GetConstraintAwarePositionIK::Response &response) 00177 { 00178 if(!isReady(response.error_code)) 00179 return true; 00180 00181 if(!checkConstraintAwareIKService(request_in,response,ik_solver_info_)) 00182 { 00183 ROS_ERROR("IK service request is malformed"); 00184 return true; 00185 } 00186 00187 collision_models_interface_->resetToStartState(*collision_models_interface_->getPlanningSceneState()); 00188 00189 collision_models_interface_->disableCollisionsForNonUpdatedLinks(group_); 00190 00191 ros::Time start_time = ros::Time::now(); 00192 ROS_DEBUG("Received IK request is in the frame: %s",request_in.ik_request.pose_stamped.header.frame_id.c_str()); 00193 00194 ik_request_ = request_in.ik_request; 00195 constraints_ = request_in.constraints; 00196 00197 geometry_msgs::PoseStamped pose_msg_in = ik_request_.pose_stamped; 00198 geometry_msgs::PoseStamped pose_msg_out; 00199 00200 ROS_DEBUG_STREAM("Ik request in frame " << pose_msg_in.header << " is " 00201 << pose_msg_in.pose.position.x << " " 00202 << pose_msg_in.pose.position.y << " " 00203 << pose_msg_in.pose.position.z << " " 00204 << pose_msg_in.pose.orientation.x << " " 00205 << pose_msg_in.pose.orientation.y << " " 00206 << pose_msg_in.pose.orientation.z << " " 00207 << pose_msg_in.pose.orientation.w); 00208 00209 planning_environment::setRobotStateAndComputeTransforms(request_in.ik_request.robot_state, *collision_models_interface_->getPlanningSceneState()); 00210 00211 if(!collision_models_interface_->convertPoseGivenWorldTransform(*collision_models_interface_->getPlanningSceneState(), 00212 root_name_, 00213 pose_msg_in.header, 00214 pose_msg_in.pose, 00215 pose_msg_out)) { 00216 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE; 00217 return true; 00218 } 00219 ik_request_.pose_stamped = pose_msg_out; 00220 00221 ROS_DEBUG_STREAM("Ik request in frame " << root_name_ << " is " 00222 << pose_msg_out.pose.position.x << " " 00223 << pose_msg_out.pose.position.y << " " 00224 << pose_msg_out.pose.position.z << " " 00225 << pose_msg_out.pose.orientation.x << " " 00226 << pose_msg_out.pose.orientation.y << " " 00227 << pose_msg_out.pose.orientation.z << " " 00228 << pose_msg_out.pose.orientation.w); 00229 00230 KDL::JntArray jnt_pos_out; 00231 KDL::JntArray jnt_pos_in; 00232 KDL::Frame p_in; 00233 tf::PoseMsgToKDL(pose_msg_out.pose,p_in); 00234 jnt_pos_in.resize(dimension_); 00235 jnt_pos_out.resize(dimension_); 00236 for(unsigned int i=0; i < request_in.ik_request.ik_seed_state.joint_state.name.size(); i++) 00237 { 00238 int tmp_index = pr2_arm_kinematics::getJointIndex(request_in.ik_request.ik_seed_state.joint_state.name[i],ik_solver_info_); 00239 if(tmp_index != -1) { 00240 ROS_DEBUG_STREAM("Setting name " << request_in.ik_request.ik_seed_state.joint_state.name[i] 00241 << " index " << tmp_index 00242 << " to " << request_in.ik_request.ik_seed_state.joint_state.position[i]); 00243 jnt_pos_in(tmp_index) = request_in.ik_request.ik_seed_state.joint_state.position[i]; 00244 } 00245 } 00246 00247 ros::Time ik_solver_time = ros::Time::now(); 00248 bool ik_valid = CartToJntSearch(jnt_pos_in, 00249 p_in, 00250 jnt_pos_out, 00251 request_in.timeout.toSec(), 00252 response.error_code); 00253 00254 if(ik_valid) 00255 { 00256 response.solution.joint_state.name = ik_solver_info_.joint_names; 00257 response.solution.joint_state.position.resize(dimension_); 00258 for(int i=0; i < dimension_; i++) 00259 { 00260 response.solution.joint_state.position[i] = jnt_pos_out(i); 00261 ROS_DEBUG("IK Solution: %s %d: %f",response.solution.joint_state.name[i].c_str(),i,jnt_pos_out(i)); 00262 } 00263 /* 00264 if(visualize_solution_) 00265 { 00266 arm_navigation_msgs::DisplayTrajectory display_trajectory; 00267 display_trajectory.trajectory.joint_trajectory.points.resize(1); 00268 display_trajectory.trajectory.joint_trajectory.points[0].positions = response.solution.joint_state.position; 00269 display_trajectory.trajectory.joint_trajectory.joint_names = response.solution.joint_state.name; 00270 planning_monitor_->convertKinematicStateToRobotState(*kinematic_state_,display_trajectory.robot_state); 00271 display_trajectory_publisher_.publish(display_trajectory); 00272 } 00273 */ 00274 ROS_DEBUG("IK service time: %f",(ros::Time::now()-start_time).toSec()); 00275 response.error_code.val = response.error_code.SUCCESS; 00276 return true; 00277 } 00278 else 00279 { 00280 ROS_DEBUG_STREAM("An IK solution could not be found " << response.error_code.val); 00281 // if(ros::Time::now()-last_planning_scene_drop_ > ros::Duration(5.0)) { 00282 // last_planning_scene_drop_ = ros::Time::now(); 00283 // std::string filename = "bad_ik_"; 00284 // std::string str = boost::lexical_cast<std::string>(ros::Time::now().toSec()); 00285 // filename += str; 00286 // collision_models_interface_->writePlanningSceneBag(filename, 00287 // collision_models_interface_->getLastPlanningScene()); 00288 00289 // } 00290 if(response.error_code.val != response.error_code.IK_LINK_IN_COLLISION) { 00291 sendEndEffectorPose(collision_models_interface_->getPlanningSceneState(),true); 00292 std_msgs::ColorRGBA col; 00293 col.a = .9; 00294 col.r = 1.0; 00295 col.b = 1.0; 00296 col.g = 0.0; 00297 visualization_msgs::MarkerArray arr; 00298 collision_models_interface_->getRobotMarkersGivenState(*collision_models_interface_->getPlanningSceneState(), 00299 arr, 00300 col, 00301 "start_pose", 00302 ros::Duration(0.0)); 00303 vis_marker_array_publisher_.publish(arr); 00304 } 00305 return true; 00306 } 00307 } 00308 00309 void PR2ArmIKConstraintAware::collisionCheck(const KDL::JntArray &jnt_array, 00310 const KDL::Frame &p_in, 00311 arm_navigation_msgs::ArmNavigationErrorCodes &error_code) 00312 { 00313 ros::Time n1 = ros::Time::now(); 00314 00315 std::map<std::string, double> joint_values; 00316 for(unsigned int i=0; i < ik_solver_info_.joint_names.size(); i++) 00317 { 00318 joint_values[ik_solver_info_.joint_names[i]] = jnt_array(i); 00319 } 00320 collision_models_interface_->getPlanningSceneState()->setKinematicState(joint_values); 00321 if(collision_models_interface_->getPlanningSceneState() == NULL) { 00322 ROS_INFO_STREAM("Messed up"); 00323 } 00324 if(collision_models_interface_->isKinematicStateInCollision(*(collision_models_interface_->getPlanningSceneState()))) { 00325 visualization_msgs::MarkerArray arr; 00326 std_msgs::ColorRGBA col; 00327 col.a = .9; 00328 col.r = 1.0; 00329 col.b = 0.0; 00330 col.g = 0.0; 00331 collision_models_interface_->getAllCollisionPointMarkers(*collision_models_interface_->getPlanningSceneState(), 00332 arr, 00333 col, 00334 ros::Duration(0.0)); 00335 vis_marker_array_publisher_.publish(arr); 00336 error_code.val = error_code.KINEMATICS_STATE_IN_COLLISION; 00337 } else { 00338 error_code.val = error_code.SUCCESS; 00339 } 00340 00341 if(!planning_environment::doesKinematicStateObeyConstraints(*(collision_models_interface_->getPlanningSceneState()), 00342 constraints_)) { 00343 error_code.val = error_code.INVALID_GOAL_POSITION_CONSTRAINTS; 00344 } 00345 ROS_DEBUG_STREAM("Collision check took " << (ros::Time::now()-n1).toSec()); 00346 } 00347 00348 void PR2ArmIKConstraintAware::initialPoseCheck(const KDL::JntArray &jnt_array, 00349 const KDL::Frame &p_in, 00350 arm_navigation_msgs::ArmNavigationErrorCodes &error_code) 00351 { 00352 std::string kinematic_frame_id = pr2_arm_ik_solver_->getFrameId(); 00353 std::string planning_frame_id = collision_models_interface_->getWorldFrameId(); 00354 geometry_msgs::PoseStamped pose_stamped; 00355 tf::PoseKDLToMsg(p_in,pose_stamped.pose); 00356 pose_stamped.header.stamp = ros::Time::now(); 00357 pose_stamped.header.frame_id = kinematic_frame_id; 00358 if(!collision_models_interface_->convertPoseGivenWorldTransform(*collision_models_interface_->getPlanningSceneState(), 00359 planning_frame_id, 00360 pose_stamped.header, 00361 pose_stamped.pose, 00362 pose_stamped)) { 00363 ROS_ERROR_STREAM("Cannot transform from " << pose_stamped.header.frame_id << " to " << planning_frame_id); 00364 } 00365 //disabling all collision for arm links 00366 collision_space::EnvironmentModel::AllowedCollisionMatrix save_acm = collision_models_interface_->getCurrentAllowedCollisionMatrix(); 00367 collision_space::EnvironmentModel::AllowedCollisionMatrix acm = save_acm; 00368 for(unsigned int i = 0; i < arm_links_.size(); i++) { 00369 acm.changeEntry(arm_links_[i], true); 00370 } 00371 00372 collision_models_interface_->setAlteredAllowedCollisionMatrix(acm); 00373 00374 btTransform transform; 00375 tf::poseMsgToTF(pose_stamped.pose,transform); 00376 if(!collision_models_interface_->getPlanningSceneState()->hasLinkState(ik_request_.ik_link_name)) { 00377 ROS_ERROR("Could not find end effector root_link %s", ik_request_.ik_link_name.c_str()); 00378 error_code.val = error_code.INVALID_LINK_NAME; 00379 return; 00380 } 00381 collision_models_interface_->getPlanningSceneState()->updateKinematicStateWithLinkAt(ik_request_.ik_link_name, transform); 00382 if(collision_models_interface_->isKinematicStateInCollision(*(collision_models_interface_->getPlanningSceneState()))) { 00383 visualization_msgs::MarkerArray arr; 00384 std_msgs::ColorRGBA col; 00385 col.a = .9; 00386 col.r = 1.0; 00387 col.b = 0.0; 00388 col.g = 0.0; 00389 collision_models_interface_->getAllCollisionPointMarkers(*collision_models_interface_->getPlanningSceneState(), 00390 arr, 00391 col, 00392 ros::Duration(0.0)); 00393 vis_marker_array_publisher_.publish(arr); 00394 error_code.val = error_code.IK_LINK_IN_COLLISION; 00395 ROS_DEBUG_STREAM("Initial pose check failing"); 00396 sendEndEffectorPose(collision_models_interface_->getPlanningSceneState(), false); 00397 } 00398 else 00399 error_code.val = error_code.SUCCESS; 00400 00401 collision_models_interface_->setAlteredAllowedCollisionMatrix(save_acm); 00402 } 00403 00404 void PR2ArmIKConstraintAware::sendEndEffectorPose(const planning_models::KinematicState* state, bool valid) { 00405 visualization_msgs::MarkerArray hand_array; 00406 std_msgs::ColorRGBA col; 00407 col.a = .8; 00408 col.b = 0.0; 00409 00410 if(valid) { 00411 col.g = 1.0; 00412 } else { 00413 col.r = 1.0; 00414 } 00415 00416 collision_models_interface_->getRobotMarkersGivenState(*state, 00417 hand_array, 00418 col, 00419 "end_effector", 00420 ros::Duration(0.0), 00421 &end_effector_collision_links_); 00422 vis_marker_array_publisher_.publish(hand_array); 00423 } 00424 00425 void PR2ArmIKConstraintAware::printStringVec(const std::string &prefix, const std::vector<std::string> &string_vector) 00426 { 00427 ROS_DEBUG("%s",prefix.c_str()); 00428 for(unsigned int i=0; i < string_vector.size(); i++) 00429 { 00430 ROS_DEBUG("%s",string_vector[i].c_str()); 00431 } 00432 } 00433 } // namespace 00434