Go to the documentation of this file.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 <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
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 }