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
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(),setup_collision_environment_(false)
00060 {
00061 node_handle_.param<bool>("visualize_solution",visualize_solution_,true);
00062 ROS_DEBUG("Advertising services");
00063
00064 if(!isActive())
00065 {
00066 ROS_ERROR("Could not load pr2_arm_kinematics_constraint_aware server");
00067 }
00068 else
00069 {
00070 ROS_INFO("Loading pr2_arm_kinematics_constraint_aware server");
00071 }
00072 advertiseIK();
00073
00074 if(setupCollisionEnvironment())
00075 ROS_INFO("Collision environment setup.");
00076 else
00077 ROS_ERROR("Could not initialize collision environment");
00078 }
00079
00080 void PR2ArmIKConstraintAware::advertiseIK()
00081 {
00082 ik_collision_service_ = node_handle_.advertiseService(IK_WITH_COLLISION_SERVICE,&PR2ArmIKConstraintAware::getConstraintAwarePositionIK,this);
00083 display_trajectory_publisher_ = root_handle_.advertise<motion_planning_msgs::DisplayTrajectory>("ik_solution_display", 1);
00084 }
00085
00086 bool PR2ArmIKConstraintAware::isReady(motion_planning_msgs::ArmNavigationErrorCodes &error_code)
00087 {
00088 if(!active_)
00089 {
00090 ROS_ERROR("IK service is not ready");
00091 return false;
00092 }
00093 if(!setup_collision_environment_)
00094 {
00095 ROS_INFO("Waiting for collision environment setup.");
00096 if(!setupCollisionEnvironment())
00097 {
00098 ROS_INFO("Could not initialize collision environment");
00099 error_code.val = error_code.COLLISION_CHECKING_UNAVAILABLE;
00100 return false;
00101 }
00102 else
00103 {
00104 setup_collision_environment_ = true;
00105 }
00106 }
00107 error_code.val = error_code.SUCCESS;
00108 return true;
00109 }
00110
00111
00112 int PR2ArmIKConstraintAware::CartToJntSearch(const KDL::JntArray& q_in,
00113 const KDL::Frame& p_in,
00114 KDL::JntArray &q_out,
00115 const double &timeout,
00116 motion_planning_msgs::ArmNavigationErrorCodes& error_code)
00117 {
00118 if(!isReady(error_code))
00119 return -1;
00120 bool ik_valid = (pr2_arm_ik_solver_->CartToJntSearch(q_in,
00121 p_in,
00122 q_out,
00123 timeout,
00124 error_code,
00125 boost::bind(&PR2ArmIKConstraintAware::initialPoseCheck, this, _1, _2, _3),
00126 boost::bind(&PR2ArmIKConstraintAware::collisionCheck, this, _1, _2, _3)) >= 0);
00127
00128 return ik_valid;
00129 }
00130
00131 bool PR2ArmIKConstraintAware::getConstraintAwarePositionIK(kinematics_msgs::GetConstraintAwarePositionIK::Request &request_in,
00132 kinematics_msgs::GetConstraintAwarePositionIK::Response &response)
00133 {
00134
00135 if(!isReady(response.error_code))
00136 return true;
00137
00138 if(!checkConstraintAwareIKService(request_in,response,ik_solver_info_))
00139 {
00140 ROS_ERROR("IK service request is malformed");
00141 return true;
00142 }
00143
00144 ros::Time start_time = ros::Time::now();
00145 ROS_INFO("Received IK request is in the frame: %s",request_in.ik_request.pose_stamped.header.frame_id.c_str());
00146
00147 ik_request_ = request_in.ik_request;
00148 collision_operations_ = request_in.ordered_collision_operations;
00149 link_padding_ = request_in.link_padding;
00150 allowed_contacts_ = request_in.allowed_contacts;
00151 constraints_ = request_in.constraints;
00152
00153 geometry_msgs::PoseStamped pose_msg_in = ik_request_.pose_stamped;
00154 geometry_msgs::PoseStamped pose_msg_out;
00155 if(!pr2_arm_kinematics::convertPoseToRootFrame(pose_msg_in,pose_msg_out,root_name_,tf_))
00156 {
00157 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00158 return true;
00159 }
00160 motion_planning_msgs::Constraints emp;
00161
00162
00163 planning_monitor_->prepareForValidityChecks(ik_solver_info_.joint_names,
00164 collision_operations_,
00165 allowed_contacts_,
00166 emp,
00167 constraints_,
00168 link_padding_,
00169 response.error_code);
00170
00171
00172 if(response.error_code.val == response.error_code.FRAME_TRANSFORM_FAILURE) {
00173 return true;
00174 }
00175
00176 kinematic_state_ = new planning_models::KinematicState(planning_monitor_->getKinematicModel());
00177
00178 planning_monitor_->setRobotStateAndComputeTransforms(ik_request_.robot_state, *kinematic_state_);
00179
00180 ik_request_.pose_stamped = pose_msg_out;
00181 ROS_INFO("Transformed IK request is in the frame: %s",ik_request_.pose_stamped.header.frame_id.c_str());
00182
00183 KDL::JntArray jnt_pos_out;
00184 KDL::JntArray jnt_pos_in;
00185 KDL::Frame p_in;
00186 tf::PoseMsgToKDL(pose_msg_out.pose,p_in);
00187 jnt_pos_in.resize(dimension_);
00188 jnt_pos_out.resize(dimension_);
00189 for(unsigned int i=0; i < request_in.ik_request.ik_seed_state.joint_state.name.size(); i++)
00190 {
00191 int tmp_index = pr2_arm_kinematics::getJointIndex(request_in.ik_request.ik_seed_state.joint_state.name[i],ik_solver_info_);
00192 if(tmp_index != -1) {
00193 ROS_DEBUG_STREAM("Setting name " << request_in.ik_request.ik_seed_state.joint_state.name[i]
00194 << " index " << tmp_index
00195 << " to " << request_in.ik_request.ik_seed_state.joint_state.position[i]);
00196 jnt_pos_in(tmp_index) = request_in.ik_request.ik_seed_state.joint_state.position[i];
00197 }
00198 }
00199
00200 ros::Time ik_solver_time = ros::Time::now();
00201 bool ik_valid = CartToJntSearch(jnt_pos_in,
00202 p_in,
00203 jnt_pos_out,
00204 request_in.timeout.toSec(),
00205 response.error_code);
00206 ROS_INFO("IK solver time: %f",(ros::Time::now()-ik_solver_time).toSec());
00207
00208 planning_monitor_->revertToDefaultState();
00209 if(ik_valid)
00210 {
00211 response.solution.joint_state.name = ik_solver_info_.joint_names;
00212 response.solution.joint_state.position.resize(dimension_);
00213 for(int i=0; i < dimension_; i++)
00214 {
00215 response.solution.joint_state.position[i] = jnt_pos_out(i);
00216 ROS_DEBUG("IK Solution: %s %d: %f",response.solution.joint_state.name[i].c_str(),i,jnt_pos_out(i));
00217 }
00218 if(visualize_solution_)
00219 {
00220 motion_planning_msgs::DisplayTrajectory display_trajectory;
00221 display_trajectory.trajectory.joint_trajectory.points.resize(1);
00222 display_trajectory.trajectory.joint_trajectory.points[0].positions = response.solution.joint_state.position;
00223 display_trajectory.trajectory.joint_trajectory.joint_names = response.solution.joint_state.name;
00224 planning_monitor_->convertKinematicStateToRobotState(*kinematic_state_,display_trajectory.robot_state);
00225 display_trajectory_publisher_.publish(display_trajectory);
00226 }
00227 ROS_INFO("IK service time: %f",(ros::Time::now()-start_time).toSec());
00228 response.error_code.val = response.error_code.SUCCESS;
00229 delete kinematic_state_;
00230 return true;
00231 }
00232 else
00233 {
00234 ROS_ERROR("An IK solution could not be found");
00235 if(response.error_code.val != response.error_code.IK_LINK_IN_COLLISION) {
00236 sendEndEffectorPose(kinematic_state_,true);
00237 }
00238 delete kinematic_state_;
00239 return true;
00240 }
00241 }
00242
00243 void PR2ArmIKConstraintAware::collisionCheck(const KDL::JntArray &jnt_array,
00244 const KDL::Frame &p_in,
00245 motion_planning_msgs::ArmNavigationErrorCodes &error_code)
00246 {
00247 std::map<std::string, double> joint_values;
00248 for(unsigned int i=0; i < ik_solver_info_.joint_names.size(); i++)
00249 {
00250 joint_values[ik_solver_info_.joint_names[i]] = jnt_array(i);
00251 }
00252 kinematic_state_->setKinematicState(joint_values);
00253 planning_monitor_->getEnvironmentModel()->updateRobotModel(kinematic_state_);
00254
00255 bool check = (!planning_monitor_->getEnvironmentModel()->isCollision() && !planning_monitor_->getEnvironmentModel()->isSelfCollision());
00256 if(!check) {
00257 planning_monitor_->broadcastCollisions();
00258 error_code.val = error_code.KINEMATICS_STATE_IN_COLLISION;
00259 }
00260 else {
00261 error_code.val = error_code.SUCCESS;
00262 }
00263
00264 if(!planning_monitor_->checkGoalConstraints(kinematic_state_, false)) {
00265 error_code.val = error_code.INVALID_GOAL_POSITION_CONSTRAINTS;
00266 ROS_INFO("Constraints violated at current state");
00267 }
00268 }
00269
00270 void PR2ArmIKConstraintAware::initialPoseCheck(const KDL::JntArray &jnt_array,
00271 const KDL::Frame &p_in,
00272 motion_planning_msgs::ArmNavigationErrorCodes &error_code)
00273 {
00274 std::string kinematic_frame_id = pr2_arm_ik_solver_->getFrameId();
00275 std::string planning_frame_id = planning_monitor_->getWorldFrameId();
00276 geometry_msgs::PoseStamped pose_stamped;
00277 tf::PoseKDLToMsg(p_in,pose_stamped.pose);
00278 pose_stamped.header.stamp = ros::Time::now();
00279 pose_stamped.header.frame_id = kinematic_frame_id;
00280 if (!tf_.canTransform(planning_frame_id,
00281 pose_stamped.header.frame_id,
00282 pose_stamped.header.stamp))
00283 {
00284 std::string err;
00285 ros::Time tmp;
00286 if(tf_.getLatestCommonTime(pose_stamped.header.frame_id,planning_frame_id,tmp,&err) != tf::NO_ERROR)
00287 {
00288 ROS_ERROR("Cannot transform from '%s' to '%s'. TF said: %s",
00289 pose_stamped.header.frame_id.c_str(),planning_frame_id.c_str(), err.c_str());
00290 }
00291 else
00292 pose_stamped.header.stamp = tmp;
00293 }
00294
00295 try
00296 {
00297 tf_.transformPose(planning_frame_id,pose_stamped,pose_stamped);
00298 }
00299 catch(tf::TransformException& ex)
00300 {
00301 ROS_ERROR("Cannot transform from '%s' to '%s'. Tf said: %s",
00302 pose_stamped.header.frame_id.c_str(),planning_frame_id.c_str(), ex.what());
00303 error_code.val = error_code.FRAME_TRANSFORM_FAILURE;
00304 return;
00305 }
00306
00307
00308 std::vector<std::vector<bool> > orig_allowed;
00309 std::map<std::string, unsigned int> vecIndices;
00310 planning_monitor_->getEnvironmentModel()->getCurrentAllowedCollisionMatrix(orig_allowed, vecIndices);
00311
00312 std::vector<std::vector<bool> > new_allowed = orig_allowed;
00313 for(unsigned int i = 0; i < arm_links_.size(); i++) {
00314 unsigned int vind = vecIndices[arm_links_[i]];
00315 for(unsigned int j = 0; j < new_allowed.size(); j++) {
00316 new_allowed[vind][j] = true;
00317 new_allowed[j][vind] = true;
00318 }
00319 }
00320
00321
00322 planning_monitor_->getEnvironmentModel()->setAllowedCollisionMatrix(new_allowed, vecIndices);
00323
00324 btTransform transform;
00325 tf::poseMsgToTF(pose_stamped.pose,transform);
00326 if(!kinematic_state_->hasLinkState(ik_request_.ik_link_name)) {
00327 ROS_ERROR("Could not find end effector root_link %s", ik_request_.ik_link_name.c_str());
00328 error_code.val = error_code.INVALID_LINK_NAME;
00329 return;
00330 }
00331 kinematic_state_->updateKinematicStateWithLinkAt(ik_request_.ik_link_name, transform);
00332 planning_monitor_->getEnvironmentModel()->updateRobotModel(kinematic_state_);
00333
00334 bool check = ( !planning_monitor_->getEnvironmentModel()->isCollision() &&
00335 !planning_monitor_->getEnvironmentModel()->isSelfCollision() );
00336 if(!check) {
00337 error_code.val = error_code.IK_LINK_IN_COLLISION;
00338 planning_monitor_->broadcastCollisions();
00339 sendEndEffectorPose(kinematic_state_, false);
00340 }
00341 else
00342 error_code.val = error_code.SUCCESS;
00343
00344 planning_monitor_->getEnvironmentModel()->setAllowedCollisionMatrix(orig_allowed, vecIndices);
00345
00346 ROS_DEBUG("Initial pose check done with result %s",check ? "not_in_collision" : "in_collision" );
00347 }
00348
00349 void PR2ArmIKConstraintAware::sendEndEffectorPose(const planning_models::KinematicState* state, bool valid) {
00350 if(!robot_model_initialized_) return;
00351 visualization_msgs::MarkerArray hand_array;
00352 unsigned int id = 0;
00353 for(unsigned int i = 0; i < end_effector_collision_links_.size(); i++) {
00354 boost::shared_ptr<const urdf::Link> urdf_link = robot_model_.getLink(end_effector_collision_links_[i]);
00355 if(urdf_link == NULL) {
00356 ROS_DEBUG_STREAM("No entry in urdf for link " << end_effector_collision_links_[i]);
00357 continue;
00358 }
00359 if(!urdf_link->collision) {
00360 continue;
00361 }
00362 const urdf::Geometry *geom = urdf_link->collision->geometry.get();
00363 if(!geom) {
00364 ROS_DEBUG_STREAM("No collision geometry for link " << end_effector_collision_links_[i]);
00365 continue;
00366 }
00367 const urdf::Mesh *mesh = dynamic_cast<const urdf::Mesh*>(geom);
00368 if(mesh) {
00369 if (!mesh->filename.empty()) {
00370 planning_models::KinematicState::LinkState* ls = state->getLinkState(end_effector_collision_links_[i]);
00371 visualization_msgs::Marker mark;
00372 mark.header.frame_id = planning_monitor_->getWorldFrameId();
00373 mark.header.stamp = ros::Time::now();
00374 mark.id = id++;
00375 if(!valid) {
00376 mark.ns = "initial_pose_collision";
00377 } else {
00378 mark.ns = "initial_pose_ok";
00379 }
00380 mark.type = mark.MESH_RESOURCE;
00381 mark.scale.x = 1.0;
00382 mark.scale.y = 1.0;
00383 mark.scale.z = 1.0;
00384 if(!valid) {
00385 mark.color.r = 1.0;
00386 } else {
00387 mark.color.g = 1.0;
00388 }
00389 mark.color.a = .8;
00390 mark.pose.position.x = ls->getGlobalCollisionBodyTransform().getOrigin().x();
00391 mark.pose.position.y = ls->getGlobalCollisionBodyTransform().getOrigin().y();
00392 mark.pose.position.z = ls->getGlobalCollisionBodyTransform().getOrigin().z();
00393 mark.pose.orientation.x = ls->getGlobalCollisionBodyTransform().getRotation().x();
00394 mark.pose.orientation.y = ls->getGlobalCollisionBodyTransform().getRotation().y();
00395 mark.pose.orientation.z = ls->getGlobalCollisionBodyTransform().getRotation().z();
00396 mark.pose.orientation.w = ls->getGlobalCollisionBodyTransform().getRotation().w();
00397 mark.mesh_resource = mesh->filename;
00398 hand_array.markers.push_back(mark);
00399 }
00400 }
00401 }
00402 vis_marker_array_publisher_.publish(hand_array);
00403 }
00404
00405 void PR2ArmIKConstraintAware::printStringVec(const std::string &prefix, const std::vector<std::string> &string_vector)
00406 {
00407 ROS_DEBUG("%s",prefix.c_str());
00408 for(unsigned int i=0; i < string_vector.size(); i++)
00409 {
00410 ROS_DEBUG("%s",string_vector[i].c_str());
00411 }
00412 }
00413
00414 bool PR2ArmIKConstraintAware::setupCollisionEnvironment()
00415 {
00416 node_handle_.param<std::string>("group", group_, std::string());
00417 node_handle_.param<bool>("use_collision_map", use_collision_map_, true);
00418 if (group_.empty())
00419 {
00420 ROS_ERROR("No 'group' parameter specified. Without the name of the group of joints to monitor, action cannot start");
00421 return false;
00422 }
00423
00424 std::string urdf_xml,full_urdf_xml;
00425 root_handle_.param("urdf_xml", urdf_xml, std::string("robot_description"));
00426 if(!root_handle_.getParam(urdf_xml,full_urdf_xml))
00427 {
00428 ROS_ERROR("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
00429 robot_model_initialized_ = false;
00430 }
00431 else
00432 {
00433 robot_model_.initString(full_urdf_xml);
00434 robot_model_initialized_ = true;
00435 }
00436
00437
00438 collision_models_ = new planning_environment::CollisionModels("robot_description");
00439
00440 if(!collision_models_)
00441 {
00442 ROS_INFO("Could not initialize collision models");
00443 return false;
00444 }
00445 planning_monitor_ = new planning_environment::PlanningMonitor(collision_models_, &tf_);
00446
00447 if(!planning_monitor_)
00448 {
00449 ROS_ERROR("Could not initialize planning monitor");
00450 return false;
00451 }
00452 else
00453 {
00454 ROS_INFO("Initialized planning monitor");
00455 }
00456
00457 planning_monitor_->setUseCollisionMap(use_collision_map_);
00458 planning_monitor_->startEnvironmentMonitor();
00459 if(!collision_models_->loadedModels())
00460 {
00461 ROS_ERROR("Could not load models");
00462 return false;
00463 }
00464 if (!collision_models_->getKinematicModel()->hasModelGroup(group_))
00465 {
00466 ROS_ERROR("Group '%s' is not known", group_.c_str());
00467 return false;
00468 }
00469 else
00470 ROS_INFO("Configuring action for '%s'", group_.c_str());
00471
00472 const std::vector<const planning_models::KinematicModel::JointModel*>& p_joints = collision_models_->getKinematicModel()->getModelGroup(group_)->getJointModels();
00473 for(unsigned int i=0; i < p_joints.size(); i++)
00474 {
00475 default_collision_links_.push_back(p_joints[i]->getChildLinkModel()->getName());
00476 }
00477
00478
00479 arm_links_ = collision_models_->getPlanningGroupLinks().at(group_);
00480
00481 if (planning_monitor_->getExpectedJointStateUpdateInterval() > 1e-3)
00482 planning_monitor_->waitForState();
00483 if (planning_monitor_->getExpectedMapUpdateInterval() > 1e-3 && use_collision_map_)
00484 planning_monitor_->waitForMap();
00485
00486
00487 end_effector_collision_links_.clear();
00488 const planning_models::KinematicModel::LinkModel* end_effector_link = planning_monitor_->getKinematicModel()->getLinkModel(ik_solver_info_.link_names.back());
00489 std::vector<const planning_models::KinematicModel::LinkModel*> tempLinks;
00490 planning_monitor_->getKinematicModel()->getChildLinkModels(end_effector_link, tempLinks);
00491 for (unsigned int i = 0 ; i < tempLinks.size() ; ++i)
00492 end_effector_collision_links_.push_back(tempLinks[i]->getName());
00493 for(unsigned int i=0; i < end_effector_collision_links_.size(); i++)
00494 {
00495 default_collision_links_.push_back(end_effector_collision_links_[i]);
00496 }
00497
00498 printStringVec("Default collision links",default_collision_links_);
00499 printStringVec("End effector links",end_effector_collision_links_);
00500
00501 ROS_DEBUG("Root link name is: %s",root_name_.c_str());
00502
00503 planning_monitor_->setOnCollisionContactCallback(boost::bind(&PR2ArmIKConstraintAware::contactFound, this, _1));
00504 vis_marker_publisher_ = root_handle_.advertise<visualization_msgs::Marker>("kinematics_collisions", 128);
00505 vis_marker_array_publisher_ = root_handle_.advertise<visualization_msgs::MarkerArray>("kinematics_collisions_array", 128);
00506
00507 setup_collision_environment_ = true;
00508 return true;
00509 }
00510
00513 void PR2ArmIKConstraintAware::contactFound(collision_space::EnvironmentModel::Contact &contact)
00514 {
00515
00516 static int count = 0;
00517
00518 std::string ns_name;
00519 if(contact.link1 != NULL) {
00520
00521 if(contact.link1_attached_body_index == 0) {
00522 ns_name += contact.link1->getName()+"+";
00523 } else {
00524 if(contact.link1->getAttachedBodyModels().size() < contact.link1_attached_body_index) {
00525 ROS_ERROR("Link doesn't have attached body with indicated index");
00526 } else {
00527 ns_name += contact.link1->getAttachedBodyModels()[contact.link1_attached_body_index-1]->getName()+"+";
00528 }
00529 }
00530 }
00531
00532 if(contact.link2 != NULL) {
00533
00534 if(contact.link2_attached_body_index == 0) {
00535 ns_name += contact.link2->getName();
00536 } else {
00537 if(contact.link2->getAttachedBodyModels().size() < contact.link2_attached_body_index) {
00538 ROS_ERROR("Link doesn't have attached body with indicated index");
00539 } else {
00540 ns_name += contact.link2->getAttachedBodyModels()[contact.link2_attached_body_index-1]->getName();
00541 }
00542 }
00543 }
00544
00545 if(!contact.object_name.empty()) {
00546
00547 ns_name += contact.object_name;
00548 }
00549
00550 visualization_msgs::Marker mk;
00551 mk.header.stamp = planning_monitor_->lastPoseUpdate();
00552 mk.header.frame_id = planning_monitor_->getWorldFrameId();
00553 mk.ns = ns_name;
00554 mk.id = count++;
00555 mk.type = visualization_msgs::Marker::SPHERE;
00556 mk.action = visualization_msgs::Marker::ADD;
00557 mk.pose.position.x = contact.pos.x();
00558 mk.pose.position.y = contact.pos.y();
00559 mk.pose.position.z = contact.pos.z();
00560 mk.pose.orientation.w = 1.0;
00561
00562 mk.scale.x = mk.scale.y = mk.scale.z = 0.01;
00563
00564 mk.color.a = 0.6;
00565 mk.color.r = 1.0;
00566 mk.color.g = 0.04;
00567 mk.color.b = 0.04;
00568
00569
00570
00571 vis_marker_publisher_.publish(mk);
00572 }
00573
00574 }
00575