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 collision_models_interface_ = new planning_environment::CollisionModelsInterface("robot_description");
00066
00067 kinematics::KinematicsBase* kinematics_solver = NULL;
00068 try
00069 {
00070 kinematics_solver = kinematics_loader_.createClassInstance(kinematics_solver_name);
00071 }
00072 catch(pluginlib::PluginlibException& ex)
00073 {
00074 ROS_ERROR("The plugin failed to load. Error1: %s", ex.what());
00075 return;
00076 }
00077
00078 solver_ = new ArmKinematicsSolverConstraintAware(kinematics_solver,
00079 collision_models_interface_,
00080 group_);
00081
00082 active_ = solver_->isActive();
00083
00084 if(active_) {
00085 getChainInfoFromRobotModel(*collision_models_interface_->getParsedDescription(),
00086 solver_->getBaseName(),
00087 solver_->getTipName(),
00088 chain_info_);
00089 }
00090
00091 advertiseBaseKinematicsServices();
00092 advertiseConstraintIKService();
00093 }
00094
00095 void ArmKinematicsConstraintAware::advertiseBaseKinematicsServices()
00096 {
00097 ik_service_ = node_handle_.advertiseService(IK_SERVICE,&ArmKinematicsConstraintAware::getPositionIK,this);
00098 fk_service_ = node_handle_.advertiseService(FK_SERVICE,&ArmKinematicsConstraintAware::getPositionFK,this);
00099 ik_solver_info_service_ = node_handle_.advertiseService(IK_INFO_SERVICE,&ArmKinematicsConstraintAware::getIKSolverInfo,this);
00100 fk_solver_info_service_ = node_handle_.advertiseService(FK_INFO_SERVICE,&ArmKinematicsConstraintAware::getFKSolverInfo,this);
00101 }
00102
00103 void ArmKinematicsConstraintAware::advertiseConstraintIKService()
00104 {
00105 ik_collision_service_ = node_handle_.advertiseService(IK_WITH_COLLISION_SERVICE,&ArmKinematicsConstraintAware::getConstraintAwarePositionIK,this);
00106 display_trajectory_publisher_ = root_handle_.advertise<arm_navigation_msgs::DisplayTrajectory>("ik_solution_display", 1);
00107 }
00108
00109 bool ArmKinematicsConstraintAware::isReady(arm_navigation_msgs::ArmNavigationErrorCodes &error_code)
00110 {
00111 if(!active_)
00112 {
00113 ROS_ERROR("IK service is not ready");
00114 return false;
00115 }
00116 if(!collision_models_interface_->isPlanningSceneSet()) {
00117 ROS_WARN("Planning scene not set");
00118 error_code.val = error_code.COLLISION_CHECKING_UNAVAILABLE;
00119 return false;
00120 }
00121 error_code.val = error_code.SUCCESS;
00122 return true;
00123 }
00124
00125 bool ArmKinematicsConstraintAware::getConstraintAwarePositionIK(kinematics_msgs::GetConstraintAwarePositionIK::Request &request_in,
00126 kinematics_msgs::GetConstraintAwarePositionIK::Response &response)
00127 {
00128 if(!isReady(response.error_code))
00129 return true;
00130
00131 if(!checkConstraintAwareIKService(request_in,response,chain_info_))
00132 {
00133 ROS_ERROR("IK service request is malformed");
00134 return true;
00135 }
00136
00137 collision_models_interface_->disableCollisionsForNonUpdatedLinks(group_);
00138
00139
00140 ros::Time start_time = ros::Time::now();
00141 ROS_DEBUG("Received IK request is in the frame: %s",request_in.ik_request.pose_stamped.header.frame_id.c_str());
00142
00143 geometry_msgs::PoseStamped pose_msg_in = request_in.ik_request.pose_stamped;
00144 ROS_DEBUG_STREAM("Before Pose is " << pose_msg_in.pose.position.x << " " << pose_msg_in.pose.position.y << " " << pose_msg_in.pose.position.z);
00145 geometry_msgs::PoseStamped pose_msg_out;
00146 planning_environment::setRobotStateAndComputeTransforms(request_in.ik_request.robot_state, *collision_models_interface_->getPlanningSceneState());
00147
00148 if(!collision_models_interface_->convertPoseGivenWorldTransform(*collision_models_interface_->getPlanningSceneState(),
00149 solver_->getBaseName(),
00150 pose_msg_in.header,
00151 pose_msg_in.pose,
00152 pose_msg_out)) {
00153 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00154 return true;
00155 }
00156 ROS_DEBUG_STREAM("Pose is " << pose_msg_out.pose.position.x << " " << pose_msg_out.pose.position.y << " " << pose_msg_out.pose.position.z);
00157
00158 ros::Time ik_solver_time = ros::Time::now();
00159 bool ik_valid = solver_->findConstraintAwareSolution(pose_msg_out.pose,
00160 request_in.constraints,
00161 collision_models_interface_->getPlanningSceneState(),
00162 response.solution.joint_state,
00163 response.error_code,
00164 true);
00165 if(ik_valid)
00166 {
00167 response.error_code.val = response.error_code.SUCCESS;
00168 return true;
00169 }
00170 else
00171 {
00172 ROS_DEBUG("A collision aware ik solution could not be found");
00173 if(response.error_code.val != response.error_code.IK_LINK_IN_COLLISION)
00174 {
00175
00176 }
00177 return true;
00178 }
00179 }
00180
00181 void ArmKinematicsConstraintAware::sendEndEffectorPose(const planning_models::KinematicState* state, bool valid) {
00182 boost::shared_ptr<urdf::Model> robot_model = collision_models_interface_->getParsedDescription();
00183 visualization_msgs::MarkerArray hand_array;
00184 unsigned int id = 0;
00185 for(unsigned int i = 0; i < solver_->getLinkNames().size(); i++) {
00186 boost::shared_ptr<const urdf::Link> urdf_link = robot_model->getLink(solver_->getLinkNames()[i]);
00187 if(urdf_link == NULL) {
00188 ROS_DEBUG_STREAM("No entry in urdf for link " << solver_->getLinkNames()[i]);
00189 continue;
00190 }
00191 if(!urdf_link->collision) {
00192 continue;
00193 }
00194 const urdf::Geometry *geom = urdf_link->collision->geometry.get();
00195 if(!geom) {
00196 ROS_DEBUG_STREAM("No collision geometry for link " << solver_->getLinkNames()[i]);
00197 continue;
00198 }
00199 const urdf::Mesh *mesh = dynamic_cast<const urdf::Mesh*>(geom);
00200 if(mesh) {
00201 if (!mesh->filename.empty()) {
00202 planning_models::KinematicState::LinkState* ls = state->getLinkState(solver_->getLinkNames()[i]);
00203 visualization_msgs::Marker mark;
00204 mark.header.frame_id = collision_models_interface_->getWorldFrameId();
00205 mark.header.stamp = ros::Time::now();
00206 mark.id = id++;
00207 if(!valid) {
00208 mark.ns = "initial_pose_collision";
00209 } else {
00210 mark.ns = "initial_pose_ok";
00211 }
00212 mark.type = mark.MESH_RESOURCE;
00213 mark.scale.x = 1.0;
00214 mark.scale.y = 1.0;
00215 mark.scale.z = 1.0;
00216 if(!valid) {
00217 mark.color.r = 1.0;
00218 } else {
00219 mark.color.g = 1.0;
00220 }
00221 mark.color.a = .8;
00222 mark.pose.position.x = ls->getGlobalCollisionBodyTransform().getOrigin().x();
00223 mark.pose.position.y = ls->getGlobalCollisionBodyTransform().getOrigin().y();
00224 mark.pose.position.z = ls->getGlobalCollisionBodyTransform().getOrigin().z();
00225 mark.pose.orientation.x = ls->getGlobalCollisionBodyTransform().getRotation().x();
00226 mark.pose.orientation.y = ls->getGlobalCollisionBodyTransform().getRotation().y();
00227 mark.pose.orientation.z = ls->getGlobalCollisionBodyTransform().getRotation().z();
00228 mark.pose.orientation.w = ls->getGlobalCollisionBodyTransform().getRotation().w();
00229 mark.mesh_resource = mesh->filename;
00230 hand_array.markers.push_back(mark);
00231 }
00232 }
00233 }
00234 vis_marker_array_publisher_.publish(hand_array);
00235 }
00236
00237 bool ArmKinematicsConstraintAware::getPositionIK(kinematics_msgs::GetPositionIK::Request &request,
00238 kinematics_msgs::GetPositionIK::Response &response)
00239 {
00240 if(!isReady(response.error_code)) {
00241 if(request.ik_request.pose_stamped.header.frame_id != solver_->getBaseName()) {
00242 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00243 return true;
00244 }
00245 }
00246
00247 if(!checkIKService(request,response,chain_info_))
00248 return true;
00249
00250 planning_models::KinematicState* state;
00251 if(collision_models_interface_->isPlanningSceneSet()) {
00252 state = new planning_models::KinematicState(*collision_models_interface_->getPlanningSceneState());
00253 } else {
00254 state = new planning_models::KinematicState(collision_models_interface_->getKinematicModel());
00255 state->setKinematicStateToDefault();
00256 }
00257
00258 planning_environment::setRobotStateAndComputeTransforms(request.ik_request.robot_state,
00259 *state);
00260
00261 geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped;
00262 geometry_msgs::PoseStamped pose_msg_out;
00263
00264 if(!collision_models_interface_->convertPoseGivenWorldTransform(*state,
00265 solver_->getBaseName(),
00266 pose_msg_in.header,
00267 pose_msg_in.pose,
00268 pose_msg_out)) {
00269 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00270 delete state;
00271 return true;
00272 }
00273 ROS_DEBUG_STREAM("Pose is " << pose_msg_out.pose.position.x << " " << pose_msg_out.pose.position.y << " " << pose_msg_out.pose.position.z);
00274
00275
00276 bool ik_valid = solver_->getPositionIK(pose_msg_out.pose,
00277 state,
00278 response.solution.joint_state,
00279 response.error_code);
00280
00281 if(ik_valid)
00282 {
00283 response.error_code.val = response.error_code.SUCCESS;
00284 }
00285 delete state;
00286 return ik_valid;
00287 }
00288
00289 bool ArmKinematicsConstraintAware::getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
00290 kinematics_msgs::GetKinematicSolverInfo::Response &response)
00291 {
00292 if(!active_)
00293 {
00294 ROS_ERROR("IK node not active");
00295 return true;
00296 }
00297 response.kinematic_solver_info = chain_info_;
00298 return true;
00299 }
00300
00301 bool ArmKinematicsConstraintAware::getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
00302 kinematics_msgs::GetKinematicSolverInfo::Response &response)
00303 {
00304 if(!active_)
00305 {
00306 ROS_ERROR("IK node not active");
00307 return true;
00308 }
00309 response.kinematic_solver_info = chain_info_;
00310 return true;
00311 }
00312
00313 bool ArmKinematicsConstraintAware::getPositionFK(kinematics_msgs::GetPositionFK::Request &request,
00314 kinematics_msgs::GetPositionFK::Response &response)
00315 {
00316 if(!active_)
00317 {
00318 ROS_ERROR("FK service not active");
00319 return true;
00320 }
00321
00322 if(!checkFKService(request,response,chain_info_))
00323 return true;
00324
00325 planning_models::KinematicState* state;
00326 if(collision_models_interface_->isPlanningSceneSet()) {
00327 state = new planning_models::KinematicState(*collision_models_interface_->getPlanningSceneState());
00328 } else {
00329 state = new planning_models::KinematicState(collision_models_interface_->getKinematicModel());
00330 state->setKinematicStateToDefault();
00331 }
00332
00333 planning_environment::setRobotStateAndComputeTransforms(request.robot_state,
00334 *state);
00335
00336 std::vector<geometry_msgs::Pose> poses;
00337 bool valid = solver_->getPositionFK(state,
00338 request.fk_link_names,
00339 poses);
00340 if(valid) {
00341 response.pose_stamped.resize(poses.size());
00342 response.fk_link_names.resize(poses.size());
00343 for(unsigned int i=0; i < poses.size(); i++) {
00344 std_msgs::Header world_header;
00345 world_header.frame_id = collision_models_interface_->getWorldFrameId();
00346
00347 if(!collision_models_interface_->convertPoseGivenWorldTransform(*state,
00348 request.header.frame_id,
00349 world_header,
00350 poses[i],
00351 response.pose_stamped[i])) {
00352 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00353 delete state;
00354 return true;
00355 }
00356 response.fk_link_names[i] = request.fk_link_names[i];
00357 response.error_code.val = response.error_code.SUCCESS;
00358 }
00359 }
00360 else
00361 {
00362 ROS_ERROR("Could not compute FK");
00363 response.error_code.val = response.error_code.NO_FK_SOLUTION;
00364 valid = false;
00365 }
00366 delete state;
00367 return valid;
00368 }
00369
00370 }
00371