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 <arm_kinematics_constraint_aware/arm_kinematics_constraint_aware.h>
00035
00036 #include <planning_environment/models/model_utils.h>
00037 #include <sensor_msgs/JointState.h>
00038 #include <kinematics_msgs/utils.h>
00039 #include <visualization_msgs/Marker.h>
00040 #include <visualization_msgs/MarkerArray.h>
00041
00042 namespace arm_kinematics_constraint_aware {
00043
00044 static const std::string IK_WITH_COLLISION_SERVICE = "get_constraint_aware_ik";
00045 static const std::string IK_INFO_SERVICE = "get_ik_solver_info";
00046 static const std::string FK_INFO_SERVICE = "get_fk_solver_info";
00047 static const std::string IK_SERVICE = "get_ik";
00048 static const std::string FK_SERVICE = "get_fk";
00049 static const double IK_DEFAULT_TIMEOUT = 10.0;
00050
00051 ArmKinematicsConstraintAware::ArmKinematicsConstraintAware(): kinematics_loader_("kinematics_base","kinematics::KinematicsBase"),node_handle_("~")
00052 {
00053 std::string group_name, kinematics_solver_name;
00054 node_handle_.param<bool>("visualize_solution",visualize_solution_,true);
00055 node_handle_.param<std::string>("group", group_, std::string());
00056 node_handle_.param<std::string>("kinematics_solver",kinematics_solver_name," ");
00057 ROS_INFO("Using kinematics solver name: %s",kinematics_solver_name.c_str());
00058 if (group_.empty())
00059 {
00060 ROS_ERROR("No 'group' parameter specified. Without the name of the group of joints to monitor, node cannot compute collision aware inverse kinematics");
00061 active_ = false;
00062 return;
00063 }
00064
00065 kinematics_solver_ = NULL;
00066 try
00067 {
00068 kinematics_solver_ = kinematics_loader_.createClassInstance(kinematics_solver_name);
00069 }
00070 catch(pluginlib::PluginlibException& ex)
00071 {
00072 ROS_ERROR("The plugin failed to load. Error1: %s", ex.what());
00073 active_ = false;
00074 return;
00075 }
00076 if(kinematics_solver_->initialize(group_))
00077 active_ = true;
00078 else
00079 {
00080 active_ = false;
00081 return;
00082 }
00083 root_name_ = kinematics_solver_->getBaseFrame();
00084 if(!getChainInfo(group_,chain_info_))
00085 {
00086 ROS_ERROR("Could not construct chain info for group %s",group_.c_str());
00087 return;
00088 }
00089 collision_models_interface_ = new planning_environment::CollisionModelsInterface("robot_description");
00090 if(group_.empty()) {
00091 ROS_WARN("Must specify planning group in configuration");
00092 }
00093 const planning_models::KinematicModel::JointModelGroup* joint_model_group = collision_models_interface_->getKinematicModel()->getModelGroup(group_);
00094 if(joint_model_group == NULL) {
00095 ROS_WARN_STREAM("No joint group " << group_);
00096 }
00097 arm_links_ = joint_model_group->getGroupLinkNames();
00098
00099 const planning_models::KinematicModel::LinkModel* end_effector_link = collision_models_interface_->getKinematicModel()->getLinkModel(chain_info_.link_names.back());
00100 end_effector_collision_links_ = collision_models_interface_->getKinematicModel()->getChildLinkModelNames(end_effector_link);
00101
00102 advertiseBaseKinematicsServices();
00103 advertiseConstraintIKService();
00104 }
00105
00106 void ArmKinematicsConstraintAware::advertiseBaseKinematicsServices()
00107 {
00108 ik_service_ = node_handle_.advertiseService(IK_SERVICE,&ArmKinematicsConstraintAware::getPositionIK,this);
00109 fk_service_ = node_handle_.advertiseService(FK_SERVICE,&ArmKinematicsConstraintAware::getPositionFK,this);
00110 ik_solver_info_service_ = node_handle_.advertiseService(IK_INFO_SERVICE,&ArmKinematicsConstraintAware::getIKSolverInfo,this);
00111 fk_solver_info_service_ = node_handle_.advertiseService(FK_INFO_SERVICE,&ArmKinematicsConstraintAware::getFKSolverInfo,this);
00112 }
00113
00114 void ArmKinematicsConstraintAware::advertiseConstraintIKService()
00115 {
00116 ik_collision_service_ = node_handle_.advertiseService(IK_WITH_COLLISION_SERVICE,&ArmKinematicsConstraintAware::getConstraintAwarePositionIK,this);
00117 display_trajectory_publisher_ = root_handle_.advertise<arm_navigation_msgs::DisplayTrajectory>("ik_solution_display", 1);
00118 }
00119
00120 bool ArmKinematicsConstraintAware::isReady(arm_navigation_msgs::ArmNavigationErrorCodes &error_code)
00121 {
00122 if(!active_)
00123 {
00124 ROS_ERROR("IK service is not ready");
00125 return false;
00126 }
00127 if(!collision_models_interface_->isPlanningSceneSet()) {
00128 ROS_WARN("Planning scene not set");
00129 error_code.val = error_code.COLLISION_CHECKING_UNAVAILABLE;
00130 return false;
00131 }
00132 error_code.val = error_code.SUCCESS;
00133 return true;
00134 }
00135
00136 bool ArmKinematicsConstraintAware::getConstraintAwarePositionIK(kinematics_msgs::GetConstraintAwarePositionIK::Request &request_in,
00137 kinematics_msgs::GetConstraintAwarePositionIK::Response &response)
00138 {
00139 if(!isReady(response.error_code))
00140 return true;
00141
00142 if(!checkConstraintAwareIKService(request_in,response,chain_info_))
00143 {
00144 ROS_ERROR("IK service request is malformed");
00145 return true;
00146 }
00147
00148 collision_models_interface_->disableCollisionsForNonUpdatedLinks(group_);
00149
00150 ros::Time start_time = ros::Time::now();
00151 ROS_DEBUG("Received IK request is in the frame: %s",request_in.ik_request.pose_stamped.header.frame_id.c_str());
00152
00153 ik_request_ = request_in.ik_request;
00154 constraints_ = request_in.constraints;
00155
00156 geometry_msgs::PoseStamped pose_msg_in = ik_request_.pose_stamped;
00157 geometry_msgs::PoseStamped pose_msg_out;
00158 planning_environment::setRobotStateAndComputeTransforms(request_in.ik_request.robot_state, *collision_models_interface_->getPlanningSceneState());
00159
00160 if(!collision_models_interface_->convertPoseGivenWorldTransform(*collision_models_interface_->getPlanningSceneState(),
00161 root_name_,
00162 pose_msg_in.header,
00163 pose_msg_in.pose,
00164 pose_msg_out)) {
00165 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00166 return true;
00167 }
00168 ik_request_.pose_stamped = pose_msg_out;
00169 ROS_DEBUG_STREAM("Pose is " << pose_msg_out.pose.position.x << " " << pose_msg_out.pose.position.y << " " << pose_msg_out.pose.position.z);
00170 ROS_DEBUG("Transformed IK request is in the frame: %s",ik_request_.pose_stamped.header.frame_id.c_str());
00171 arm_kinematics_constraint_aware::reorderJointState(ik_request_.ik_seed_state.joint_state,chain_info_);
00172
00173 ros::Time ik_solver_time = ros::Time::now();
00174 int kinematics_error_code;
00175 bool ik_valid = kinematics_solver_->searchPositionIK(ik_request_.pose_stamped.pose,
00176 ik_request_.ik_seed_state.joint_state.position,
00177 request_in.timeout.toSec(),
00178 response.solution.joint_state.position,
00179 boost::bind(&ArmKinematicsConstraintAware::initialPoseCheck, this, _1, _2, _3),
00180 boost::bind(&ArmKinematicsConstraintAware::collisionCheck, this, _1, _2, _3),kinematics_error_code);
00181 ROS_DEBUG("IK solver time: %f",(ros::Time::now()-ik_solver_time).toSec());
00182
00183 if(ik_valid)
00184 {
00185 response.solution.joint_state.name = chain_info_.joint_names;
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197 kinematics_msgs::GetPositionFK::Request req;
00198 kinematics_msgs::GetPositionFK::Response res;
00199 req.header = pose_msg_out.header;
00200 req.robot_state.joint_state = response.solution.joint_state;
00201 req.fk_link_names.push_back(request_in.ik_request.ik_link_name);
00202 getPositionFK(req,res);
00203 ROS_DEBUG_STREAM("Fk says " << res.pose_stamped[0].pose.position.x << " " << res.pose_stamped[0].pose.position.y << " " << res.pose_stamped[0].pose.position.z);
00204
00205 response.error_code.val = response.error_code.SUCCESS;
00206 return true;
00207 }
00208 else
00209 {
00210 ROS_DEBUG("A collision aware ik solution could not be found");
00211 response.error_code = kinematicsErrorCodeToMotionPlanningErrorCode(kinematics_error_code);
00212 if(response.error_code.val != response.error_code.IK_LINK_IN_COLLISION)
00213 {
00214 sendEndEffectorPose(collision_models_interface_->getPlanningSceneState(),true);
00215 }
00216 return true;
00217 }
00218 }
00219
00220 void ArmKinematicsConstraintAware::collisionCheck(const geometry_msgs::Pose &ik_pose,
00221 const std::vector<double> &ik_solution,
00222 int &error_code)
00223 {
00224 std::map<std::string, double> joint_values;
00225 for(unsigned int i=0; i < chain_info_.joint_names.size(); i++)
00226 joint_values[chain_info_.joint_names[i]] = ik_solution[i];
00227
00228 collision_models_interface_->getPlanningSceneState()->setKinematicState(joint_values);
00229 if(collision_models_interface_->getPlanningSceneState() == NULL) {
00230 ROS_INFO_STREAM("Messed up");
00231 }
00232 if(collision_models_interface_->isKinematicStateInCollision(*(collision_models_interface_->getPlanningSceneState()))) {
00233
00234 error_code = kinematics::STATE_IN_COLLISION;
00235 } else {
00236 error_code = kinematics::SUCCESS;
00237 }
00238
00239 if(!planning_environment::doesKinematicStateObeyConstraints(*(collision_models_interface_->getPlanningSceneState()),
00240 constraints_)) {
00241 error_code = kinematics::GOAL_CONSTRAINTS_VIOLATED;
00242 }
00243 }
00244
00245 void ArmKinematicsConstraintAware::initialPoseCheck(const geometry_msgs::Pose &ik_pose,
00246 const std::vector<double> &ik_solution,
00247 int &error_code)
00248 {
00249 std::string kinematic_frame_id = kinematics_solver_->getBaseFrame();
00250 std::string planning_frame_id = collision_models_interface_->getWorldFrameId();
00251 geometry_msgs::PoseStamped pose_stamped;
00252 pose_stamped.pose = ik_pose;
00253 pose_stamped.header.stamp = ros::Time::now();
00254 pose_stamped.header.frame_id = kinematic_frame_id;
00255 if(!collision_models_interface_->convertPoseGivenWorldTransform(*collision_models_interface_->getPlanningSceneState(),
00256 planning_frame_id,
00257 pose_stamped.header,
00258 pose_stamped.pose,
00259 pose_stamped)) {
00260 ROS_ERROR_STREAM("Cannot transform from " << pose_stamped.header.frame_id << " to " << planning_frame_id);
00261 }
00262
00263 collision_space::EnvironmentModel::AllowedCollisionMatrix save_acm = collision_models_interface_->getCurrentAllowedCollisionMatrix();
00264 collision_space::EnvironmentModel::AllowedCollisionMatrix acm = save_acm;
00265 for(unsigned int i = 0; i < arm_links_.size(); i++) {
00266 acm.changeEntry(arm_links_[i], true);
00267 }
00268 collision_models_interface_->setAlteredAllowedCollisionMatrix(acm);
00269
00270 btTransform transform;
00271 tf::poseMsgToTF(pose_stamped.pose,transform);
00272 if(!collision_models_interface_->getPlanningSceneState()->hasLinkState(ik_request_.ik_link_name)) {
00273 ROS_ERROR("Could not find end effector root_link %s", ik_request_.ik_link_name.c_str());
00274 error_code = kinematics::INVALID_LINK_NAME;
00275 return;
00276 }
00277 collision_models_interface_->getPlanningSceneState()->updateKinematicStateWithLinkAt(ik_request_.ik_link_name, transform);
00278 if(collision_models_interface_->isKinematicStateInCollision(*(collision_models_interface_->getPlanningSceneState()))) {
00279 error_code = kinematics::IK_LINK_IN_COLLISION;
00280 ROS_DEBUG_STREAM("Initial pose check failing");
00281 sendEndEffectorPose(collision_models_interface_->getPlanningSceneState(), false);
00282 }
00283 else
00284 error_code = kinematics::SUCCESS;
00285
00286 collision_models_interface_->setAlteredAllowedCollisionMatrix(save_acm);
00287 }
00288
00289 void ArmKinematicsConstraintAware::sendEndEffectorPose(const planning_models::KinematicState* state, bool valid) {
00290 boost::shared_ptr<urdf::Model> robot_model = collision_models_interface_->getParsedDescription();
00291 visualization_msgs::MarkerArray hand_array;
00292 unsigned int id = 0;
00293 for(unsigned int i = 0; i < end_effector_collision_links_.size(); i++) {
00294 boost::shared_ptr<const urdf::Link> urdf_link = robot_model->getLink(end_effector_collision_links_[i]);
00295 if(urdf_link == NULL) {
00296 ROS_DEBUG_STREAM("No entry in urdf for link " << end_effector_collision_links_[i]);
00297 continue;
00298 }
00299 if(!urdf_link->collision) {
00300 continue;
00301 }
00302 const urdf::Geometry *geom = urdf_link->collision->geometry.get();
00303 if(!geom) {
00304 ROS_DEBUG_STREAM("No collision geometry for link " << end_effector_collision_links_[i]);
00305 continue;
00306 }
00307 const urdf::Mesh *mesh = dynamic_cast<const urdf::Mesh*>(geom);
00308 if(mesh) {
00309 if (!mesh->filename.empty()) {
00310 planning_models::KinematicState::LinkState* ls = state->getLinkState(end_effector_collision_links_[i]);
00311 visualization_msgs::Marker mark;
00312 mark.header.frame_id = collision_models_interface_->getWorldFrameId();
00313 mark.header.stamp = ros::Time::now();
00314 mark.id = id++;
00315 if(!valid) {
00316 mark.ns = "initial_pose_collision";
00317 } else {
00318 mark.ns = "initial_pose_ok";
00319 }
00320 mark.type = mark.MESH_RESOURCE;
00321 mark.scale.x = 1.0;
00322 mark.scale.y = 1.0;
00323 mark.scale.z = 1.0;
00324 if(!valid) {
00325 mark.color.r = 1.0;
00326 } else {
00327 mark.color.g = 1.0;
00328 }
00329 mark.color.a = .8;
00330 mark.pose.position.x = ls->getGlobalCollisionBodyTransform().getOrigin().x();
00331 mark.pose.position.y = ls->getGlobalCollisionBodyTransform().getOrigin().y();
00332 mark.pose.position.z = ls->getGlobalCollisionBodyTransform().getOrigin().z();
00333 mark.pose.orientation.x = ls->getGlobalCollisionBodyTransform().getRotation().x();
00334 mark.pose.orientation.y = ls->getGlobalCollisionBodyTransform().getRotation().y();
00335 mark.pose.orientation.z = ls->getGlobalCollisionBodyTransform().getRotation().z();
00336 mark.pose.orientation.w = ls->getGlobalCollisionBodyTransform().getRotation().w();
00337 mark.mesh_resource = mesh->filename;
00338 hand_array.markers.push_back(mark);
00339 }
00340 }
00341 }
00342
00343 }
00344
00345 void ArmKinematicsConstraintAware::printStringVec(const std::string &prefix, const std::vector<std::string> &string_vector)
00346 {
00347 ROS_DEBUG("%s",prefix.c_str());
00348 for(unsigned int i=0; i < string_vector.size(); i++)
00349 {
00350 ROS_DEBUG("%s",string_vector[i].c_str());
00351 }
00352 }
00353
00354 bool ArmKinematicsConstraintAware::getPositionIK(kinematics_msgs::GetPositionIK::Request &request,
00355 kinematics_msgs::GetPositionIK::Response &response)
00356 {
00357 bool scene_set = collision_models_interface_->isPlanningSceneSet();
00358 if(!isReady(response.error_code)) {
00359 if(request.ik_request.pose_stamped.header.frame_id != root_name_) {
00360 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00361 return true;
00362 }
00363 }
00364
00365 if(!checkIKService(request,response,chain_info_))
00366 return true;
00367
00368 geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped;
00369 geometry_msgs::PoseStamped pose_msg_out;
00370 if(scene_set) {
00371 planning_environment::setRobotStateAndComputeTransforms(request.ik_request.robot_state, *collision_models_interface_->getPlanningSceneState());
00372
00373 if(!collision_models_interface_->convertPoseGivenWorldTransform(*collision_models_interface_->getPlanningSceneState(),
00374 root_name_,
00375 pose_msg_in.header,
00376 pose_msg_in.pose,
00377 pose_msg_out)) {
00378 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00379 return true;
00380 }
00381 } else {
00382 pose_msg_out = pose_msg_in;
00383 }
00384 request.ik_request.pose_stamped = pose_msg_out;
00385 ROS_DEBUG_STREAM("Pose is " << pose_msg_out.pose.position.x << " " << pose_msg_out.pose.position.y << " " << pose_msg_out.pose.position.z);
00386
00387 arm_kinematics_constraint_aware::reorderJointState(request.ik_request.ik_seed_state.joint_state,chain_info_);
00388
00389 int kinematics_error_code;
00390 bool ik_valid = kinematics_solver_->searchPositionIK(pose_msg_out.pose,
00391 request.ik_request.ik_seed_state.joint_state.position,
00392 request.timeout.toSec(),
00393 response.solution.joint_state.position,
00394 kinematics_error_code);
00395
00396 response.error_code = kinematicsErrorCodeToMotionPlanningErrorCode(kinematics_error_code);
00397
00398 if(ik_valid)
00399 {
00400 response.solution.joint_state.name = chain_info_.joint_names;
00401 response.error_code.val = response.error_code.SUCCESS;
00402 kinematics_msgs::GetPositionFK::Request req;
00403 kinematics_msgs::GetPositionFK::Response res;
00404 req.header = request.ik_request.pose_stamped.header;
00405 req.robot_state.joint_state = response.solution.joint_state;
00406 req.fk_link_names.push_back(request.ik_request.ik_link_name);
00407 getPositionFK(req,res);
00408 ROS_DEBUG_STREAM("Fk says " << res.pose_stamped[0].pose.position.x << " " << res.pose_stamped[0].pose.position.y << " " << res.pose_stamped[0].pose.position.z);
00409 return true;
00410 }
00411 else
00412 {
00413 ROS_DEBUG("An IK solution could not be found");
00414 return true;
00415 }
00416 }
00417
00418 bool ArmKinematicsConstraintAware::getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
00419 kinematics_msgs::GetKinematicSolverInfo::Response &response)
00420 {
00421 if(!active_)
00422 {
00423 ROS_ERROR("IK node not active");
00424 return true;
00425 }
00426 response.kinematic_solver_info = chain_info_;
00427 return true;
00428 }
00429
00430 bool ArmKinematicsConstraintAware::getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
00431 kinematics_msgs::GetKinematicSolverInfo::Response &response)
00432 {
00433 if(!active_)
00434 {
00435 ROS_ERROR("IK node not active");
00436 return true;
00437 }
00438 response.kinematic_solver_info = chain_info_;
00439 return true;
00440 }
00441
00442 bool ArmKinematicsConstraintAware::getPositionFK(kinematics_msgs::GetPositionFK::Request &request,
00443 kinematics_msgs::GetPositionFK::Response &response)
00444 {
00445 if(!active_)
00446 {
00447 ROS_ERROR("FK service not active");
00448 return true;
00449 }
00450
00451 if(!checkFKService(request,response,chain_info_))
00452 return true;
00453
00454 arm_kinematics_constraint_aware::reorderJointState(request.robot_state.joint_state,chain_info_);
00455
00456 response.pose_stamped.resize(request.fk_link_names.size());
00457 response.fk_link_names.resize(request.fk_link_names.size());
00458
00459 bool valid = true;
00460 std::vector<geometry_msgs::Pose> solutions;
00461 solutions.resize(request.fk_link_names.size());
00462 if(kinematics_solver_->getPositionFK(request.fk_link_names,request.robot_state.joint_state.position,solutions) >=0)
00463 {
00464 for(unsigned int i=0; i < solutions.size(); i++)
00465 {
00466 geometry_msgs::PoseStamped pose_stamped;
00467 pose_stamped.pose = solutions[i];
00468 pose_stamped.header.frame_id = root_name_;
00469 pose_stamped.header.stamp = ros::Time();
00470
00471 if(!collision_models_interface_->isPlanningSceneSet()) {
00472 if(request.header.frame_id != root_name_) {
00473 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00474 return true;
00475 }
00476 } else if(!collision_models_interface_->convertPoseGivenWorldTransform(*collision_models_interface_->getPlanningSceneState(),
00477 request.header.frame_id,
00478 pose_stamped.header,
00479 solutions[i],
00480 pose_stamped)) {
00481 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00482 return true;
00483 }
00484 response.pose_stamped[i] = pose_stamped;
00485 response.fk_link_names[i] = request.fk_link_names[i];
00486 response.error_code.val = response.error_code.SUCCESS;
00487 }
00488 }
00489 else
00490 {
00491 ROS_ERROR("Could not compute FK");
00492 response.error_code.val = response.error_code.NO_FK_SOLUTION;
00493 valid = false;
00494 }
00495 return valid;
00496 }
00497
00498 }
00499
