65 #define IKFAST_NO_MAIN // Don't include main() from IKFast 184 bool getPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
185 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
203 bool getPositionIK(
const std::vector<geometry_msgs::Pose>& ik_poses,
const std::vector<double>& ik_seed_state,
215 bool searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
216 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
228 bool searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
229 const std::vector<double>& consistency_limits, std::vector<double>& solution,
230 moveit_msgs::MoveItErrorCodes& error_code,
241 bool searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
242 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
243 moveit_msgs::MoveItErrorCodes& error_code,
256 bool searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
257 const std::vector<double>& consistency_limits, std::vector<double>& solution,
258 const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
269 bool getPositionFK(
const std::vector<std::string>& link_names,
const std::vector<double>& joint_angles,
270 std::vector<geometry_msgs::Pose>& poses)
const;
289 bool initialize(
const std::string& robot_description,
const std::string& group_name,
const std::string& base_name,
290 const std::string& tip_name,
double search_discretization);
303 double harmonize(
const std::vector<double>& ik_seed_state, std::vector<double>& solution)
const;
306 std::vector<double>& solution)
const;
308 bool getCount(
int& count,
const int& max_count,
const int& min_count)
const;
321 const std::string& base_name,
const std::string& tip_name,
322 double search_discretization)
324 setValues(robot_description, group_name, base_name, tip_name, search_discretization);
329 node_handle.
param(
"robot", robot, std::string());
335 if (free_params_.size() > 1)
337 ROS_FATAL(
"Only one free joint parameter supported!");
340 else if (free_params_.size() == 1)
350 std::string urdf_xml, full_urdf_xml;
351 node_handle.
param(
"urdf_xml", urdf_xml, robot_description);
355 if (!node_handle.
getParam(full_urdf_xml, xml_string))
357 ROS_FATAL_NAMED(
"ikfast",
"Could not load the xml from parameter server: %s", urdf_xml.c_str());
361 node_handle.
param(full_urdf_xml, xml_string, std::string());
366 urdf::LinkConstSharedPtr link = robot_model.getLink(
getTipFrame());
370 link_names_.push_back(link->name);
371 urdf::JointSharedPtr joint = link->parent_joint;
374 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
378 joint_names_.push_back(joint->name);
381 if (joint->type != urdf::Joint::CONTINUOUS)
385 lower = joint->safety->soft_lower_limit;
386 upper = joint->safety->soft_upper_limit;
390 lower = joint->limits->lower;
391 upper = joint->limits->upper;
403 joint_has_limits_vector_.push_back(
true);
404 joint_min_vector_.push_back(lower);
405 joint_max_vector_.push_back(upper);
409 joint_has_limits_vector_.push_back(
false);
410 joint_min_vector_.push_back(-
M_PI);
411 joint_max_vector_.push_back(
M_PI);
417 ROS_WARN_NAMED(
"ikfast",
"no joint corresponding to %s", link->name.c_str());
419 link = link->getParent();
424 ROS_FATAL_STREAM_NAMED(
"ikfast",
"Joint numbers mismatch: URDF has " << joint_names_.size() <<
" and IKFast has " 429 std::reverse(link_names_.begin(), link_names_.end());
430 std::reverse(joint_names_.begin(), joint_names_.end());
431 std::reverse(joint_min_vector_.begin(), joint_min_vector_.end());
432 std::reverse(joint_max_vector_.begin(), joint_max_vector_.end());
433 std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
436 ROS_DEBUG_STREAM_NAMED(
"ikfast", joint_names_[i] <<
" " << joint_min_vector_[i] <<
" " << joint_max_vector_[i]
437 <<
" " << joint_has_limits_vector_[i]);
445 if (discretization.empty())
447 ROS_ERROR(
"The 'discretization' map is empty");
459 std::string redundant_joint = joint_names_[free_params_[0]];
461 << discretization.begin()->first <<
", only joint '" << redundant_joint <<
"' with index " 466 if (discretization.begin()->second <= 0.0)
478 ROS_ERROR_STREAM(
"Changing the redundant joints isn't permitted by this group's solver ");
489 trans[0] = pose_frame.
p[0];
490 trans[1] = pose_frame.
p[1];
491 trans[2] = pose_frame.
p[2];
505 vals[0] = mult(0, 0);
506 vals[1] = mult(0, 1);
507 vals[2] = mult(0, 2);
508 vals[3] = mult(1, 0);
509 vals[4] = mult(1, 1);
510 vals[5] = mult(1, 2);
511 vals[6] = mult(2, 0);
512 vals[7] = mult(2, 1);
513 vals[8] = mult(2, 2);
516 ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
526 ComputeIk(trans, direction.
data, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
534 ROS_ERROR_NAMED(
"ikfast",
"IK for this IkParameterizationType not implemented yet.");
540 ROS_ERROR_NAMED(
"ikfast",
"IK for this IkParameterizationType not implemented yet.");
550 ROS_ERROR_NAMED(
"ikfast",
"IK for this IkParameterizationType not implemented yet.");
554 ROS_ERROR_NAMED(
"ikfast",
"Unknown IkParameterizationType! Was the solver generated with an incompatible version " 561 std::vector<double>& solution)
const 564 solution.resize(num_joints_);
568 std::vector<IkReal> vsolfree(sol.
GetFree().size());
569 sol.
GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL);
582 std::vector<double> ss = ik_seed_state;
583 for (
size_t i = 0; i < ik_seed_state.size(); ++i)
585 while (ss[i] > 2 *
M_PI)
589 while (ss[i] < 2 *
M_PI)
593 while (solution[i] > 2 *
M_PI)
595 solution[i] -= 2 *
M_PI;
597 while (solution[i] < 2 *
M_PI)
599 solution[i] += 2 *
M_PI;
601 dist_sqr += fabs(ik_seed_state[i] - solution[i]);
630 const std::vector<double>& ik_seed_state,
631 std::vector<double>& solution)
const 633 double mindist = DBL_MAX;
635 std::vector<double> sol;
641 double dist =
harmonize(ik_seed_state, sol);
644 if (minindex == -1 || dist < mindist)
659 free_params_.clear();
660 for (
int i = 0; i < count; ++i)
661 free_params_.push_back(array[i]);
668 if (-count >= min_count)
673 else if (count + 1 <= max_count)
685 if (1 - count <= max_count)
690 else if (count - 1 >= min_count)
701 const std::vector<double>& joint_angles,
702 std::vector<geometry_msgs::Pose>& poses)
const 710 ROS_ERROR_NAMED(
"ikfast",
"Can only compute FK for Transform6D IK type!");
715 if (link_names.size() == 0)
721 if (link_names.size() != 1 || link_names[0] !=
getTipFrame())
729 IkReal eerot[9], eetrans[3];
730 IkReal
angles[joint_angles.size()];
731 for (
unsigned char i = 0; i < joint_angles.size(); i++)
732 angles[i] = joint_angles[i];
737 for (
int i = 0; i < 3; ++i)
738 p_out.
p.
data[i] = eetrans[i];
740 for (
int i = 0; i < 9; ++i)
741 p_out.
M.
data[i] = eerot[i];
750 const std::vector<double>& ik_seed_state,
double timeout,
751 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
755 std::vector<double> consistency_limits;
757 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
762 const std::vector<double>& ik_seed_state,
double timeout,
763 const std::vector<double>& consistency_limits,
764 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
768 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
773 const std::vector<double>& ik_seed_state,
double timeout,
774 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
775 moveit_msgs::MoveItErrorCodes& error_code,
778 std::vector<double> consistency_limits;
779 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
784 const std::vector<double>& ik_seed_state,
double timeout,
785 const std::vector<double>& consistency_limits,
786 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
787 moveit_msgs::MoveItErrorCodes& error_code,
796 if (free_params_.size() == 0)
801 if (!
getPositionIK(ik_pose, ik_seed_state, solution, error_code))
804 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
809 if (!solution_callback.empty())
811 solution_callback(ik_pose, solution, error_code);
812 if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
834 error_code.val = error_code.NO_IK_SOLUTION;
841 << ik_seed_state.size());
842 error_code.val = error_code.NO_IK_SOLUTION;
846 if (!consistency_limits.empty() && consistency_limits.size() !=
num_joints_)
849 << num_joints_ <<
" instead of size " << consistency_limits.size());
850 error_code.val = error_code.NO_IK_SOLUTION;
860 std::vector<double> vfree(free_params_.size());
865 double initial_guess = ik_seed_state[free_params_[0]];
866 vfree[0] = initial_guess;
870 int num_positive_increments;
871 int num_negative_increments;
873 if (!consistency_limits.empty())
877 double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess + consistency_limits[free_params_[0]]);
878 double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess - consistency_limits[free_params_[0]]);
892 ROS_DEBUG_STREAM_NAMED(
"ikfast",
"Free param is " << free_params_[0] <<
" initial guess is " << initial_guess
893 <<
", # positive increments: " << num_positive_increments
894 <<
", # negative increments: " << num_negative_increments);
895 if ((search_mode &
OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000)
898 double best_costs = -1.0;
899 std::vector<double> best_solution;
900 int nattempts = 0, nvalid = 0;
905 int numsol =
solve(frame, vfree, solutions);
913 for (
int s = 0;
s < numsol; ++
s)
916 std::vector<double> sol;
919 bool obeys_limits =
true;
920 for (
unsigned int i = 0; i < sol.size(); i++)
922 if (joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
924 obeys_limits =
false;
935 if (!solution_callback.empty())
937 solution_callback(ik_pose, solution, error_code);
941 error_code.val = error_code.SUCCESS;
944 if (error_code.val == error_code.SUCCESS)
947 if (search_mode & OPTIMIZE_MAX_JOINT)
951 for (
unsigned int i = 0; i < solution.size(); i++)
953 double d = fabs(ik_seed_state[i] - solution[i]);
957 if (costs < best_costs || best_costs == -1.0)
960 best_solution = solution;
971 if (!
getCount(counter, num_positive_increments, -num_negative_increments))
974 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
984 if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0)
986 solution = best_solution;
987 error_code.val = error_code.SUCCESS;
992 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
998 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
1009 std::vector<double> vfree(free_params_.size());
1010 for (std::size_t i = 0; i < free_params_.size(); ++i)
1012 int p = free_params_[i];
1013 ROS_ERROR(
"%u is %f", p, ik_seed_state[p]);
1014 vfree[i] = ik_seed_state[p];
1021 int numsol =
solve(frame, vfree, solutions);
1027 for (
int s = 0;
s < numsol; ++
s)
1029 std::vector<double> sol;
1031 ROS_DEBUG_NAMED(
"ikfast",
"Sol %d: %e %e %e %e %e %e",
s, sol[0], sol[1], sol[2], sol[3], sol[4],
1034 bool obeys_limits =
true;
1035 for (
unsigned int i = 0; i < sol.size(); i++)
1038 if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] -
LIMIT_TOLERANCE)) ||
1042 obeys_limits =
false;
1044 << joint_has_limits_vector_[i] <<
" being " 1045 << joint_min_vector_[i] <<
" to " << joint_max_vector_[i]);
1053 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
1063 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
1068 const std::vector<double>& ik_seed_state,
1069 std::vector<std::vector<double> >& solutions,
1082 if (ik_poses.empty())
1089 if (ik_poses.size() > 1)
1091 ROS_ERROR(
"ik_poses contains multiple entries, only one is allowed");
1098 ROS_ERROR_STREAM(
"ik_seed_state only has " << ik_seed_state.size() <<
" entries, this ikfast solver requires " 1107 std::vector<IkSolutionList<IkReal> > solution_set;
1109 std::vector<double> vfree;
1111 std::vector<double> sampled_joint_vals;
1119 joint_has_limits_vector_[redundant_joint_indices_.front()])
1121 double joint_min = joint_min_vector_[redundant_joint_indices_.front()];
1122 double joint_max = joint_max_vector_[redundant_joint_indices_.front()];
1124 double jv = sampled_joint_vals[0];
1140 for (
unsigned int i = 0; i < sampled_joint_vals.size(); i++)
1143 vfree.push_back(sampled_joint_vals[i]);
1144 numsol +=
solve(frame, vfree, ik_solutions);
1145 solution_set.push_back(ik_solutions);
1151 numsol =
solve(frame, vfree, ik_solutions);
1152 solution_set.push_back(ik_solutions);
1156 bool solutions_found =
false;
1162 for (
unsigned int r = 0;
r < solution_set.size();
r++)
1164 ik_solutions = solution_set[
r];
1166 for (
int s = 0;
s < numsol; ++
s)
1168 std::vector<double> sol;
1171 bool obeys_limits =
true;
1172 for (
unsigned int i = 0; i < sol.size(); i++)
1175 if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] -
LIMIT_TOLERANCE)) ||
1179 obeys_limits =
false;
1181 "ikfast",
"Not in limits! " << i <<
" value " << sol[i] <<
" has limit: " << joint_has_limits_vector_[i]
1182 <<
" being " << joint_min_vector_[i] <<
" to " << joint_max_vector_[i]);
1189 solutions_found =
true;
1190 solutions.push_back(sol);
1195 if (solutions_found)
1211 std::vector<double>& sampled_joint_vals)
const 1213 double joint_min = -
M_PI;
1214 double joint_max =
M_PI;
1220 joint_min = joint_min_vector_[index];
1221 joint_max = joint_max_vector_[index];
1228 int steps = std::ceil((joint_max - joint_min) / joint_dscrt);
1229 for (
unsigned int i = 0; i < steps; i++)
1231 sampled_joint_vals.push_back(joint_min + joint_dscrt * i);
1233 sampled_joint_vals.push_back(joint_max);
1238 int steps = std::ceil((joint_max - joint_min) / joint_dscrt);
1239 steps = steps > 0 ? steps : 1;
1240 double diff = joint_max - joint_min;
1241 for (
int i = 0; i < steps; i++)
1243 sampled_joint_vals.push_back(((diff * std::rand()) / (static_cast<double>(RAND_MAX))) + joint_min);
1252 ROS_ERROR_STREAM(
"Discretization method " << method <<
" is not supported");
std::vector< unsigned int > redundant_joint_indices_
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
KinematicError kinematic_error
IKFAST_API int GetIkType()
std::vector< double > joint_max_vector_
std::vector< std::string > joint_names_
SEARCH_MODE
Search modes for searchPositionIK(), see there.
local point on end effector origin reaches desired 3D global point
#define ROS_DEBUG_STREAM_NAMED(name, args)
std::vector< bool > joint_has_limits_vector_
#define ROS_ERROR_STREAM_NAMED(name, args)
std::vector< double > joint_min_vector_
URDF_EXPORT bool initString(const std::string &xmlstring)
end effector origin reaches desired 3D translation
#define ROS_WARN_NAMED(name,...)
2D translation along XY plane
end effector reaches desired 3D rotation
end effector reaches desired 6D transformation
const std::vector< std::string > & getLinkNames() const
IKFAST_API int * GetFreeParameters()
virtual size_t GetNumSolutions() const
returns the number of solutions stored
PLUGINLIB_EXPORT_CLASS(katana_450_6m90a_kinematics::IKFastKinematicsPlugin, kinematics::KinematicsBase)
UNSUPORTED_DISCRETIZATION_REQUESTED
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
The discrete solutions are returned in this structure.
IKFAST_API int GetNumJoints()
#define ROS_INFO_STREAM_NAMED(name, args)
const std::vector< std::string > & getJointNames() const
virtual void Clear()
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
std::vector< std::string > link_names_
virtual const std::vector< int > & GetFree() const =0
Gets the indices of the configuration space that have to be preset before a full solution can be retu...
const double LIMIT_TOLERANCE
void getSolution(const IkSolutionList< IkReal > &solutions, int i, std::vector< double > &solution) const
Gets a specific solution from the set.
number of parameterizations (does not count IKP_None)
bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const
Given a set of joint angles and a set of links, compute their pose.
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
Given a desired pose of the end-effector, compute the joint angles to reach it.
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
void setSearchDiscretization(const std::map< int, double > &discretization)
Sets the discretization value for the redundant joint.
#define ROS_DEBUG_NAMED(name,...)
the mask for the unique ids
#define ROS_WARN_STREAM_ONCE_NAMED(name, args)
IKFAST_API int GetNumFreeParameters()
std::vector< int > free_params_
virtual const std::string & getTipFrame() const
void fillFreeParams(int count, int *array)
#define ROS_FATAL_STREAM_NAMED(name, args)
ray on end effector coordinate system reaches desired global ray
bool searchParam(const std::string &key, std::string &result) const
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
double harmonize(const std::vector< double > &ik_seed_state, std::vector< double > &solution) const
direction on end effector coordinate system points to desired 3D position
DiscretizationMethod discretization_method
std::vector< DiscretizationMethod > supported_methods_
bool getCount(int &count, const int &max_count, const int &min_count) 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)
def xml_string(rootXml, addHeader=True)
bit is set if the data represents the time-derivate velocity of an IkParameterization ...
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
IkParameterizationType
The types of inverse kinematics parameterizations supported.
std::map< int, double > redundant_joint_discretization_
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
bool setRedundantJoints(const std::vector< unsigned int > &redundant_joint_indices)
Overrides the default method to prevent changing the redundant joints.
static const double DEFAULT_SEARCH_DISCRETIZATION
bool initialize(const std::string &robot_description, const std::string &group_name, const std::string &base_name, const std::string &tip_name, double search_discretization)
int solve(KDL::Frame &pose_frame, const std::vector< double > &vfree, IkSolutionList< IkReal > &solutions) const
Calls the IK solver from IKFast.
void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
void getClosestSolution(const IkSolutionList< IkReal > &solutions, const std::vector< double > &ik_seed_state, std::vector< double > &solution) const
bool sampleRedundantJoint(kinematics::DiscretizationMethod method, std::vector< double > &sampled_joint_vals) const
samples the designated redundant joint using the chosen discretization method
Default implementation of IkSolutionListBase.
bool getParam(const std::string &key, std::string &s) const
double search_discretization_
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
#define ROS_FATAL_NAMED(name,...)
#define ROS_ERROR_NAMED(name,...)
MULTIPLE_TIPS_NOT_SUPPORTED
#define ROS_ERROR_STREAM(args)
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
Given a desired pose of the end-effector, search for the joint angles required to reach it...
direction on end effector coordinate system reaches desired direction
#define ROS_WARN_STREAM_NAMED(name, args)