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