65 #define IKFAST_NO_MAIN // Don't include main() from IKFast 159 std::vector<bool> joint_has_limits_vector_;
164 const std::string name_{
"ikfast" };
166 const std::vector<std::string>& getJointNames()
const 197 bool getPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
198 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
216 bool getPositionIK(
const std::vector<geometry_msgs::Pose>& ik_poses,
const std::vector<double>& ik_seed_state,
228 bool searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
229 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
241 bool searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
242 const std::vector<double>& consistency_limits, std::vector<double>& solution,
243 moveit_msgs::MoveItErrorCodes& error_code,
254 bool searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
255 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
256 moveit_msgs::MoveItErrorCodes& error_code,
269 bool searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
270 const std::vector<double>& consistency_limits, std::vector<double>& solution,
271 const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
282 bool getPositionFK(
const std::vector<std::string>& link_names,
const std::vector<double>& joint_angles,
283 std::vector<geometry_msgs::Pose>& poses)
const;
294 void setSearchDiscretization(
const std::map<unsigned int, double>& discretization);
299 bool setRedundantJoints(
const std::vector<unsigned int>& redundant_joint_indices);
302 bool initialize(
const std::string& robot_description,
const std::string& group_name,
const std::string& base_name,
303 const std::vector<std::string>& tip_names,
double search_discretization);
305 bool initialize(
const std::string& robot_description,
const std::string& group_name,
const std::string& base_name,
306 const std::string& tip_name,
double search_discretization);
322 void getSolution(
const IkSolutionList<IkReal>& solutions,
const std::vector<double>& ik_seed_state,
int i,
323 std::vector<double>& solution)
const;
325 double harmonize(
const std::vector<double>& ik_seed_state, std::vector<double>& solution)
const;
327 void getClosestSolution(
const IkSolutionList<IkReal>& solutions,
const std::vector<double>& ik_seed_state,
328 std::vector<double>& solution)
const;
329 void fillFreeParams(
int count,
int* array);
330 bool getCount(
int& count,
const int& max_count,
const int& min_count)
const;
343 const std::string& base_name,
const std::string& tip_name,
344 double search_discretization)
347 std::vector<std::string> tipnames {tip_name};
348 return initialize(robot_description, group_name, base_name, tipnames, search_discretization);
352 const std::string& base_name,
const std::vector<std::string>& tip_names,
353 double search_discretization)
355 setValues(robot_description, group_name, base_name, tip_names, search_discretization);
360 lookupParam(
"robot", robot, std::string());
365 if (free_params_.size() > 1)
367 ROS_FATAL(
"Only one free joint parameter supported!");
370 else if (free_params_.size() == 1)
372 redundant_joint_indices_.clear();
373 redundant_joint_indices_.push_back(free_params_[0]);
380 std::string urdf_xml, full_urdf_xml;
381 lookupParam(
"urdf_xml", urdf_xml, robot_description);
385 if (!node_handle.
getParam(full_urdf_xml, xml_string))
387 ROS_FATAL_NAMED(name_,
"Could not load the xml from parameter server: %s", urdf_xml.c_str());
395 urdf::LinkConstSharedPtr link = robot_model.getLink(getTipFrame());
396 while (link->name != base_frame_ && joint_names_.size() <= num_joints_)
399 link_names_.push_back(link->name);
400 urdf::JointSharedPtr joint = link->parent_joint;
403 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
407 joint_names_.push_back(joint->name);
410 if (joint->type != urdf::Joint::CONTINUOUS)
414 lower = joint->safety->soft_lower_limit;
415 upper = joint->safety->soft_upper_limit;
419 lower = joint->limits->lower;
420 upper = joint->limits->upper;
432 joint_has_limits_vector_.push_back(
true);
433 joint_min_vector_.push_back(lower);
434 joint_max_vector_.push_back(upper);
438 joint_has_limits_vector_.push_back(
false);
439 joint_min_vector_.push_back(-M_PI);
440 joint_max_vector_.push_back(M_PI);
446 ROS_WARN_NAMED(name_,
"no joint corresponding to %s", link->name.c_str());
448 link = link->getParent();
451 if (joint_names_.size() != num_joints_)
453 ROS_FATAL_STREAM_NAMED(name_,
"Joint numbers mismatch: URDF has " << joint_names_.size() <<
" and IKFast has " 458 std::reverse(link_names_.begin(), link_names_.end());
459 std::reverse(joint_names_.begin(), joint_names_.end());
460 std::reverse(joint_min_vector_.begin(), joint_min_vector_.end());
461 std::reverse(joint_max_vector_.begin(), joint_max_vector_.end());
462 std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
464 for (
size_t i = 0; i < num_joints_; ++i)
465 ROS_DEBUG_STREAM_NAMED(name_, joint_names_[i] <<
" " << joint_min_vector_[i] <<
" " << joint_max_vector_[i] <<
" " 466 << joint_has_limits_vector_[i]);
474 if (discretization.empty())
476 ROS_ERROR(
"The 'discretization' map is empty");
480 if (redundant_joint_indices_.empty())
486 if (discretization.begin()->first != redundant_joint_indices_[0])
488 std::string redundant_joint = joint_names_[free_params_[0]];
490 << discretization.begin()->first <<
", only joint '" << redundant_joint <<
"' with index " 491 << redundant_joint_indices_[0] <<
" is redundant.");
495 if (discretization.begin()->second <= 0.0)
501 redundant_joint_discretization_.clear();
502 redundant_joint_discretization_[redundant_joint_indices_[0]] = discretization.begin()->second;
507 ROS_ERROR_STREAM(
"Changing the redundant joints isn't permitted by this group's solver ");
518 trans[0] = pose_frame.
p[0];
519 trans[1] = pose_frame.
p[1];
520 trans[2] = pose_frame.
p[2];
534 vals[0] = mult(0, 0);
535 vals[1] = mult(0, 1);
536 vals[2] = mult(0, 2);
537 vals[3] = mult(1, 0);
538 vals[4] = mult(1, 1);
539 vals[5] = mult(1, 2);
540 vals[6] = mult(2, 0);
541 vals[7] = mult(2, 1);
542 vals[8] = mult(2, 2);
545 ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
555 ComputeIk(trans, direction.
data, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
564 ROS_ERROR_NAMED(name_,
"IK for this IkParameterizationType not implemented yet.");
570 ROS_ERROR_NAMED(name_,
"IK for this IkParameterizationType not implemented yet.");
577 ROS_ERROR_NAMED(name_,
"IK for this IkParameterizationType not implemented yet.");
581 double roll, pitch, yaw;
585 pose_frame.
M.
GetRPY(roll, pitch, yaw);
586 ComputeIk(trans, &yaw, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
593 pose_frame.
M.
GetRPY(roll, pitch, yaw);
594 ComputeIk(trans, &roll, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
601 pose_frame.
M.
GetRPY(roll, pitch, yaw);
602 ComputeIk(trans, &pitch, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
607 "Was the solver generated with an incompatible version of Openrave?");
613 std::vector<double>& solution)
const 616 solution.resize(num_joints_);
620 std::vector<IkReal> vsolfree(sol.
GetFree().size());
621 sol.
GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL);
632 const std::vector<double>& ik_seed_state,
int i,
633 std::vector<double>& solution)
const 636 solution.resize(num_joints_);
640 std::vector<IkReal> vsolfree(sol.
GetFree().size());
641 sol.
GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL);
644 for (std::size_t i = 0; i < num_joints_; ++i)
646 if (joint_has_limits_vector_[i])
648 double signed_distance = solution[i] - ik_seed_state[i];
651 signed_distance -= 2 *
M_PI;
652 solution[i] -= 2 *
M_PI;
656 signed_distance += 2 *
M_PI;
657 solution[i] += 2 *
M_PI;
666 std::vector<double> ss = ik_seed_state;
667 for (
size_t i = 0; i < ik_seed_state.size(); ++i)
669 while (ss[i] > 2 *
M_PI)
673 while (ss[i] < 2 *
M_PI)
677 while (solution[i] > 2 *
M_PI)
679 solution[i] -= 2 *
M_PI;
681 while (solution[i] < 2 *
M_PI)
683 solution[i] += 2 *
M_PI;
685 dist_sqr += fabs(ik_seed_state[i] - solution[i]);
714 const std::vector<double>& ik_seed_state,
715 std::vector<double>& solution)
const 717 double mindist = DBL_MAX;
719 std::vector<double> sol;
724 getSolution(solutions, i, sol);
725 double dist = harmonize(ik_seed_state, sol);
728 if (minindex == -1 || dist < mindist)
736 getSolution(solutions, minindex, solution);
737 harmonize(ik_seed_state, solution);
743 free_params_.clear();
744 for (
int i = 0; i < count; ++i)
745 free_params_.push_back(array[i]);
752 if (-count >= min_count)
757 else if (count + 1 <= max_count)
769 if (1 - count <= max_count)
774 else if (count - 1 >= min_count)
785 const std::vector<double>& joint_angles,
786 std::vector<geometry_msgs::Pose>& poses)
const 794 ROS_ERROR_NAMED(name_,
"Can only compute FK for Transform6D IK type!");
799 if (link_names.size() == 0)
805 if (link_names.size() != 1 || link_names[0] != getTipFrame())
807 ROS_ERROR_NAMED(name_,
"Can compute FK for %s only", getTipFrame().c_str());
813 IkReal eerot[9], eetrans[3];
815 if (joint_angles.size() != num_joints_)
821 IkReal angles[num_joints_];
822 for (
unsigned char i = 0; i < num_joints_; i++)
823 angles[i] = joint_angles[i];
828 for (
int i = 0; i < 3; ++i)
829 p_out.
p.
data[i] = eetrans[i];
831 for (
int i = 0; i < 9; ++i)
832 p_out.
M.
data[i] = eerot[i];
841 const std::vector<double>& ik_seed_state,
double timeout,
842 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
846 std::vector<double> consistency_limits;
848 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
853 const std::vector<double>& ik_seed_state,
double timeout,
854 const std::vector<double>& consistency_limits,
855 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
859 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
864 const std::vector<double>& ik_seed_state,
double timeout,
865 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
866 moveit_msgs::MoveItErrorCodes& error_code,
869 std::vector<double> consistency_limits;
870 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
875 const std::vector<double>& ik_seed_state,
double ,
876 const std::vector<double>& consistency_limits,
877 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
878 moveit_msgs::MoveItErrorCodes& error_code,
887 if (free_params_.size() == 0)
891 std::vector<geometry_msgs::Pose> ik_poses(1, ik_pose);
892 std::vector<std::vector<double>> solutions;
895 if (!getPositionIK(ik_poses, ik_seed_state, solutions, kinematic_result, options))
898 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
903 std::vector<LimitObeyingSol> solutions_obey_limits;
904 for (std::size_t i = 0; i < solutions.size(); ++i)
907 for (std::size_t j = 0; j < ik_seed_state.size(); ++j)
909 dist_from_seed += fabs(ik_seed_state[j] - solutions[i][j]);
912 solutions_obey_limits.push_back({ solutions[i], dist_from_seed });
914 std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end());
917 if (!solution_callback.empty())
919 for (std::size_t i = 0; i < solutions_obey_limits.size(); ++i)
921 solution_callback(ik_pose, solutions_obey_limits[i].
value, error_code);
922 if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
924 solution = solutions_obey_limits[i].value;
935 solution = solutions_obey_limits[0].value;
936 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
946 error_code.val = error_code.NO_IK_SOLUTION;
950 if (ik_seed_state.size() != num_joints_)
953 << ik_seed_state.size());
954 error_code.val = error_code.NO_IK_SOLUTION;
958 if (!consistency_limits.empty() && consistency_limits.size() != num_joints_)
960 ROS_ERROR_STREAM_NAMED(name_,
"Consistency limits be empty or must have size " << num_joints_ <<
" instead of size " 961 << consistency_limits.size());
962 error_code.val = error_code.NO_IK_SOLUTION;
972 std::vector<double> vfree(free_params_.size());
976 double initial_guess = ik_seed_state[free_params_[0]];
977 vfree[0] = initial_guess;
981 int num_positive_increments;
982 int num_negative_increments;
984 if (!consistency_limits.empty())
988 double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess + consistency_limits[free_params_[0]]);
989 double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess - consistency_limits[free_params_[0]]);
991 num_positive_increments = (int)((max_limit - initial_guess) / redundant_joint_discretization_.at(0));
992 num_negative_increments = (int)((initial_guess - min_limit) / redundant_joint_discretization_.at(0));
996 num_positive_increments = (joint_max_vector_[free_params_[0]] - initial_guess)
997 / redundant_joint_discretization_.at(0);
998 num_negative_increments = (initial_guess - joint_min_vector_[free_params_[0]])
999 / redundant_joint_discretization_.at(0);
1005 ROS_DEBUG_STREAM_NAMED(name_,
"Free param is " << free_params_[0] <<
" initial guess is " << initial_guess
1006 <<
", # positive increments: " << num_positive_increments
1007 <<
", # negative increments: " << num_negative_increments);
1008 if ((search_mode &
OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000)
1011 double best_costs = -1.0;
1012 std::vector<double> best_solution;
1013 int nattempts = 0, nvalid = 0;
1018 unsigned int numsol = solve(frame, vfree, solutions);
1026 for (
unsigned int s = 0;
s < numsol; ++
s)
1029 std::vector<double> sol;
1030 getSolution(solutions, ik_seed_state,
s, sol);
1032 bool obeys_limits =
true;
1033 for (
unsigned int i = 0; i < sol.size(); i++)
1035 if (joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
1037 obeys_limits =
false;
1045 getSolution(solutions, ik_seed_state,
s, solution);
1048 if (!solution_callback.empty())
1050 solution_callback(ik_pose, solution, error_code);
1054 error_code.val = error_code.SUCCESS;
1057 if (error_code.val == error_code.SUCCESS)
1060 if (search_mode & OPTIMIZE_MAX_JOINT)
1064 for (
unsigned int i = 0; i < solution.size(); i++)
1066 double d = fabs(ik_seed_state[i] - solution[i]);
1070 if (costs < best_costs || best_costs == -1.0)
1073 best_solution = solution;
1084 if (!getCount(counter, num_positive_increments, -num_negative_increments))
1087 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
1091 vfree[0] = initial_guess + redundant_joint_discretization_.at(0) * counter;
1097 if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0)
1099 solution = best_solution;
1100 error_code.val = error_code.SUCCESS;
1105 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
1111 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
1122 if (ik_seed_state.size() < num_joints_)
1124 ROS_ERROR_STREAM(
"ik_seed_state only has " << ik_seed_state.size() <<
" entries, this ikfast solver requires " 1130 for (std::size_t i = 0; i < ik_seed_state.size(); i++)
1133 if (joint_has_limits_vector_[i] && ((ik_seed_state[i] < (joint_min_vector_[i] -
LIMIT_TOLERANCE)) ||
1137 <<
" has limit: " << joint_has_limits_vector_[i] <<
" being " 1138 << joint_min_vector_[i] <<
" to " << joint_max_vector_[i]);
1143 std::vector<double> vfree(free_params_.size());
1144 for (std::size_t i = 0; i < free_params_.size(); ++i)
1146 int p = free_params_[i];
1147 ROS_ERROR(
"%u is %f", p, ik_seed_state[p]);
1148 vfree[i] = ik_seed_state[p];
1155 unsigned int numsol = solve(frame, vfree, solutions);
1158 std::vector<LimitObeyingSol> solutions_obey_limits;
1162 std::vector<double> solution_obey_limits;
1163 for (std::size_t
s = 0;
s < numsol; ++
s)
1165 std::vector<double> sol;
1166 getSolution(solutions, ik_seed_state,
s, sol);
1167 ROS_DEBUG_NAMED(name_,
"Sol %d: %e %e %e %e %e %e", (
int)
s, sol[0], sol[1], sol[2], sol[3], sol[4],
1170 bool obeys_limits =
true;
1171 for (std::size_t i = 0; i < sol.size(); i++)
1174 if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] -
LIMIT_TOLERANCE)) ||
1178 obeys_limits =
false;
1180 << joint_has_limits_vector_[i] <<
" being " 1181 << joint_min_vector_[i] <<
" to " << joint_max_vector_[i]);
1188 getSolution(solutions, ik_seed_state, s, solution_obey_limits);
1190 for (std::size_t i = 0; i < ik_seed_state.size(); ++i)
1192 dist_from_seed += fabs(ik_seed_state[i] - solution_obey_limits[i]);
1195 solutions_obey_limits.push_back({ solution_obey_limits, dist_from_seed });
1205 if (!solutions_obey_limits.empty())
1207 std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end());
1208 solution = solutions_obey_limits[0].value;
1209 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
1213 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
1218 const std::vector<double>& ik_seed_state,
1219 std::vector<std::vector<double>>& solutions,
1232 if (ik_poses.empty())
1239 if (ik_poses.size() > 1)
1241 ROS_ERROR(
"ik_poses contains multiple entries, only one is allowed");
1246 if (ik_seed_state.size() < num_joints_)
1248 ROS_ERROR_STREAM(
"ik_seed_state only has " << ik_seed_state.size() <<
" entries, this ikfast solver requires " 1257 std::vector<IkSolutionList<IkReal>> solution_set;
1259 std::vector<double> vfree;
1260 unsigned int numsol = 0;
1261 std::vector<double> sampled_joint_vals;
1262 if (!redundant_joint_indices_.empty())
1265 sampled_joint_vals.push_back(ik_seed_state[redundant_joint_indices_[0]]);
1269 joint_has_limits_vector_[redundant_joint_indices_.front()])
1271 double joint_min = joint_min_vector_[redundant_joint_indices_.front()];
1272 double joint_max = joint_max_vector_[redundant_joint_indices_.front()];
1274 double jv = sampled_joint_vals[0];
1290 for (
unsigned int i = 0; i < sampled_joint_vals.size(); i++)
1293 vfree.push_back(sampled_joint_vals[i]);
1294 numsol += solve(frame, vfree, ik_solutions);
1295 solution_set.push_back(ik_solutions);
1301 numsol = solve(frame, vfree, ik_solutions);
1302 solution_set.push_back(ik_solutions);
1306 bool solutions_found =
false;
1312 for (
unsigned int r = 0;
r < solution_set.size();
r++)
1314 ik_solutions = solution_set[
r];
1316 for (
unsigned int s = 0;
s < numsol; ++
s)
1318 std::vector<double> sol;
1319 getSolution(ik_solutions, ik_seed_state,
s, sol);
1321 bool obeys_limits =
true;
1322 for (
unsigned int i = 0; i < sol.size(); i++)
1325 if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] -
LIMIT_TOLERANCE)) ||
1329 obeys_limits =
false;
1331 << joint_has_limits_vector_[i] <<
" being " 1332 << joint_min_vector_[i] <<
" to " << joint_max_vector_[i]);
1339 solutions_found =
true;
1340 solutions.push_back(sol);
1345 if (solutions_found)
1361 std::vector<double>& sampled_joint_vals)
const 1363 double joint_min = -
M_PI;
1364 double joint_max =
M_PI;
1365 int index = redundant_joint_indices_.front();
1366 double joint_dscrt = redundant_joint_discretization_.at(index);
1368 if (joint_has_limits_vector_[redundant_joint_indices_.front()])
1370 joint_min = joint_min_vector_[index];
1371 joint_max = joint_max_vector_[index];
1378 unsigned int steps = std::ceil((joint_max - joint_min) / joint_dscrt);
1379 for (
unsigned int i = 0; i < steps; i++)
1381 sampled_joint_vals.push_back(joint_min + joint_dscrt * i);
1383 sampled_joint_vals.push_back(joint_max);
1388 unsigned int steps = std::ceil((joint_max - joint_min) / joint_dscrt);
1389 steps = steps > 0 ? steps : 1;
1390 double diff = joint_max - joint_min;
1391 for (
unsigned int i = 0; i < steps; i++)
1393 sampled_joint_vals.push_back(((diff * std::rand()) / (static_cast<double>(RAND_MAX))) + joint_min);
1402 ROS_ERROR_STREAM(
"Discretization method " << method <<
" is not supported");
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
KinematicError kinematic_error
const std::vector< std::string > & getLinkNames() const
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
URDF_EXPORT bool initString(const std::string &xmlstring)
#define ROS_WARN_NAMED(name,...)
bool initialize(const std::string &robot_description, const std::string &group_name, const std::string &base_name, const std::vector< std::string > &tip_names, double search_discretization)
bool getCount(int &count, const int &max_count, const int &min_count) const
ROSCONSOLE_DECL void initialize()
virtual size_t GetNumSolutions() const
returns the number of solutions stored
the mask for the unique ids
void getClosestSolution(const IkSolutionList< IkReal > &solutions, const std::vector< double > &ik_seed_state, std::vector< double > &solution) const
UNSUPORTED_DISCRETIZATION_REQUESTED
ray on end effector coordinate system reaches desired global ray
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
IkParameterizationType
The types of inverse kinematics parameterizations supported.
std::vector< int > free_params_
The discrete solutions are returned in this structure.
unsigned int solve(KDL::Frame &pose_frame, const std::vector< double > &vfree, IkSolutionList< IkReal > &solutions) const
Calls the IK solver from IKFast.
bool sampleRedundantJoint(kinematics::DiscretizationMethod method, std::vector< double > &sampled_joint_vals) const
samples the designated redundant joint using the chosen discretization method
local point on end effector origin reaches desired 3D global point
std::vector< std::string > link_names_
void setSearchDiscretization(const std::map< unsigned int, double > &discretization)
Sets the discretization value for the redundant joint.
#define ROS_INFO_STREAM_NAMED(name, args)
bool operator<(const LimitObeyingSol &a) const
2D translation along XY plane
virtual void Clear()
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
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...
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...
IKFAST_API int GetNumFreeParameters()
void fillFreeParams(int count, int *array)
number of parameterizations (does not count IKP_None)
#define ROS_DEBUG_NAMED(name,...)
const double LIMIT_TOLERANCE
std::vector< double > value
bool setRedundantJoints(const std::vector< unsigned int > &redundant_joint_indices)
Overrides the default method to prevent changing the redundant joints.
#define ROS_WARN_STREAM_ONCE_NAMED(name, args)
PLUGINLIB_EXPORT_CLASS(ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase)
end effector reaches desired 6D transformation
end effector reaches desired 3D rotation
#define ROS_FATAL_STREAM_NAMED(name, args)
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
bool searchParam(const std::string &key, std::string &result) const
void fromMsg(const A &, B &b)
IKFAST_API int * GetFreeParameters()
direction on end effector coordinate system points to desired 3D position
DiscretizationMethod discretization_method
double harmonize(const std::vector< double > &ik_seed_state, std::vector< double > &solution) const
def xml_string(rootXml, addHeader=True)
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
void GetRPY(double &roll, double &pitch, double &yaw) const
direction on end effector coordinate system reaches desired direction
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
bit is set if the data represents the time-derivate velocity of an IkParameterization ...
Default implementation of IkSolutionListBase.
bool getParam(const std::string &key, std::string &s) const
#define ROS_FATAL_NAMED(name,...)
#define ROS_ERROR_NAMED(name,...)
IKFAST_API int GetIkType()
MULTIPLE_TIPS_NOT_SUPPORTED
IKFAST_API int GetNumJoints()
#define ROS_ERROR_STREAM(args)
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.
static constexpr double DEFAULT_SEARCH_DISCRETIZATION
end effector origin reaches desired 3D translation
std::vector< double > joint_min_vector_
std::vector< std::string > joint_names_
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
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.
std::vector< double > joint_max_vector_
void getSolution(const IkSolutionList< IkReal > &solutions, int i, std::vector< double > &solution) const
Gets a specific solution from the set.
#define ROS_WARN_STREAM_NAMED(name, args)
SEARCH_MODE
Search modes for searchPositionIK(), see there.