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/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
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
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
00145 ompl::base::ScopedState<ompl::base::CompoundStateSpace> start(state_space_);
00146
00147
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
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
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
00188 if(!ompl_ros_interface::constraintsToOmplState(tmp_constraints,goal,false))
00189 return false;
00190
00191
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
00427
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