ompl_ros_rpy_ik_task_space_planner.cpp
Go to the documentation of this file.
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.getStateCount();
00071   ROS_DEBUG("Path has %d waypoints",(int)path.getStateCount());
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.getState(i)),
00079                                              robot_state))
00080     {
00081       ROS_ERROR("Could not transform solution waypoint");
00082       std::stringstream string_stream;
00083       state_space_->printState(path.getState(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   tf::Quaternion orientation;
00202   double roll,pitch,yaw;
00203   tf::quaternionMsgToTF(desired_pose.pose.orientation,orientation);
00204   tf::Matrix3x3 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   tf::Quaternion orientation;
00259   double roll,pitch,yaw;
00260   tf::quaternionMsgToTF(orientation_constraint.orientation,orientation);
00261   tf::Matrix3x3 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   tf::Transform 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   tf::Transform 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   tf::Quaternion orientation;
00463   double roll,pitch,yaw;
00464   tf::quaternionMsgToTF(desired_pose.pose.orientation,orientation);
00465   tf::Matrix3x3 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                                      


ompl_ros_interface
Author(s): Sachin Chitta
autogenerated on Thu Dec 12 2013 11:10:58