$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/planners/ompl_ros_rpy_ik_task_space_planner.h> 00038 #include <planning_environment/models/model_utils.h> 00039 00040 namespace ompl_ros_interface 00041 { 00042 bool OmplRosRPYIKTaskSpacePlanner::initializeStateValidityChecker(ompl_ros_interface::OmplRosStateValidityCheckerPtr &state_validity_checker) 00043 { 00044 state_validity_checker.reset(new ompl_ros_interface::OmplRosTaskSpaceValidityChecker(planner_->getSpaceInformation().get(), 00045 collision_models_interface_, 00046 planning_frame_id_)); 00047 boost::shared_ptr<ompl_ros_interface::OmplRosStateTransformer> state_transformer; 00048 state_transformer.reset(new ompl_ros_interface::OmplRosRPYIKStateTransformer(state_space_, physical_joint_group_)); 00049 if(!state_transformer->initialize()) 00050 return false; 00051 if(!(dynamic_cast<ompl_ros_interface::OmplRosTaskSpaceValidityChecker*>(state_validity_checker.get()))->setStateTransformer(state_transformer)) 00052 return false; 00053 state_transformer_ = state_transformer; 00054 if(!node_handle_.hasParam(state_space_->getName()+"/tip_name")) 00055 { 00056 ROS_ERROR("Could not find tip name for state_space %s",state_space_->getName().c_str()); 00057 return false; 00058 } 00059 node_handle_.getParam(state_space_->getName()+"/tip_name",end_effector_name_); 00060 original_real_vector_bounds_.reset(new ompl::base::RealVectorBounds(state_space_->as<ompl::base::CompoundStateSpace>()->getSubSpace("real_vector")->as<ompl::base::RealVectorStateSpace>()->getBounds())); 00061 return true; 00062 } 00063 00064 arm_navigation_msgs::RobotTrajectory OmplRosRPYIKTaskSpacePlanner::getSolutionPath() 00065 { 00066 arm_navigation_msgs::RobotTrajectory robot_trajectory; 00067 00068 ompl::geometric::PathGeometric path = planner_->getSolutionPath(); 00069 path.interpolate(); 00070 unsigned int num_points = path.states.size(); 00071 ROS_DEBUG("Path has %d waypoints",(int)path.states.size()); 00072 for(unsigned int i=0; i < num_points; i++) 00073 { 00074 arm_navigation_msgs::RobotState robot_state; 00075 trajectory_msgs::JointTrajectoryPoint joint_trajectory_point; 00076 arm_navigation_msgs::MultiDOFJointTrajectoryPoint multi_dof_joint_trajectory_point; 00077 00078 if(!state_transformer_->inverseTransform(*(path.states[i]), 00079 robot_state)) 00080 { 00081 ROS_ERROR("Could not transform solution waypoint"); 00082 std::stringstream string_stream; 00083 state_space_->printState(path.states[i],string_stream); 00084 ROS_ERROR("State: %d %s",i,string_stream.str().c_str()); 00085 } 00086 00087 if(i==0) 00088 { 00089 robot_trajectory.joint_trajectory.joint_names = robot_state.joint_state.name; 00090 robot_trajectory.joint_trajectory.header.stamp = robot_state.joint_state.header.stamp; 00091 robot_trajectory.joint_trajectory.header.frame_id = robot_state.joint_state.header.frame_id; 00092 00093 robot_trajectory.multi_dof_joint_trajectory.stamp = ros::Duration(robot_state.multi_dof_joint_state.stamp.toSec()); 00094 robot_trajectory.multi_dof_joint_trajectory.joint_names = robot_state.multi_dof_joint_state.joint_names; 00095 robot_trajectory.multi_dof_joint_trajectory.frame_ids = robot_state.multi_dof_joint_state.frame_ids; 00096 robot_trajectory.multi_dof_joint_trajectory.child_frame_ids = robot_state.multi_dof_joint_state.child_frame_ids; 00097 } 00098 // arm_navigation_msgs::printJointState(robot_state.joint_state); 00099 arm_navigation_msgs::robotStateToRobotTrajectoryPoint(robot_state, 00100 joint_trajectory_point, 00101 multi_dof_joint_trajectory_point); 00102 if(!robot_state.joint_state.name.empty()) 00103 robot_trajectory.joint_trajectory.points.push_back(joint_trajectory_point); 00104 if(!robot_state.multi_dof_joint_state.joint_names.empty()) 00105 robot_trajectory.multi_dof_joint_trajectory.points.push_back(multi_dof_joint_trajectory_point); 00106 } 00107 return robot_trajectory; 00108 } 00109 00110 bool OmplRosRPYIKTaskSpacePlanner::setStart(arm_navigation_msgs::GetMotionPlan::Request &request, 00111 arm_navigation_msgs::GetMotionPlan::Response &response) 00112 { 00113 //Use the path constraints to set component bounds first 00114 arm_navigation_msgs::ArmNavigationErrorCodes error_code; 00115 state_space_->as<ompl::base::CompoundStateSpace>()->getSubSpace("real_vector")->as<ompl::base::RealVectorStateSpace>()->setBounds(*original_real_vector_bounds_); 00116 arm_navigation_msgs::Constraints tmp_constraints = request.motion_plan_request.path_constraints; 00117 collision_models_interface_->convertConstraintsGivenNewWorldTransform(*collision_models_interface_->getPlanningSceneState(), 00118 tmp_constraints, 00119 state_transformer_->getFrame()); 00120 00121 arm_navigation_msgs::PositionConstraint position_constraint; 00122 arm_navigation_msgs::OrientationConstraint orientation_constraint; 00123 if(!getEndEffectorConstraints(tmp_constraints,position_constraint,orientation_constraint,false)) 00124 { 00125 ROS_ERROR("Could not get end effector constraints for setting start state"); 00126 return false; 00127 } 00128 if(!position_constraint.header.frame_id.empty()) 00129 if(!positionConstraintToOmplStateBounds(position_constraint, 00130 state_space_)) 00131 { 00132 ROS_ERROR("Could not convert position constraint"); 00133 return false; 00134 } 00135 00136 if(!orientation_constraint.header.frame_id.empty()) 00137 if(!orientationConstraintToOmplStateBounds(orientation_constraint, 00138 state_space_)) 00139 { 00140 ROS_ERROR("Could not convert orientation constraint"); 00141 return false; 00142 } 00143 00144 // Now, set the start state - first from the current state but then overwrite with what's in the request 00145 ompl::base::ScopedState<ompl::base::CompoundStateSpace> start(state_space_); 00146 00147 //everything should be in the current planning state 00148 arm_navigation_msgs::RobotState cur_state; 00149 planning_environment::convertKinematicStateToRobotState(*collision_models_interface_->getPlanningSceneState(), 00150 ros::Time::now(), 00151 collision_models_interface_->getWorldFrameId(), 00152 cur_state); 00153 00154 ompl_ros_interface::robotStateToOmplState(cur_state,start,false); 00155 geometry_msgs::PoseStamped end_effector_pose = getEndEffectorPose(request.motion_plan_request.start_state); 00156 ROS_DEBUG("Setting start"); 00157 poseStampedToOmplState(end_effector_pose,start,false); 00158 00159 // Check the start state validity 00160 ompl_ros_interface::OmplRosTaskSpaceValidityChecker *my_checker = dynamic_cast<ompl_ros_interface::OmplRosTaskSpaceValidityChecker*>(state_validity_checker_.get()); 00161 if(!my_checker->isStateValid(start.get())) 00162 { 00163 response.error_code = my_checker->getLastErrorCode(); 00164 if(response.error_code.val == response.error_code.PATH_CONSTRAINTS_VIOLATED) 00165 response.error_code.val = response.error_code.START_STATE_VIOLATES_PATH_CONSTRAINTS; 00166 else if(response.error_code.val == response.error_code.COLLISION_CONSTRAINTS_VIOLATED) 00167 response.error_code.val = response.error_code.START_STATE_IN_COLLISION; 00168 ROS_ERROR_STREAM("Start state is invalid with code " << response.error_code.val); 00169 return false; 00170 } 00171 planner_->getProblemDefinition()->clearStartStates(); 00172 planner_->addStartState(start); 00173 ROS_DEBUG("Setting start state successful"); 00174 return true; 00175 } 00176 00177 00178 bool OmplRosRPYIKTaskSpacePlanner::constraintsToOmplState(const arm_navigation_msgs::Constraints &constraints, 00179 ompl::base::ScopedState<ompl::base::CompoundStateSpace> &goal) 00180 { 00181 // Transform the constraints 00182 arm_navigation_msgs::ArmNavigationErrorCodes error_code; 00183 arm_navigation_msgs::Constraints tmp_constraints = constraints; 00184 collision_models_interface_->convertConstraintsGivenNewWorldTransform(*collision_models_interface_->getPlanningSceneState(), 00185 tmp_constraints, 00186 state_transformer_->getFrame()); 00187 // Set all physical constraints that directly map onto the joints 00188 if(!ompl_ros_interface::constraintsToOmplState(tmp_constraints,goal,false)) 00189 return false; 00190 00191 // Set RPY, position constraints 00192 arm_navigation_msgs::PositionConstraint position_constraint; 00193 arm_navigation_msgs::OrientationConstraint orientation_constraint; 00194 if(!getEndEffectorConstraints(tmp_constraints,position_constraint,orientation_constraint,true)) { 00195 ROS_WARN("Goal constraints probably don't have a position and orientation constraint"); 00196 return false; 00197 } 00198 geometry_msgs::PoseStamped desired_pose = arm_navigation_msgs::poseConstraintsToPoseStamped(position_constraint, 00199 orientation_constraint); 00200 00201 btQuaternion orientation; 00202 double roll,pitch,yaw; 00203 tf::quaternionMsgToTF(desired_pose.pose.orientation,orientation); 00204 btMatrix3x3 rotation(orientation); 00205 rotation.getRPY(roll,pitch,yaw); 00206 00207 ROS_DEBUG("Setting goal: %f %f %f, %f %f %f", 00208 desired_pose.pose.position.x, 00209 desired_pose.pose.position.y, 00210 desired_pose.pose.position.z, 00211 roll, 00212 pitch, 00213 yaw); 00214 ROS_DEBUG("Setting goal (quaternion): %f %f %f %f", 00215 desired_pose.pose.orientation.x, 00216 desired_pose.pose.orientation.y, 00217 desired_pose.pose.orientation.z, 00218 desired_pose.pose.orientation.w); 00219 00220 if(!poseStampedToOmplState(desired_pose,goal)) 00221 return false; 00222 00223 return true; 00224 } 00225 00226 bool OmplRosRPYIKTaskSpacePlanner::positionConstraintToOmplStateBounds(const arm_navigation_msgs::PositionConstraint &position_constraint, 00227 ompl::base::StateSpacePtr &goal) 00228 { 00229 int real_vector_index = state_space_->as<ompl::base::CompoundStateSpace>()->getSubSpaceIndex("real_vector"); 00230 int x_index = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index)->getDimensionIndex("x"); 00231 int y_index = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index)->getDimensionIndex("y"); 00232 int z_index = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index)->getDimensionIndex("z"); 00233 00234 ompl::base::RealVectorBounds real_vector_bounds = state_space_->as<ompl::base::CompoundStateSpace>()->getSubSpace("real_vector")->as<ompl::base::RealVectorStateSpace>()->getBounds(); 00235 00236 real_vector_bounds.low[x_index] = position_constraint.position.x-position_constraint.constraint_region_shape.dimensions[0]/2.0; 00237 real_vector_bounds.low[y_index] = position_constraint.position.y-position_constraint.constraint_region_shape.dimensions[1]/2.0; 00238 real_vector_bounds.low[z_index] = position_constraint.position.z-position_constraint.constraint_region_shape.dimensions[2]/2.0; 00239 00240 real_vector_bounds.high[x_index] = position_constraint.position.x+position_constraint.constraint_region_shape.dimensions[0]/2.0; 00241 real_vector_bounds.high[y_index] = position_constraint.position.y+position_constraint.constraint_region_shape.dimensions[1]/2.0; 00242 real_vector_bounds.high[z_index] = position_constraint.position.z+position_constraint.constraint_region_shape.dimensions[2]/2.0; 00243 00244 return true; 00245 } 00246 00247 bool OmplRosRPYIKTaskSpacePlanner::orientationConstraintToOmplStateBounds(const arm_navigation_msgs::OrientationConstraint &orientation_constraint, 00248 ompl::base::StateSpacePtr &goal) 00249 { 00250 int real_vector_index = state_space_->as<ompl::base::CompoundStateSpace>()->getSubSpaceIndex("real_vector"); 00251 00252 int roll_index = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index)->getDimensionIndex("roll"); 00253 int pitch_index = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index)->getDimensionIndex("pitch"); 00254 int yaw_index = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index)->getDimensionIndex("yaw"); 00255 00256 ompl::base::RealVectorBounds real_vector_bounds = state_space_->as<ompl::base::CompoundStateSpace>()->getSubSpace("real_vector")->as<ompl::base::RealVectorStateSpace>()->getBounds(); 00257 00258 btQuaternion orientation; 00259 double roll,pitch,yaw; 00260 tf::quaternionMsgToTF(orientation_constraint.orientation,orientation); 00261 btMatrix3x3 rotation(orientation); 00262 rotation.getRPY(roll,pitch,yaw); 00263 00264 double min_roll = roll - 0.75*orientation_constraint.absolute_roll_tolerance; 00265 double max_roll = roll + 0.75*orientation_constraint.absolute_roll_tolerance; 00266 if(fabs(orientation_constraint.absolute_roll_tolerance-M_PI) <= 0.001) 00267 { 00268 ROS_DEBUG("Roll is unconstrained"); 00269 min_roll = -M_PI; 00270 max_roll = M_PI; 00271 } 00272 double min_pitch = pitch - 0.75*orientation_constraint.absolute_pitch_tolerance; 00273 double max_pitch = pitch + 0.75*orientation_constraint.absolute_pitch_tolerance; 00274 if(fabs(orientation_constraint.absolute_pitch_tolerance-M_PI) <= 0.001) 00275 { 00276 ROS_DEBUG("Pitch is unconstrained"); 00277 min_pitch = -M_PI; 00278 max_pitch = M_PI; 00279 } 00280 double min_yaw = yaw - 0.75*orientation_constraint.absolute_yaw_tolerance; 00281 double max_yaw = yaw + 0.75*orientation_constraint.absolute_yaw_tolerance; 00282 if(fabs(orientation_constraint.absolute_yaw_tolerance-M_PI) <= 0.001) 00283 { 00284 ROS_DEBUG("Yaw is unconstrained"); 00285 min_yaw = -M_PI; 00286 max_yaw = M_PI; 00287 } 00288 real_vector_bounds.low[roll_index] = min_roll; 00289 real_vector_bounds.low[yaw_index] = min_yaw; 00290 real_vector_bounds.low[pitch_index] = min_pitch; 00291 real_vector_bounds.high[roll_index] = max_roll; 00292 real_vector_bounds.high[yaw_index] = max_yaw; 00293 real_vector_bounds.high[pitch_index] = max_pitch; 00294 ROS_DEBUG("Resetting constraints"); 00295 00296 ROS_DEBUG("Roll constraint: [%f, %f]",min_roll,max_roll); 00297 ROS_DEBUG("Pitch constraint: [%f, %f]",min_pitch,max_pitch); 00298 ROS_DEBUG("Yaw constraint: [%f, %f]",min_yaw,max_yaw); 00299 00300 state_space_->as<ompl::base::CompoundStateSpace>()->getSubSpace("real_vector")->as<ompl::base::RealVectorStateSpace>()->setBounds(real_vector_bounds); 00301 return true; 00302 } 00303 00304 00305 bool OmplRosRPYIKTaskSpacePlanner::getEndEffectorConstraints(const arm_navigation_msgs::Constraints &constraints, 00306 arm_navigation_msgs::PositionConstraint &position_constraint, 00307 arm_navigation_msgs::OrientationConstraint &orientation_constraint, 00308 const bool &need_both_constraints) 00309 { 00310 int position_index = -1; 00311 int orientation_index = -1; 00312 for(unsigned int i=0; i < constraints.position_constraints.size(); i++) 00313 { 00314 if(constraints.position_constraints[i].link_name == end_effector_name_) 00315 { 00316 position_index = i; 00317 break; 00318 } 00319 } 00320 for(unsigned int i=0; i < constraints.orientation_constraints.size(); i++) 00321 { 00322 if(constraints.orientation_constraints[i].link_name == end_effector_name_) 00323 { 00324 orientation_index = i; 00325 break; 00326 } 00327 } 00328 if((position_index < 0 || orientation_index < 0) && need_both_constraints) 00329 { 00330 ROS_ERROR("Need at least one position and orientation constraint to be specified in the message"); 00331 return false; 00332 } 00333 if(position_index >= 0) 00334 position_constraint = constraints.position_constraints[position_index]; 00335 if(orientation_index >= 0) 00336 orientation_constraint = constraints.orientation_constraints[orientation_index]; 00337 return true; 00338 } 00339 00340 bool OmplRosRPYIKTaskSpacePlanner::poseStampedToOmplState(const geometry_msgs::PoseStamped &desired_pose, 00341 ompl::base::ScopedState<ompl::base::CompoundStateSpace> &goal, 00342 const bool &return_if_outside_constraints) 00343 { 00344 btTransform desired_pose_tf; 00345 double roll, pitch, yaw, x, y, z; 00346 tf::poseMsgToTF(desired_pose.pose,desired_pose_tf); 00347 desired_pose_tf.getBasis().getRPY(roll,pitch,yaw); 00348 x = desired_pose.pose.position.x; 00349 y = desired_pose.pose.position.y; 00350 z = desired_pose.pose.position.z; 00351 00352 int real_vector_index = state_space_->as<ompl::base::CompoundStateSpace>()->getSubSpaceIndex("real_vector"); 00353 int x_index = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index)->getDimensionIndex("x"); 00354 int y_index = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index)->getDimensionIndex("y"); 00355 int z_index = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index)->getDimensionIndex("z"); 00356 00357 int roll_index = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index)->getDimensionIndex("roll"); 00358 int pitch_index = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index)->getDimensionIndex("pitch"); 00359 int yaw_index = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index)->getDimensionIndex("yaw"); 00360 00361 00362 double min_value,max_value; 00363 min_value = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index)->getBounds().low[roll_index]; 00364 max_value = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index)->getBounds().high[roll_index]; 00365 if(!checkAndCorrectForWrapAround(roll,min_value,max_value) && return_if_outside_constraints) 00366 { 00367 ROS_ERROR("Roll %f does not lie within constraint bounds [%f,%f]",roll,min_value,max_value); 00368 return false; 00369 } 00370 else 00371 ROS_DEBUG("Roll : %f [%f %f]",roll,min_value,max_value); 00372 00373 min_value = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index)->getBounds().low[pitch_index]; 00374 max_value = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index)->getBounds().high[pitch_index]; 00375 if(!checkAndCorrectForWrapAround(pitch,min_value,max_value) && return_if_outside_constraints) 00376 { 00377 ROS_ERROR("Pitch %f does not lie within constraint bounds [%f,%f]",pitch,min_value,max_value); 00378 return false; 00379 } 00380 else 00381 ROS_DEBUG("Pitch : %f [%f %f]",pitch,min_value,max_value); 00382 00383 min_value = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index)->getBounds().low[yaw_index]; 00384 max_value = state_space_->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(real_vector_index)->getBounds().high[yaw_index]; 00385 if(!checkAndCorrectForWrapAround(yaw,min_value,max_value) && return_if_outside_constraints) 00386 { 00387 ROS_ERROR("Yaw %f does not lie within constraint bounds [%f,%f]",yaw,min_value,max_value); 00388 return false; 00389 } 00390 else 00391 ROS_DEBUG("Yaw : %f [%f %f]",yaw,min_value,max_value); 00392 00393 goal->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index)->values[x_index] = x; 00394 goal->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index)->values[y_index] = y; 00395 goal->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index)->values[z_index] = z; 00396 00397 goal->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index)->values[roll_index] = roll; 00398 goal->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index)->values[pitch_index] = pitch; 00399 goal->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index)->values[yaw_index] = yaw; 00400 return true; 00401 } 00402 00403 bool OmplRosRPYIKTaskSpacePlanner::checkAndCorrectForWrapAround(double &angle, 00404 const double &min_v, 00405 const double &max_v) 00406 { 00407 if(angle >= min_v && angle <= max_v) 00408 return true; 00409 double new_angle = angle - 2*M_PI; 00410 if(new_angle >= min_v && new_angle <= max_v) 00411 { 00412 angle = new_angle; 00413 return true; 00414 } 00415 new_angle = angle + 2*M_PI; 00416 if(new_angle >= min_v && new_angle <= max_v) 00417 { 00418 angle = new_angle; 00419 return true; 00420 } 00421 return false; 00422 } 00423 00424 geometry_msgs::PoseStamped OmplRosRPYIKTaskSpacePlanner::getEndEffectorPose(const arm_navigation_msgs::RobotState &robot_state) 00425 { 00426 // 1. First check if the request contains a start state for the end effector pose 00427 // 2. Otherwise set the start pose to the current pose 00428 for(unsigned int i=0; i < robot_state.multi_dof_joint_state.child_frame_ids.size(); i++) 00429 { 00430 if(robot_state.multi_dof_joint_state.child_frame_ids[i] == end_effector_name_) 00431 { 00432 geometry_msgs::PoseStamped desired_pose; 00433 desired_pose.pose = robot_state.multi_dof_joint_state.poses[i]; 00434 desired_pose.header.stamp = ros::Time(); 00435 desired_pose.header.frame_id = robot_state.multi_dof_joint_state.frame_ids[i]; 00436 if(!collision_models_interface_->convertPoseGivenWorldTransform(*collision_models_interface_->getPlanningSceneState(), 00437 state_transformer_->getFrame(), 00438 desired_pose.header, 00439 desired_pose.pose, 00440 desired_pose)) { 00441 ROS_WARN_STREAM("getEndEffectorPose has problems transforming pose into frame " << state_transformer_->getFrame()); 00442 } 00443 return desired_pose; 00444 } 00445 } 00446 00447 planning_environment::setRobotStateAndComputeTransforms(robot_state, *collision_models_interface_->getPlanningSceneState()); 00448 btTransform end_effector_pose = collision_models_interface_->getPlanningSceneState()->getLinkState(end_effector_name_)->getGlobalLinkTransform(); 00449 00450 geometry_msgs::PoseStamped desired_pose; 00451 tf::poseTFToMsg(end_effector_pose,desired_pose.pose); 00452 desired_pose.header.stamp = ros::Time(); 00453 desired_pose.header.frame_id = collision_models_interface_->getWorldFrameId(); 00454 if(!collision_models_interface_->convertPoseGivenWorldTransform(*collision_models_interface_->getPlanningSceneState(), 00455 state_transformer_->getFrame(), 00456 desired_pose.header, 00457 desired_pose.pose, 00458 desired_pose)) { 00459 ROS_WARN_STREAM("getEndEffectorPose has problems transforming pose into frame " << state_transformer_->getFrame()); 00460 } 00461 00462 btQuaternion orientation; 00463 double roll,pitch,yaw; 00464 tf::quaternionMsgToTF(desired_pose.pose.orientation,orientation); 00465 btMatrix3x3 rotation(orientation); 00466 rotation.getRPY(roll,pitch,yaw); 00467 00468 ROS_DEBUG("End effector pose in frame %s: %f %f %f, %f %f %f",state_transformer_->getFrame().c_str(), desired_pose.pose.position.x,desired_pose.pose.position.y,desired_pose.pose.position.z,roll,pitch,yaw); 00469 ROS_DEBUG_STREAM("Quaternion is " 00470 << orientation.x() << " " 00471 << orientation.y() << " " 00472 << orientation.z() << " " 00473 << orientation.w()); 00474 return desired_pose; 00475 } 00476 00477 } 00478