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 #include <pr2_arm_kinematics_constraint_aware/pr2_arm_kinematics_constraint_aware.h>
00035 #include <pr2_arm_kinematics/pr2_arm_kinematics_utils.h>
00036 #include <geometry_msgs/PoseStamped.h>
00037 #include <kdl_parser/kdl_parser.hpp>
00038 #include <tf_conversions/tf_kdl.h>
00039 #include "ros/ros.h"
00040 #include <algorithm>
00041 #include <numeric>
00042
00043 #include <sensor_msgs/JointState.h>
00044 #include <kinematics_msgs/utils.h>
00045 #include <visualization_msgs/Marker.h>
00046 #include <visualization_msgs/MarkerArray.h>
00047 #include <planning_environment/models/model_utils.h>
00048
00049 using namespace KDL;
00050 using namespace tf;
00051 using namespace std;
00052 using namespace ros;
00053
00054 namespace pr2_arm_kinematics {
00055
00056 static const std::string IK_WITH_COLLISION_SERVICE = "get_constraint_aware_ik";
00057 static const double IK_DEFAULT_TIMEOUT = 10.0;
00058
00059 PR2ArmIKConstraintAware::PR2ArmIKConstraintAware(): PR2ArmKinematics(false)
00060 {
00061 node_handle_.param<bool>("visualize_solution",visualize_solution_,true);
00062 node_handle_.param<std::string>("group", group_, std::string());
00063 ROS_DEBUG("Advertising services");
00064
00065 if(!isActive())
00066 {
00067 ROS_ERROR("Could not load pr2_arm_kinematics_constraint_aware server");
00068 }
00069 else
00070 {
00071 ROS_INFO("Loading pr2_arm_kinematics_constraint_aware server");
00072 }
00073 collision_models_interface_ = new planning_environment::CollisionModelsInterface("robot_description");
00074 if(group_.empty()) {
00075 ROS_WARN("Must specify planning group in configuration");
00076 }
00077 const planning_models::KinematicModel::JointModelGroup* joint_model_group = collision_models_interface_->getKinematicModel()->getModelGroup(group_);
00078 if(joint_model_group == NULL) {
00079 ROS_WARN_STREAM("No joint group " << group_);
00080 }
00081 arm_links_ = joint_model_group->getGroupLinkNames();
00082
00083 const planning_models::KinematicModel::LinkModel* end_effector_link = collision_models_interface_->getKinematicModel()->getLinkModel(ik_solver_info_.link_names.back());
00084 end_effector_collision_links_ = collision_models_interface_->getKinematicModel()->getChildLinkModelNames(end_effector_link);
00085
00086 vis_marker_publisher_ = root_handle_.advertise<visualization_msgs::Marker>("kinematics_collisions", 128);
00087 vis_marker_array_publisher_ = root_handle_.advertise<visualization_msgs::MarkerArray>("kinematics_collisions_array", 128);
00088
00089 advertiseIK();
00090 }
00091
00092 void PR2ArmIKConstraintAware::advertiseIK()
00093 {
00094 ik_collision_service_ = node_handle_.advertiseService(IK_WITH_COLLISION_SERVICE,&PR2ArmIKConstraintAware::getConstraintAwarePositionIK,this);
00095 display_trajectory_publisher_ = root_handle_.advertise<arm_navigation_msgs::DisplayTrajectory>("ik_solution_display", 1);
00096 }
00097
00098 bool PR2ArmIKConstraintAware::isReady(arm_navigation_msgs::ArmNavigationErrorCodes &error_code)
00099 {
00100 if(!active_)
00101 {
00102 ROS_ERROR("IK service is not ready");
00103 return false;
00104 }
00105 if(!collision_models_interface_->isPlanningSceneSet()) {
00106 ROS_INFO("Planning scene not set");
00107 error_code.val = error_code.COLLISION_CHECKING_UNAVAILABLE;
00108 return false;
00109 }
00110 error_code.val = error_code.SUCCESS;
00111 return true;
00112 }
00113
00114 bool PR2ArmIKConstraintAware::transformPose(const std::string& des_frame,
00115 const geometry_msgs::PoseStamped& pose_in,
00116 geometry_msgs::PoseStamped& pose_out)
00117 {
00118 if(!collision_models_interface_->convertPoseGivenWorldTransform(*collision_models_interface_->getPlanningSceneState(),
00119 des_frame,
00120 pose_in.header,
00121 pose_in.pose,
00122 pose_out)) {
00123 ROS_WARN_STREAM("Problem transforming pose");
00124 return false;
00125 }
00126 return true;
00127 }
00128
00129
00130 int PR2ArmIKConstraintAware::CartToJntSearch(const KDL::JntArray& q_in,
00131 const KDL::Frame& p_in,
00132 KDL::JntArray &q_out,
00133 const double &timeout,
00134 arm_navigation_msgs::ArmNavigationErrorCodes& error_code)
00135 {
00136 if(!isReady(error_code))
00137 return -1;
00138 bool ik_valid = (pr2_arm_ik_solver_->CartToJntSearch(q_in,
00139 p_in,
00140 q_out,
00141 timeout,
00142 error_code,
00143 boost::bind(&PR2ArmIKConstraintAware::initialPoseCheck, this, _1, _2, _3),
00144 boost::bind(&PR2ArmIKConstraintAware::collisionCheck, this, _1, _2, _3)) >= 0);
00145
00146 return ik_valid;
00147 }
00148
00149 bool PR2ArmIKConstraintAware::getPositionIK(kinematics_msgs::GetPositionIK::Request &request,
00150 kinematics_msgs::GetPositionIK::Response &response)
00151 {
00152 if(!isReady(response.error_code))
00153 return true;
00154
00155 if(!checkIKService(request,response,ik_solver_info_))
00156 return true;
00157
00158 collision_models_interface_->resetToStartState(*collision_models_interface_->getPlanningSceneState());
00159
00160 geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped;
00161
00162 planning_environment::setRobotStateAndComputeTransforms(request.ik_request.robot_state, *collision_models_interface_->getPlanningSceneState());
00163
00164 if(!collision_models_interface_->convertPoseGivenWorldTransform(*collision_models_interface_->getPlanningSceneState(),
00165 root_name_,
00166 pose_msg_in.header,
00167 pose_msg_in.pose,
00168 request.ik_request.pose_stamped)) {
00169 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00170 return true;
00171 }
00172 return getPositionIKHelper(request, response);
00173 }
00174
00175 bool PR2ArmIKConstraintAware::getConstraintAwarePositionIK(kinematics_msgs::GetConstraintAwarePositionIK::Request &request_in,
00176 kinematics_msgs::GetConstraintAwarePositionIK::Response &response)
00177 {
00178 if(!isReady(response.error_code))
00179 return true;
00180
00181 if(!checkConstraintAwareIKService(request_in,response,ik_solver_info_))
00182 {
00183 ROS_ERROR("IK service request is malformed");
00184 return true;
00185 }
00186
00187 collision_models_interface_->resetToStartState(*collision_models_interface_->getPlanningSceneState());
00188
00189 collision_models_interface_->disableCollisionsForNonUpdatedLinks(group_);
00190
00191 ros::Time start_time = ros::Time::now();
00192 ROS_DEBUG("Received IK request is in the frame: %s",request_in.ik_request.pose_stamped.header.frame_id.c_str());
00193
00194 ik_request_ = request_in.ik_request;
00195 constraints_ = request_in.constraints;
00196
00197 geometry_msgs::PoseStamped pose_msg_in = ik_request_.pose_stamped;
00198 geometry_msgs::PoseStamped pose_msg_out;
00199
00200 ROS_DEBUG_STREAM("Ik request in frame " << pose_msg_in.header << " is "
00201 << pose_msg_in.pose.position.x << " "
00202 << pose_msg_in.pose.position.y << " "
00203 << pose_msg_in.pose.position.z << " "
00204 << pose_msg_in.pose.orientation.x << " "
00205 << pose_msg_in.pose.orientation.y << " "
00206 << pose_msg_in.pose.orientation.z << " "
00207 << pose_msg_in.pose.orientation.w);
00208
00209 planning_environment::setRobotStateAndComputeTransforms(request_in.ik_request.robot_state, *collision_models_interface_->getPlanningSceneState());
00210
00211 if(!collision_models_interface_->convertPoseGivenWorldTransform(*collision_models_interface_->getPlanningSceneState(),
00212 root_name_,
00213 pose_msg_in.header,
00214 pose_msg_in.pose,
00215 pose_msg_out)) {
00216 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00217 return true;
00218 }
00219 ik_request_.pose_stamped = pose_msg_out;
00220
00221 ROS_DEBUG_STREAM("Ik request in frame " << root_name_ << " is "
00222 << pose_msg_out.pose.position.x << " "
00223 << pose_msg_out.pose.position.y << " "
00224 << pose_msg_out.pose.position.z << " "
00225 << pose_msg_out.pose.orientation.x << " "
00226 << pose_msg_out.pose.orientation.y << " "
00227 << pose_msg_out.pose.orientation.z << " "
00228 << pose_msg_out.pose.orientation.w);
00229
00230 KDL::JntArray jnt_pos_out;
00231 KDL::JntArray jnt_pos_in;
00232 KDL::Frame p_in;
00233 tf::poseMsgToKDL(pose_msg_out.pose,p_in);
00234 jnt_pos_in.resize(dimension_);
00235 jnt_pos_out.resize(dimension_);
00236 for(unsigned int i=0; i < request_in.ik_request.ik_seed_state.joint_state.name.size(); i++)
00237 {
00238 int tmp_index = pr2_arm_kinematics::getJointIndex(request_in.ik_request.ik_seed_state.joint_state.name[i],ik_solver_info_);
00239 if(tmp_index != -1) {
00240 ROS_DEBUG_STREAM("Setting name " << request_in.ik_request.ik_seed_state.joint_state.name[i]
00241 << " index " << tmp_index
00242 << " to " << request_in.ik_request.ik_seed_state.joint_state.position[i]);
00243 jnt_pos_in(tmp_index) = request_in.ik_request.ik_seed_state.joint_state.position[i];
00244 }
00245 }
00246
00247 ros::Time ik_solver_time = ros::Time::now();
00248 bool ik_valid = CartToJntSearch(jnt_pos_in,
00249 p_in,
00250 jnt_pos_out,
00251 request_in.timeout.toSec(),
00252 response.error_code);
00253
00254 if(ik_valid)
00255 {
00256 response.solution.joint_state.name = ik_solver_info_.joint_names;
00257 response.solution.joint_state.position.resize(dimension_);
00258 for(int i=0; i < dimension_; i++)
00259 {
00260 response.solution.joint_state.position[i] = jnt_pos_out(i);
00261 ROS_DEBUG("IK Solution: %s %d: %f",response.solution.joint_state.name[i].c_str(),i,jnt_pos_out(i));
00262 }
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274 ROS_DEBUG("IK service time: %f",(ros::Time::now()-start_time).toSec());
00275 response.error_code.val = response.error_code.SUCCESS;
00276 return true;
00277 }
00278 else
00279 {
00280 ROS_DEBUG_STREAM("An IK solution could not be found " << response.error_code.val);
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290 if(response.error_code.val != response.error_code.IK_LINK_IN_COLLISION) {
00291 sendEndEffectorPose(collision_models_interface_->getPlanningSceneState(),true);
00292 std_msgs::ColorRGBA col;
00293 col.a = .9;
00294 col.r = 1.0;
00295 col.b = 1.0;
00296 col.g = 0.0;
00297 visualization_msgs::MarkerArray arr;
00298 collision_models_interface_->getRobotMarkersGivenState(*collision_models_interface_->getPlanningSceneState(),
00299 arr,
00300 col,
00301 "start_pose",
00302 ros::Duration(0.0));
00303 vis_marker_array_publisher_.publish(arr);
00304 }
00305 return true;
00306 }
00307 }
00308
00309 void PR2ArmIKConstraintAware::collisionCheck(const KDL::JntArray &jnt_array,
00310 const KDL::Frame &p_in,
00311 arm_navigation_msgs::ArmNavigationErrorCodes &error_code)
00312 {
00313 ros::Time n1 = ros::Time::now();
00314
00315 std::map<std::string, double> joint_values;
00316 for(unsigned int i=0; i < ik_solver_info_.joint_names.size(); i++)
00317 {
00318 joint_values[ik_solver_info_.joint_names[i]] = jnt_array(i);
00319 }
00320 collision_models_interface_->getPlanningSceneState()->setKinematicState(joint_values);
00321 if(collision_models_interface_->getPlanningSceneState() == NULL) {
00322 ROS_INFO_STREAM("Messed up");
00323 }
00324 if(collision_models_interface_->isKinematicStateInCollision(*(collision_models_interface_->getPlanningSceneState()))) {
00325 visualization_msgs::MarkerArray arr;
00326 std_msgs::ColorRGBA col;
00327 col.a = .9;
00328 col.r = 1.0;
00329 col.b = 0.0;
00330 col.g = 0.0;
00331 collision_models_interface_->getAllCollisionPointMarkers(*collision_models_interface_->getPlanningSceneState(),
00332 arr,
00333 col,
00334 ros::Duration(0.0));
00335 vis_marker_array_publisher_.publish(arr);
00336 error_code.val = error_code.KINEMATICS_STATE_IN_COLLISION;
00337 } else {
00338 error_code.val = error_code.SUCCESS;
00339 }
00340
00341 if(!planning_environment::doesKinematicStateObeyConstraints(*(collision_models_interface_->getPlanningSceneState()),
00342 constraints_)) {
00343 error_code.val = error_code.INVALID_GOAL_POSITION_CONSTRAINTS;
00344 }
00345 ROS_DEBUG_STREAM("Collision check took " << (ros::Time::now()-n1).toSec());
00346 }
00347
00348 void PR2ArmIKConstraintAware::initialPoseCheck(const KDL::JntArray &jnt_array,
00349 const KDL::Frame &p_in,
00350 arm_navigation_msgs::ArmNavigationErrorCodes &error_code)
00351 {
00352 std::string kinematic_frame_id = pr2_arm_ik_solver_->getFrameId();
00353 std::string planning_frame_id = collision_models_interface_->getWorldFrameId();
00354 geometry_msgs::PoseStamped pose_stamped;
00355 tf::poseKDLToMsg(p_in,pose_stamped.pose);
00356 pose_stamped.header.stamp = ros::Time::now();
00357 pose_stamped.header.frame_id = kinematic_frame_id;
00358 if(!collision_models_interface_->convertPoseGivenWorldTransform(*collision_models_interface_->getPlanningSceneState(),
00359 planning_frame_id,
00360 pose_stamped.header,
00361 pose_stamped.pose,
00362 pose_stamped)) {
00363 ROS_ERROR_STREAM("Cannot transform from " << pose_stamped.header.frame_id << " to " << planning_frame_id);
00364 }
00365
00366 collision_space::EnvironmentModel::AllowedCollisionMatrix save_acm = collision_models_interface_->getCurrentAllowedCollisionMatrix();
00367 collision_space::EnvironmentModel::AllowedCollisionMatrix acm = save_acm;
00368 for(unsigned int i = 0; i < arm_links_.size(); i++) {
00369 acm.changeEntry(arm_links_[i], true);
00370 }
00371
00372 collision_models_interface_->setAlteredAllowedCollisionMatrix(acm);
00373
00374 tf::Transform transform;
00375 tf::poseMsgToTF(pose_stamped.pose,transform);
00376 if(!collision_models_interface_->getPlanningSceneState()->hasLinkState(ik_request_.ik_link_name)) {
00377 ROS_ERROR("Could not find end effector root_link %s", ik_request_.ik_link_name.c_str());
00378 error_code.val = error_code.INVALID_LINK_NAME;
00379 return;
00380 }
00381 collision_models_interface_->getPlanningSceneState()->updateKinematicStateWithLinkAt(ik_request_.ik_link_name, transform);
00382 if(collision_models_interface_->isKinematicStateInCollision(*(collision_models_interface_->getPlanningSceneState()))) {
00383 visualization_msgs::MarkerArray arr;
00384 std_msgs::ColorRGBA col;
00385 col.a = .9;
00386 col.r = 1.0;
00387 col.b = 0.0;
00388 col.g = 0.0;
00389 collision_models_interface_->getAllCollisionPointMarkers(*collision_models_interface_->getPlanningSceneState(),
00390 arr,
00391 col,
00392 ros::Duration(0.0));
00393 vis_marker_array_publisher_.publish(arr);
00394 error_code.val = error_code.IK_LINK_IN_COLLISION;
00395 ROS_DEBUG_STREAM("Initial pose check failing");
00396 sendEndEffectorPose(collision_models_interface_->getPlanningSceneState(), false);
00397 }
00398 else
00399 error_code.val = error_code.SUCCESS;
00400
00401 collision_models_interface_->setAlteredAllowedCollisionMatrix(save_acm);
00402 }
00403
00404 void PR2ArmIKConstraintAware::sendEndEffectorPose(const planning_models::KinematicState* state, bool valid) {
00405 visualization_msgs::MarkerArray hand_array;
00406 std_msgs::ColorRGBA col;
00407 col.a = .8;
00408 col.b = 0.0;
00409
00410 if(valid) {
00411 col.g = 1.0;
00412 } else {
00413 col.r = 1.0;
00414 }
00415
00416 collision_models_interface_->getRobotMarkersGivenState(*state,
00417 hand_array,
00418 col,
00419 "end_effector",
00420 ros::Duration(0.0),
00421 &end_effector_collision_links_);
00422 vis_marker_array_publisher_.publish(hand_array);
00423 }
00424
00425 void PR2ArmIKConstraintAware::printStringVec(const std::string &prefix, const std::vector<std::string> &string_vector)
00426 {
00427 ROS_DEBUG("%s",prefix.c_str());
00428 for(unsigned int i=0; i < string_vector.size(); i++)
00429 {
00430 ROS_DEBUG("%s",string_vector[i].c_str());
00431 }
00432 }
00433 }
00434