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
00035
00036
00037 #include <moveit/srv_kinematics_plugin/srv_kinematics_plugin.h>
00038 #include <class_loader/class_loader.h>
00039
00040
00041 #include <urdf_model/model.h>
00042 #include <srdfdom/model.h>
00043
00044 #include <moveit/robot_state/conversions.h>
00045 #include <moveit/rdf_loader/rdf_loader.h>
00046
00047
00048 #include <Eigen/Core>
00049 #include <Eigen/Geometry>
00050
00051
00052 CLASS_LOADER_REGISTER_CLASS(srv_kinematics_plugin::SrvKinematicsPlugin, kinematics::KinematicsBase)
00053
00054 namespace srv_kinematics_plugin
00055 {
00056
00057 SrvKinematicsPlugin::SrvKinematicsPlugin()
00058 : active_(false)
00059 {}
00060
00061 bool SrvKinematicsPlugin::initialize(const std::string &robot_description,
00062 const std::string& group_name,
00063 const std::string& base_frame,
00064 const std::vector<std::string>& tip_frames,
00065 double search_discretization)
00066 {
00067 bool debug = false;
00068
00069 ROS_INFO_STREAM_NAMED("srv","SrvKinematicsPlugin initializing");
00070
00071 setValues(robot_description, group_name, base_frame, tip_frames, search_discretization);
00072
00073 ros::NodeHandle private_handle("~");
00074 rdf_loader::RDFLoader rdf_loader(robot_description_);
00075 const boost::shared_ptr<srdf::Model> &srdf = rdf_loader.getSRDF();
00076 const boost::shared_ptr<urdf::ModelInterface>& urdf_model = rdf_loader.getURDF();
00077
00078 if (!urdf_model || !srdf)
00079 {
00080 ROS_ERROR_NAMED("srv","URDF and SRDF must be loaded for SRV kinematics solver to work.");
00081 return false;
00082 }
00083
00084 robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf));
00085
00086 joint_model_group_ = robot_model_->getJointModelGroup(group_name);
00087 if (!joint_model_group_)
00088 return false;
00089
00090 if (debug)
00091 {
00092 std::cout << std::endl << "Joint Model Variable Names: ------------------------------------------- " << std::endl;
00093 const std::vector<std::string> jm_names = joint_model_group_->getVariableNames();
00094 std::copy(jm_names.begin(), jm_names.end(), std::ostream_iterator<std::string>(std::cout, "\n"));
00095 std::cout << std::endl;
00096 }
00097
00098
00099 dimension_ = joint_model_group_->getVariableCount();
00100 ROS_INFO_STREAM_NAMED("srv","Dimension planning group '" << group_name << "': " << dimension_
00101 << ". Active Joints Models: " << joint_model_group_->getActiveJointModels().size()
00102 << ". Mimic Joint Models: " << joint_model_group_->getMimicJointModels().size());
00103
00104
00105 for (std::size_t i=0; i < joint_model_group_->getJointModels().size(); ++i)
00106 {
00107 ik_group_info_.joint_names.push_back(joint_model_group_->getJointModelNames()[i]);
00108 }
00109
00110 if (debug)
00111 {
00112 ROS_ERROR_STREAM_NAMED("temp","tip links available:");
00113 std::copy(tip_frames_.begin(), tip_frames_.end(), std::ostream_iterator<std::string>(std::cout, "\n"));
00114 }
00115
00116
00117 for (std::size_t i = 0; i < tip_frames_.size(); ++i)
00118 {
00119 if(!joint_model_group_->hasLinkModel(tip_frames_[i]))
00120 {
00121 ROS_ERROR_NAMED("srv","Could not find tip name '%s' in joint group '%s'", tip_frames_[i].c_str(), group_name.c_str());
00122 return false;
00123 }
00124 ik_group_info_.link_names.push_back(tip_frames_[i]);
00125 }
00126
00127
00128 ROS_DEBUG_STREAM_NAMED("srv","Looking for ROS service name on rosparm server at location: " <<
00129 private_handle.getNamespace() << "/" << group_name_ << "/kinematics_solver_service_name");
00130 std::string ik_service_name;
00131 private_handle.param(group_name_ + "/kinematics_solver_service_name", ik_service_name, std::string("solve_ik"));
00132
00133
00134 robot_state_.reset(new robot_state::RobotState(robot_model_));
00135 robot_state_->setToDefaultValues();
00136
00137
00138 ros::NodeHandle nonprivate_handle("");
00139 ik_service_client_ = boost::make_shared<ros::ServiceClient>(nonprivate_handle.serviceClient
00140 <moveit_msgs::GetPositionIK>(ik_service_name));
00141 if (!ik_service_client_->waitForExistence(ros::Duration(0.1)))
00142 ROS_WARN_STREAM_NAMED("srv","Unable to connect to ROS service client with name: " << ik_service_client_->getService());
00143 else
00144 ROS_INFO_STREAM_NAMED("srv","Service client started with ROS service name: " << ik_service_client_->getService());
00145
00146 active_ = true;
00147 ROS_DEBUG_NAMED("srv","ROS service-based kinematics solver initialized");
00148 return true;
00149 }
00150
00151 bool SrvKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int> &redundant_joints)
00152 {
00153 if(num_possible_redundant_joints_ < 0)
00154 {
00155 ROS_ERROR_NAMED("srv","This group cannot have redundant joints");
00156 return false;
00157 }
00158 if(redundant_joints.size() > num_possible_redundant_joints_)
00159 {
00160 ROS_ERROR_NAMED("srv","This group can only have %d redundant joints", num_possible_redundant_joints_);
00161 return false;
00162 }
00163
00164 return true;
00165 }
00166
00167 bool SrvKinematicsPlugin::isRedundantJoint(unsigned int index) const
00168 {
00169 for (std::size_t j=0; j < redundant_joint_indices_.size(); ++j)
00170 if (redundant_joint_indices_[j] == index)
00171 return true;
00172 return false;
00173 }
00174
00175 int SrvKinematicsPlugin::getJointIndex(const std::string &name) const
00176 {
00177 for (unsigned int i=0; i < ik_group_info_.joint_names.size(); i++) {
00178 if (ik_group_info_.joint_names[i] == name)
00179 return i;
00180 }
00181 return -1;
00182 }
00183
00184 bool SrvKinematicsPlugin::timedOut(const ros::WallTime &start_time, double duration) const
00185 {
00186 return ((ros::WallTime::now()-start_time).toSec() >= duration);
00187 }
00188
00189 bool SrvKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
00190 const std::vector<double> &ik_seed_state,
00191 std::vector<double> &solution,
00192 moveit_msgs::MoveItErrorCodes &error_code,
00193 const kinematics::KinematicsQueryOptions &options) const
00194 {
00195 const IKCallbackFn solution_callback = 0;
00196 std::vector<double> consistency_limits;
00197
00198 return searchPositionIK(ik_pose,
00199 ik_seed_state,
00200 default_timeout_,
00201 solution,
00202 solution_callback,
00203 error_code,
00204 consistency_limits,
00205 options);
00206 }
00207
00208 bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00209 const std::vector<double> &ik_seed_state,
00210 double timeout,
00211 std::vector<double> &solution,
00212 moveit_msgs::MoveItErrorCodes &error_code,
00213 const kinematics::KinematicsQueryOptions &options) const
00214 {
00215 const IKCallbackFn solution_callback = 0;
00216 std::vector<double> consistency_limits;
00217
00218 return searchPositionIK(ik_pose,
00219 ik_seed_state,
00220 timeout,
00221 solution,
00222 solution_callback,
00223 error_code,
00224 consistency_limits,
00225 options);
00226 }
00227
00228 bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00229 const std::vector<double> &ik_seed_state,
00230 double timeout,
00231 const std::vector<double> &consistency_limits,
00232 std::vector<double> &solution,
00233 moveit_msgs::MoveItErrorCodes &error_code,
00234 const kinematics::KinematicsQueryOptions &options) const
00235 {
00236 const IKCallbackFn solution_callback = 0;
00237 return searchPositionIK(ik_pose,
00238 ik_seed_state,
00239 timeout,
00240 solution,
00241 solution_callback,
00242 error_code,
00243 consistency_limits,
00244 options);
00245 }
00246
00247 bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00248 const std::vector<double> &ik_seed_state,
00249 double timeout,
00250 std::vector<double> &solution,
00251 const IKCallbackFn &solution_callback,
00252 moveit_msgs::MoveItErrorCodes &error_code,
00253 const kinematics::KinematicsQueryOptions &options) const
00254 {
00255 std::vector<double> consistency_limits;
00256 return searchPositionIK(ik_pose,
00257 ik_seed_state,
00258 timeout,
00259 solution,
00260 solution_callback,
00261 error_code,
00262 consistency_limits,
00263 options);
00264 }
00265
00266 bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00267 const std::vector<double> &ik_seed_state,
00268 double timeout,
00269 const std::vector<double> &consistency_limits,
00270 std::vector<double> &solution,
00271 const IKCallbackFn &solution_callback,
00272 moveit_msgs::MoveItErrorCodes &error_code,
00273 const kinematics::KinematicsQueryOptions &options) const
00274 {
00275 return searchPositionIK(ik_pose,
00276 ik_seed_state,
00277 timeout,
00278 solution,
00279 solution_callback,
00280 error_code,
00281 consistency_limits,
00282 options);
00283 }
00284
00285 bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00286 const std::vector<double> &ik_seed_state,
00287 double timeout,
00288 std::vector<double> &solution,
00289 const IKCallbackFn &solution_callback,
00290 moveit_msgs::MoveItErrorCodes &error_code,
00291 const std::vector<double> &consistency_limits,
00292 const kinematics::KinematicsQueryOptions &options) const
00293 {
00294
00295 std::vector<geometry_msgs::Pose> ik_poses;
00296 ik_poses.push_back(ik_pose);
00297
00298 return searchPositionIK(ik_poses,
00299 ik_seed_state,
00300 timeout,
00301 consistency_limits,
00302 solution,
00303 solution_callback,
00304 error_code,
00305 options);
00306 }
00307
00308 bool SrvKinematicsPlugin::searchPositionIK(const std::vector<geometry_msgs::Pose> &ik_poses,
00309 const std::vector<double> &ik_seed_state,
00310 double timeout,
00311 const std::vector<double> &consistency_limits,
00312 std::vector<double> &solution,
00313 const IKCallbackFn &solution_callback,
00314 moveit_msgs::MoveItErrorCodes &error_code,
00315 const kinematics::KinematicsQueryOptions &options) const
00316 {
00317
00318 if(!active_)
00319 {
00320 ROS_ERROR_NAMED("srv","kinematics not active");
00321 error_code.val = error_code.NO_IK_SOLUTION;
00322 return false;
00323 }
00324
00325
00326 if(ik_seed_state.size() != dimension_)
00327 {
00328 ROS_ERROR_STREAM_NAMED("srv","Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size());
00329 error_code.val = error_code.NO_IK_SOLUTION;
00330 return false;
00331 }
00332
00333
00334 if (tip_frames_.size() != ik_poses.size())
00335 {
00336 ROS_ERROR_STREAM_NAMED("srv","Mismatched number of pose requests (" << ik_poses.size()
00337 << ") to tip frames (" << tip_frames_.size() << ") in searchPositionIK");
00338 error_code.val = error_code.NO_IK_SOLUTION;
00339 return false;
00340 }
00341
00342
00343 moveit_msgs::GetPositionIK ik_srv;
00344 ik_srv.request.ik_request.avoid_collisions = true;
00345 ik_srv.request.ik_request.group_name = getGroupName();
00346
00347
00348 robot_state_->setJointGroupPositions(joint_model_group_, ik_seed_state);
00349 moveit::core::robotStateToRobotStateMsg(*robot_state_, ik_srv.request.ik_request.robot_state);
00350
00351
00352 geometry_msgs::PoseStamped ik_pose_st;
00353 ik_pose_st.header.frame_id = base_frame_;
00354 if (tip_frames_.size() > 1)
00355 {
00356
00357 for (std::size_t i = 0; i < tip_frames_.size(); ++i)
00358 {
00359 ik_pose_st.pose = ik_poses[i];
00360 ik_srv.request.ik_request.pose_stamped_vector.push_back(ik_pose_st);
00361 ik_srv.request.ik_request.ik_link_names.push_back(tip_frames_[i]);
00362 }
00363 }
00364 else
00365 {
00366 ik_pose_st.pose = ik_poses[0];
00367
00368
00369 ik_srv.request.ik_request.pose_stamped = ik_pose_st;
00370 ik_srv.request.ik_request.ik_link_name = getTipFrames()[0];
00371 }
00372
00373 ROS_DEBUG_STREAM_NAMED("srv","Calling service: " << ik_service_client_->getService() );
00374 if (ik_service_client_->call(ik_srv))
00375 {
00376
00377 error_code.val = ik_srv.response.error_code.val;
00378 if(error_code.val != error_code.SUCCESS)
00379 {
00380 ROS_DEBUG_NAMED("srv","An IK that satisifes the constraints and is collision free could not be found.");
00381 switch (error_code.val)
00382 {
00383
00384 ROS_DEBUG_STREAM("Request was: \n" << ik_srv.request.ik_request);
00385 ROS_DEBUG_STREAM("Response was: \n" << ik_srv.response.solution);
00386
00387 case moveit_msgs::MoveItErrorCodes::FAILURE:
00388 ROS_ERROR_STREAM_NAMED("srv","Service failed with with error code: FAILURE");
00389 break;
00390 case moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION:
00391 ROS_ERROR_STREAM_NAMED("srv","Service failed with with error code: NO IK SOLUTION");
00392 break;
00393 default:
00394 ROS_ERROR_STREAM_NAMED("srv","Service failed with with error code: " << error_code.val);
00395 }
00396 return false;
00397 }
00398 }
00399 else
00400 {
00401 ROS_ERROR_STREAM("Service call failed to connect to service: " << ik_service_client_->getService() );
00402 error_code.val = error_code.FAILURE;
00403 return false;
00404 }
00405
00406
00407 if (!moveit::core::robotStateMsgToRobotState(ik_srv.response.solution, *robot_state_))
00408 {
00409 ROS_ERROR_STREAM_NAMED("srv","An error occured converting recieved robot state message into internal robot state.");
00410 error_code.val = error_code.FAILURE;
00411 return false;
00412 }
00413
00414
00415 robot_state_->copyJointGroupPositions(joint_model_group_, solution);
00416
00417
00418 if (!solution_callback.empty())
00419 {
00420 ROS_DEBUG_STREAM_NAMED("srv","Calling solution callback on IK solution");
00421
00422
00423 solution_callback(ik_poses[0], solution, error_code);
00424
00425 if(error_code.val != error_code.SUCCESS)
00426 {
00427 switch (error_code.val)
00428 {
00429 case moveit_msgs::MoveItErrorCodes::FAILURE:
00430 ROS_ERROR_STREAM_NAMED("srv","IK solution callback failed with with error code: FAILURE");
00431 break;
00432 case moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION:
00433 ROS_ERROR_STREAM_NAMED("srv","IK solution callback failed with with error code: NO IK SOLUTION");
00434 break;
00435 default:
00436 ROS_ERROR_STREAM_NAMED("srv","IK solution callback failed with with error code: " << error_code.val);
00437 }
00438 return false;
00439 }
00440 }
00441
00442 ROS_INFO_STREAM_NAMED("srv","IK Solver Succeeded!");
00443 return true;
00444 }
00445
00446 bool SrvKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
00447 const std::vector<double> &joint_angles,
00448 std::vector<geometry_msgs::Pose> &poses) const
00449 {
00450 ros::WallTime n1 = ros::WallTime::now();
00451 if(!active_)
00452 {
00453 ROS_ERROR_NAMED("srv","kinematics not active");
00454 return false;
00455 }
00456 poses.resize(link_names.size());
00457 if(joint_angles.size() != dimension_)
00458 {
00459 ROS_ERROR_NAMED("srv","Joint angles vector must have size: %d",dimension_);
00460 return false;
00461 }
00462
00463 ROS_ERROR_STREAM_NAMED("srv","Forward kinematics not implemented");
00464
00465 return false;
00466 }
00467
00468 const std::vector<std::string>& SrvKinematicsPlugin::getJointNames() const
00469 {
00470 return ik_group_info_.joint_names;
00471 }
00472
00473 const std::vector<std::string>& SrvKinematicsPlugin::getLinkNames() const
00474 {
00475 return ik_group_info_.link_names;
00476 }
00477
00478 const std::vector<std::string>& SrvKinematicsPlugin::getVariableNames() const
00479 {
00480 return joint_model_group_->getVariableNames();
00481 }
00482
00483 }