41 #include <urdf_model/model.h> 49 #include <Eigen/Geometry> 56 SrvKinematicsPlugin::SrvKinematicsPlugin() : active_(false)
61 const std::string& base_frame,
const std::vector<std::string>& tip_frames,
62 double search_discretization)
68 setValues(robot_description, group_name, base_frame, tip_frames, search_discretization);
72 const urdf::ModelInterfaceSharedPtr& urdf_model = rdf_loader.
getURDF();
74 if (!urdf_model || !srdf)
76 ROS_ERROR_NAMED(
"srv",
"URDF and SRDF must be loaded for SRV kinematics solver to work.");
80 robot_model_.reset(
new robot_model::RobotModel(urdf_model, srdf));
88 std::cout << std::endl <<
"Joint Model Variable Names: ------------------------------------------- " << std::endl;
90 std::copy(jm_names.begin(), jm_names.end(), std::ostream_iterator<std::string>(std::cout,
"\n"));
91 std::cout << std::endl;
114 for (std::size_t i = 0; i <
tip_frames_.size(); ++i)
127 <<
"/kinematics_solver_service_name");
128 std::string ik_service_name;
129 lookupParam(
"kinematics_solver_service_name", ik_service_name, std::string(
"solve_ik"));
138 nonprivate_handle.
serviceClient<moveit_msgs::GetPositionIK>(ik_service_name));
141 "Unable to connect to ROS service client with name: " <<
ik_service_client_->getService());
146 ROS_DEBUG_NAMED(
"srv",
"ROS service-based kinematics solver initialized");
176 for (
unsigned int i = 0; i <
ik_group_info_.joint_names.size(); i++)
190 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
194 std::vector<double> consistency_limits;
197 consistency_limits, options);
201 double timeout, std::vector<double>& solution,
202 moveit_msgs::MoveItErrorCodes& error_code,
206 std::vector<double> consistency_limits;
208 return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
213 double timeout,
const std::vector<double>& consistency_limits,
214 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
218 return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
223 double timeout, std::vector<double>& solution,
225 moveit_msgs::MoveItErrorCodes& error_code,
228 std::vector<double> consistency_limits;
229 return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
234 double timeout,
const std::vector<double>& consistency_limits,
235 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
236 moveit_msgs::MoveItErrorCodes& error_code,
239 return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
244 double timeout, std::vector<double>& solution,
246 moveit_msgs::MoveItErrorCodes& error_code,
247 const std::vector<double>& consistency_limits,
251 std::vector<geometry_msgs::Pose> ik_poses;
252 ik_poses.push_back(ik_pose);
254 return searchPositionIK(ik_poses, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
259 const std::vector<double>& ik_seed_state,
double timeout,
260 const std::vector<double>& consistency_limits, std::vector<double>& solution,
262 moveit_msgs::MoveItErrorCodes& error_code,
269 error_code.val = error_code.NO_IK_SOLUTION;
277 << ik_seed_state.size());
278 error_code.val = error_code.NO_IK_SOLUTION;
285 ROS_ERROR_STREAM_NAMED(
"srv",
"Mismatched number of pose requests (" << ik_poses.size() <<
") to tip frames (" 287 <<
") in searchPositionIK");
288 error_code.val = error_code.NO_IK_SOLUTION;
293 moveit_msgs::GetPositionIK ik_srv;
294 ik_srv.request.ik_request.avoid_collisions =
true;
302 geometry_msgs::PoseStamped ik_pose_st;
307 for (std::size_t i = 0; i <
tip_frames_.size(); ++i)
309 ik_pose_st.pose = ik_poses[i];
310 ik_srv.request.ik_request.pose_stamped_vector.push_back(ik_pose_st);
311 ik_srv.request.ik_request.ik_link_names.push_back(
tip_frames_[i]);
316 ik_pose_st.pose = ik_poses[0];
319 ik_srv.request.ik_request.pose_stamped = ik_pose_st;
320 ik_srv.request.ik_request.ik_link_name =
getTipFrames()[0];
327 error_code.val = ik_srv.response.error_code.val;
328 if (error_code.val != error_code.SUCCESS)
330 ROS_DEBUG_STREAM_NAMED(
"srv",
"An IK that satisifes the constraints and is collision free could not be found." 331 <<
"\nRequest was: \n" 332 << ik_srv.request.ik_request <<
"\nResponse was: \n" 333 << ik_srv.response.solution);
334 switch (error_code.val)
336 case moveit_msgs::MoveItErrorCodes::FAILURE:
339 case moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION:
351 error_code.val = error_code.FAILURE;
359 "An error occured converting received robot state message into internal robot state.");
360 error_code.val = error_code.FAILURE;
368 if (!solution_callback.empty())
373 solution_callback(ik_poses[0], solution, error_code);
375 if (error_code.val != error_code.SUCCESS)
377 switch (error_code.val)
379 case moveit_msgs::MoveItErrorCodes::FAILURE:
382 case moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION:
398 const std::vector<double>& joint_angles,
399 std::vector<geometry_msgs::Pose>& poses)
const 406 poses.resize(link_names.size());
std::vector< unsigned int > redundant_joint_indices_
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
virtual const std::string & getGroupName() const
moveit_msgs::KinematicSolverInfo ik_group_info_
const std::vector< std::string > & getJointNames() const
Return all the joint names in the order they are used internally.
int num_possible_redundant_joints_
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
bool isRedundantJoint(unsigned int index) const
const srdf::ModelSharedPtr & getSRDF() const
robot_state::RobotStatePtr robot_state_
std::vector< std::string > tip_frames_
virtual bool setRedundantJoints(const std::vector< unsigned int > &redundant_joint_indices)
#define ROS_INFO_STREAM_NAMED(name, args)
virtual bool initialize(const std::string &robot_description, const std::string &group_name, const std::string &base_name, const std::string &tip_frame, double search_discretization)
virtual const std::vector< std::string > & getTipFrames() const
std::shared_ptr< ros::ServiceClient > ik_service_client_
const std::vector< std::string > & getVariableNames() const
Return all the variable names in the order they are represented internally.
#define ROS_DEBUG_NAMED(name,...)
const std::vector< std::string > & getLinkNames() const
Return all the link names in the order they are represented internally.
virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const
robot_model::RobotModelPtr robot_model_
robot_model::JointModelGroup * joint_model_group_
virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const
bool timedOut(const ros::WallTime &start_time, double duration) const
virtual bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const
bool robotStateMsgToRobotState(const Transforms &tf, const moveit_msgs::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
int getJointIndex(const std::string &name) const
virtual void setValues(const std::string &robot_description, const std::string &group_name, const std::string &base_frame, const std::string &tip_frame, double search_discretization)
bool lookupParam(const std::string ¶m, T &val, const T &default_val) const
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::RobotState &robot_state, bool copy_attached_bodies=true)
Specific implementation of kinematics using ROS service calls to communicate with external IK solvers...
#define ROS_ERROR_NAMED(name,...)
#define ROS_ERROR_STREAM(args)
std::string robot_description_
CLASS_LOADER_REGISTER_CLASS(Dog, Base)
const urdf::ModelInterfaceSharedPtr & getURDF() const
#define ROS_WARN_STREAM_NAMED(name, args)