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 motion_planning_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 if(!path_constraint_evaluator_set_.decide(kinematic_state_, true))
00062 {
00063 ROS_DEBUG("Path constraints violated in task space");
00064 return false;
00065 }
00066 joint_state_group_->updateKinematicLinks();
00067 planning_monitor_->getEnvironmentModel()->updateRobotModel(kinematic_state_);
00068 bool collision_validity_check = (!planning_monitor_->getEnvironmentModel()->isCollision() && !planning_monitor_->getEnvironmentModel()->isSelfCollision());
00069 if(!collision_validity_check)
00070 {
00071 ROS_DEBUG("State is in collision");
00072 }
00073 return collision_validity_check;
00074 }
00075
00076 bool OmplRosTaskSpaceValidityChecker::isStateValid(const ompl::base::State *ompl_state)
00077 {
00078 motion_planning_msgs::RobotState robot_state_msg;
00079 if(!state_transformer_->inverseTransform(*ompl_state,
00080 robot_state_msg))
00081 {
00082 ROS_DEBUG("State transformation failed");
00083 error_code_.val = error_code_.NO_IK_SOLUTION;
00084 return false;
00085 }
00086 ompl_ros_interface::robotStateToJointStateGroup(robot_state_msg,
00087 robot_state_to_joint_state_group_mapping_,
00088 joint_state_group_);
00089 std::vector<planning_models::KinematicState::JointState*> joint_states = joint_state_group_->getJointStateVector();
00090 for(unsigned int i=0; i < joint_states.size(); i++)
00091 {
00092 if(!joint_states[i]->areJointStateValuesWithinBounds())
00093 {
00094 ROS_DEBUG("State violates joint limits for Joint %s",joint_states[i]->getName().c_str());
00095 error_code_.val = error_code_.JOINT_LIMITS_VIOLATED;
00096 return false;
00097 }
00098 }
00099
00100 if(!path_constraint_evaluator_set_.decide(kinematic_state_, false))
00101 {
00102 ROS_DEBUG("Path constraints violated");
00103 error_code_.val = error_code_.PATH_CONSTRAINTS_VIOLATED;
00104 return false;
00105 }
00106
00107 joint_state_group_->updateKinematicLinks();
00108 planning_monitor_->getEnvironmentModel()->updateRobotModel(kinematic_state_);
00109 bool collision_validity_check = (!planning_monitor_->getEnvironmentModel()->isCollision() && !planning_monitor_->getEnvironmentModel()->isSelfCollision());
00110 if(!collision_validity_check)
00111 {
00112 ROS_DEBUG("State is in collision");
00113 error_code_.val = error_code_.COLLISION_CONSTRAINTS_VIOLATED;
00114 }
00115 return collision_validity_check;
00116 }
00117
00118 bool OmplRosTaskSpaceValidityChecker::initialize()
00119 {
00120 return true;
00121 }
00122
00123 bool OmplRosTaskSpaceValidityChecker::setStateTransformer(boost::shared_ptr<ompl_ros_interface::OmplRosStateTransformer> &state_transformer)
00124 {
00125 if(state_transformer)
00126 {
00127 if(state_transformer->getFrame() != parent_frame_)
00128 {
00129 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());
00130 return false;
00131 }
00132 state_transformer_ = state_transformer;
00133 }
00134 else
00135 return false;
00136 return true;
00137 }
00138
00139 void OmplRosTaskSpaceValidityChecker::configureOnRequest(planning_models::KinematicState *kinematic_state,
00140 planning_models::KinematicState::JointStateGroup *joint_state_group,
00141 const motion_planning_msgs::GetMotionPlan::Request &request)
00142 {
00143 kinematic_state_ = kinematic_state;
00144 joint_state_group_ = joint_state_group;
00145
00146 goal_constraint_evaluator_set_.clear();
00147 path_constraint_evaluator_set_.clear();
00148
00149
00150 motion_planning_msgs::Constraints goal_constraints = getPhysicalConstraints(request.motion_plan_request.goal_constraints);
00151 motion_planning_msgs::Constraints path_constraints = getPhysicalConstraints(request.motion_plan_request.path_constraints);
00152
00153 goal_constraint_evaluator_set_.add(goal_constraints.joint_constraints);
00154 goal_constraint_evaluator_set_.add(goal_constraints.position_constraints);
00155 goal_constraint_evaluator_set_.add(goal_constraints.orientation_constraints);
00156 goal_constraint_evaluator_set_.add(goal_constraints.visibility_constraints);
00157
00158 path_constraint_evaluator_set_.add(path_constraints.joint_constraints);
00159 path_constraint_evaluator_set_.add(path_constraints.position_constraints);
00160 path_constraint_evaluator_set_.add(path_constraints.orientation_constraints);
00161 path_constraint_evaluator_set_.add(path_constraints.visibility_constraints);
00162
00163 motion_planning_msgs::RobotState default_state = state_transformer_->getDefaultState();
00164 if(!getRobotStateToJointModelGroupMapping(default_state,joint_state_group_->getJointModelGroup(),robot_state_to_joint_state_group_mapping_))
00165 return;
00166 }
00167
00168 }