$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, 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 Willow Garage, Inc. 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 <ompl_ros_interface/state_validity_checkers/ompl_ros_task_space_validity_checker.h> 00038 00039 namespace ompl_ros_interface 00040 { 00041 00042 bool OmplRosTaskSpaceValidityChecker::isValid(const ompl::base::State *ompl_state) const 00043 { 00044 arm_navigation_msgs::RobotState robot_state_msg; 00045 if(!state_transformer_->inverseTransform(*ompl_state, 00046 robot_state_msg)) 00047 return false; 00048 00049 ompl_ros_interface::robotStateToJointStateGroup(robot_state_msg, 00050 robot_state_to_joint_state_group_mapping_, 00051 joint_state_group_); 00052 std::vector<planning_models::KinematicState::JointState*> joint_states = joint_state_group_->getJointStateVector(); 00053 for(unsigned int i=0; i < joint_states.size(); i++) 00054 { 00055 if(!joint_states[i]->areJointStateValuesWithinBounds()) 00056 { 00057 ROS_DEBUG("State violates joint limits for Joint %s",joint_states[i]->getName().c_str()); 00058 return false; 00059 } 00060 } 00061 joint_state_group_->updateKinematicLinks(); 00062 if(!path_constraint_evaluator_set_.decide(kinematic_state_, false)) 00063 { 00064 ROS_DEBUG("Path constraints violated in task space"); 00065 return false; 00066 } 00067 if(collision_models_interface_->isKinematicStateInCollision(*kinematic_state_)) 00068 { 00069 ROS_DEBUG("State is in collision"); 00070 return false; 00071 } 00072 return true; 00073 } 00074 00075 bool OmplRosTaskSpaceValidityChecker::isStateValid(const ompl::base::State *ompl_state) 00076 { 00077 arm_navigation_msgs::RobotState robot_state_msg; 00078 if(!state_transformer_->inverseTransform(*ompl_state, 00079 robot_state_msg)) 00080 { 00081 ROS_DEBUG("State transformation failed"); 00082 error_code_.val = error_code_.NO_IK_SOLUTION; 00083 return false; 00084 } 00085 ompl_ros_interface::robotStateToJointStateGroup(robot_state_msg, 00086 robot_state_to_joint_state_group_mapping_, 00087 joint_state_group_); 00088 std::vector<planning_models::KinematicState::JointState*> joint_states = joint_state_group_->getJointStateVector(); 00089 for(unsigned int i=0; i < joint_states.size(); i++) 00090 { 00091 if(!joint_states[i]->areJointStateValuesWithinBounds()) 00092 { 00093 ROS_DEBUG("State violates joint limits for Joint %s",joint_states[i]->getName().c_str()); 00094 error_code_.val = error_code_.JOINT_LIMITS_VIOLATED; 00095 return false; 00096 } 00097 } 00098 00099 if(!path_constraint_evaluator_set_.decide(kinematic_state_, false)) 00100 { 00101 ROS_DEBUG("Path constraints violated"); 00102 error_code_.val = error_code_.PATH_CONSTRAINTS_VIOLATED; 00103 return false; 00104 } 00105 00106 joint_state_group_->updateKinematicLinks(); 00107 if(collision_models_interface_->isKinematicStateInCollision(*kinematic_state_)) 00108 { 00109 ROS_DEBUG("State is in collision"); 00110 error_code_.val = error_code_.COLLISION_CONSTRAINTS_VIOLATED; 00111 return false; 00112 } 00113 return true; 00114 } 00115 00116 bool OmplRosTaskSpaceValidityChecker::initialize() 00117 { 00118 return true; 00119 } 00120 00121 bool OmplRosTaskSpaceValidityChecker::setStateTransformer(boost::shared_ptr<ompl_ros_interface::OmplRosStateTransformer> &state_transformer) 00122 { 00123 if(state_transformer) 00124 { 00125 if(state_transformer->getFrame() != parent_frame_) 00126 { 00127 ROS_ERROR("State transformer has parent frame %s. State transformer should function in same frame as planning state space %s",state_transformer->getFrame().c_str(),parent_frame_.c_str()); 00128 return false; 00129 } 00130 state_transformer_ = state_transformer; 00131 } 00132 else 00133 return false; 00134 return true; 00135 } 00136 00137 void OmplRosTaskSpaceValidityChecker::configureOnRequest(planning_models::KinematicState *kinematic_state, 00138 planning_models::KinematicState::JointStateGroup *joint_state_group, 00139 const arm_navigation_msgs::GetMotionPlan::Request &request) 00140 { 00141 kinematic_state_ = kinematic_state; 00142 joint_state_group_ = joint_state_group; 00143 00144 goal_constraint_evaluator_set_.clear(); 00145 path_constraint_evaluator_set_.clear(); 00146 00147 //Get the valid set of constraints that correspond to constraints on the physical joints and links of the robot 00148 arm_navigation_msgs::Constraints goal_constraints = getPhysicalConstraints(request.motion_plan_request.goal_constraints); 00149 arm_navigation_msgs::Constraints path_constraints = getPhysicalConstraints(request.motion_plan_request.path_constraints); 00150 00151 goal_constraint_evaluator_set_.add(goal_constraints.joint_constraints); 00152 goal_constraint_evaluator_set_.add(goal_constraints.position_constraints); 00153 goal_constraint_evaluator_set_.add(goal_constraints.orientation_constraints); 00154 goal_constraint_evaluator_set_.add(goal_constraints.visibility_constraints); 00155 00156 path_constraint_evaluator_set_.add(path_constraints.joint_constraints); 00157 path_constraint_evaluator_set_.add(path_constraints.position_constraints); 00158 path_constraint_evaluator_set_.add(path_constraints.orientation_constraints); 00159 path_constraint_evaluator_set_.add(path_constraints.visibility_constraints); 00160 00161 arm_navigation_msgs::RobotState default_state = state_transformer_->getDefaultState(); 00162 if(!getRobotStateToJointModelGroupMapping(default_state,joint_state_group_->getJointModelGroup(),robot_state_to_joint_state_group_mapping_)) 00163 return; 00164 } 00165 00166 }