44 #include <rclcpp/rclcpp.hpp>
47 #include <Eigen/Geometry>
48 #include <tf2_kdl/tf2_kdl.hpp>
49 #include <tf2_eigen/tf2_eigen.hpp>
50 #include <ikfast_kinematics_parameters.hpp>
74 #define IKFAST_NO_MAIN
161 #include "_ROBOT_NAME___GROUP_NAME__ikfast_solver.cpp"
165 std::vector<std::string> joint_names_;
166 std::vector<double> joint_min_vector_;
167 std::vector<double> joint_max_vector_;
168 std::vector<bool> joint_has_limits_vector_;
169 std::vector<std::string> link_names_;
170 const size_t num_joints_;
171 std::vector<int> free_params_;
173 std::shared_ptr<ikfast_kinematics::ParamListener> param_listener_;
174 ikfast_kinematics::Params params_;
178 const std::string IKFAST_TIP_FRAME_ =
"_EEF_LINK_";
179 const std::string IKFAST_BASE_FRAME_ =
"_BASE_LINK_";
185 bool tip_transform_required_;
186 bool base_transform_required_;
190 Eigen::Isometry3d chain_base_to_group_base_;
191 Eigen::Isometry3d group_tip_to_chain_tip_;
195 const std::vector<std::string>& getJointNames()
const override
199 const std::vector<std::string>& getLinkNames()
const override
210 srand(time(
nullptr));
227 getPositionIK(
const geometry_msgs::msg::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
228 std::vector<double>& solution, moveit_msgs::msg::MoveItErrorCodes& error_code,
246 bool getPositionIK(
const std::vector<geometry_msgs::msg::Pose>& ik_poses,
const std::vector<double>& ik_seed_state,
258 bool searchPositionIK(
259 const geometry_msgs::msg::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
260 std::vector<double>& solution, moveit_msgs::msg::MoveItErrorCodes& error_code,
272 bool searchPositionIK(
273 const geometry_msgs::msg::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
274 const std::vector<double>& consistency_limits, std::vector<double>& solution,
275 moveit_msgs::msg::MoveItErrorCodes& error_code,
286 bool searchPositionIK(
287 const geometry_msgs::msg::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
288 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
289 moveit_msgs::msg::MoveItErrorCodes& error_code,
302 bool searchPositionIK(
303 const geometry_msgs::msg::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
304 const std::vector<double>& consistency_limits, std::vector<double>& solution,
305 const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code,
316 bool getPositionFK(
const std::vector<std::string>& link_names,
const std::vector<double>& joint_angles,
317 std::vector<geometry_msgs::msg::Pose>& poses)
const override;
328 void setSearchDiscretization(
const std::map<unsigned int, double>& discretization);
333 bool setRedundantJoints(
const std::vector<unsigned int>& redundant_joint_indices)
override;
337 const std::string& group_name,
const std::string& base_frame,
338 const std::vector<std::string>& tip_frames,
double search_discretization)
override;
344 size_t solve(KDL::Frame& pose_frame,
const std::vector<double>& vfree, IkSolutionList<IkReal>& solutions)
const;
349 void getSolution(
const IkSolutionList<IkReal>& solutions,
int i, std::vector<double>& solution)
const;
354 void getSolution(
const IkSolutionList<IkReal>& solutions,
const std::vector<double>& ik_seed_state,
int i,
355 std::vector<double>& solution)
const;
360 double enforceLimits(
double val,
double min,
double max)
const;
362 void fillFreeParams(
int count,
int* array);
363 bool getCount(
int& count,
int max_count,
int min_count)
const;
374 bool computeRelativeTransform(
const std::string& from,
const std::string& to, Eigen::Isometry3d& transform,
375 bool& differs_from_identity);
382 void transformToChainFrame(
const geometry_msgs::msg::Pose& ik_pose, KDL::Frame& ik_pose_chain)
const;
385 bool IKFastKinematicsPlugin::computeRelativeTransform(
const std::string& from,
const std::string& to,
386 Eigen::Isometry3d& transform,
bool& differs_from_identity)
388 RobotStatePtr robot_state;
389 robot_state = std::make_shared<RobotState>(robot_model_);
390 robot_state->setToDefaultValues();
393 auto* from_link = robot_model_->getLinkModel(from, &has_link);
394 auto* to_link = robot_model_->getLinkModel(to, &has_link);
395 if (!from_link || !to_link)
398 if (robot_model_->getRigidlyConnectedParentLinkModel(from_link) !=
399 robot_model_->getRigidlyConnectedParentLinkModel(to_link))
401 RCLCPP_ERROR_STREAM(
getLogger(),
"Link frames " << from <<
" and " << to <<
" are not rigidly connected.");
405 transform = robot_state->getGlobalLinkTransform(from_link).inverse() * robot_state->getGlobalLinkTransform(to_link);
406 differs_from_identity = !transform.matrix().isIdentity();
410 bool IKFastKinematicsPlugin::initialize(
const rclcpp::Node::SharedPtr& node,
412 const std::string& base_frame,
const std::vector<std::string>& tip_frames,
413 double search_discretization)
415 if (tip_frames.size() != 1)
417 RCLCPP_ERROR(
getLogger(),
"Expecting exactly one tip frame.");
422 std::string kinematics_param_prefix =
"robot_description_kinematics." + group_name;
423 param_listener_ = std::make_shared<ikfast_kinematics::ParamListener>(node, kinematics_param_prefix);
424 params_ = param_listener_->get_params();
426 storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization);
428 RCLCPP_INFO_STREAM(
getLogger(),
"Using link_prefix: '" << params_.link_prefix <<
'\'');
432 RCLCPP_ERROR_STREAM(
getLogger(),
"tip frame '" << tip_frames_[0] <<
"' does not exist.");
434 RCLCPP_ERROR_STREAM(
getLogger(),
"base_frame '" << base_frame_ <<
"' does not exist.");
436 if (!robot_model.
hasLinkModel(params_.link_prefix + IKFAST_TIP_FRAME_))
437 RCLCPP_ERROR_STREAM(
getLogger(),
"prefixed tip frame '" << params_.link_prefix + IKFAST_TIP_FRAME_
438 <<
"' does not exist. "
439 "Please check your link_prefix parameter.");
440 if (!robot_model.
hasLinkModel(params_.link_prefix + IKFAST_BASE_FRAME_))
441 RCLCPP_ERROR_STREAM(
getLogger(),
"prefixed base frame '" << params_.link_prefix + IKFAST_BASE_FRAME_
442 <<
"' does not exist. "
443 "Please check your link_prefix parameter.");
449 if (!computeRelativeTransform(tip_frames_[0], params_.link_prefix + IKFAST_TIP_FRAME_, group_tip_to_chain_tip_,
450 tip_transform_required_) ||
451 !computeRelativeTransform(params_.link_prefix + IKFAST_BASE_FRAME_, base_frame_, chain_base_to_group_base_,
452 base_transform_required_))
460 if (free_params_.size() > 1)
462 RCLCPP_ERROR(
getLogger(),
"Only one free joint parameter supported!");
465 else if (free_params_.size() == 1)
467 redundant_joint_indices_.clear();
468 redundant_joint_indices_.push_back(free_params_[0]);
469 KinematicsBase::setSearchDiscretization(search_discretization);
475 RCLCPP_ERROR_STREAM(
getLogger(),
"Unknown planning group: " << group_name);
479 RCLCPP_DEBUG(
getLogger(),
"Registering joints and links");
482 while (link && link != base_link)
485 link_names_.push_back(link->
getName());
491 joint_names_.push_back(joint->
getName());
500 if (joint_names_.size() != num_joints_)
502 RCLCPP_FATAL(
getLogger(),
"Joint numbers of RobotModel (%zd) and IKFast solver (%zd) do not match",
503 joint_names_.size(), num_joints_);
507 std::reverse(link_names_.begin(), link_names_.end());
508 std::reverse(joint_names_.begin(), joint_names_.end());
509 std::reverse(joint_min_vector_.begin(), joint_min_vector_.end());
510 std::reverse(joint_max_vector_.begin(), joint_max_vector_.end());
511 std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
513 for (
size_t joint_id = 0; joint_id < num_joints_; ++joint_id)
514 RCLCPP_DEBUG_STREAM(
getLogger(), joint_names_[joint_id] <<
' ' << joint_min_vector_[joint_id] <<
' '
515 << joint_max_vector_[joint_id] <<
' '
516 << joint_has_limits_vector_[joint_id]);
522 void IKFastKinematicsPlugin::setSearchDiscretization(
const std::map<unsigned int, double>& discretization)
524 if (discretization.empty())
526 RCLCPP_ERROR(
getLogger(),
"The 'discretization' map is empty");
530 if (redundant_joint_indices_.empty())
532 RCLCPP_ERROR(
getLogger(),
"This group's solver doesn't support redundant joints");
536 if (discretization.begin()->first != redundant_joint_indices_[0])
538 std::string redundant_joint = joint_names_[free_params_[0]];
539 RCLCPP_ERROR_STREAM(
getLogger(),
"Attempted to discretize a non-redundant joint "
540 << discretization.begin()->first <<
", only joint '" << redundant_joint
541 <<
"' with index " << redundant_joint_indices_[0] <<
" is redundant.");
545 if (discretization.begin()->second <= 0.0)
547 RCLCPP_ERROR_STREAM(
getLogger(),
"Discretization can not takes values that are <= 0");
551 redundant_joint_discretization_.clear();
552 redundant_joint_discretization_[redundant_joint_indices_[0]] = discretization.begin()->second;
555 bool IKFastKinematicsPlugin::setRedundantJoints(
const std::vector<unsigned int>& redundant_joint_indices)
557 RCLCPP_ERROR(
getLogger(),
"Changing the redundant joints isn't permitted by this group's solver ");
561 size_t IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame,
const std::vector<double>& vfree,
562 IkSolutionList<IkReal>& solutions)
const
568 trans[0] = pose_frame.p[0];
569 trans[1] = pose_frame.p[1];
570 trans[2] = pose_frame.p[2];
573 KDL::Vector direction;
584 vals[0] = mult(0, 0);
585 vals[1] = mult(0, 1);
586 vals[2] = mult(0, 2);
587 vals[3] = mult(1, 0);
588 vals[4] = mult(1, 1);
589 vals[5] = mult(1, 2);
590 vals[6] = mult(2, 0);
591 vals[7] = mult(2, 1);
592 vals[8] = mult(2, 2);
595 ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] :
nullptr, solutions);
596 return solutions.GetNumSolutions();
604 direction = pose_frame.M * KDL::Vector(0, 0, 1);
605 ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] :
nullptr, solutions);
606 return solutions.GetNumSolutions();
611 RCLCPP_ERROR(
getLogger(),
"IK for this IkParameterizationType not implemented yet.");
615 ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] :
nullptr, solutions);
616 return solutions.GetNumSolutions();
621 RCLCPP_ERROR(
getLogger(),
"IK for this IkParameterizationType not implemented yet.");
629 double roll, pitch, yaw;
633 pose_frame.M.GetRPY(roll, pitch, yaw);
634 ComputeIk(trans, &yaw, vfree.size() > 0 ? &vfree[0] :
nullptr, solutions);
635 return solutions.GetNumSolutions();
642 pose_frame.M.GetRPY(roll, pitch, yaw);
643 ComputeIk(trans, &roll, vfree.size() > 0 ? &vfree[0] :
nullptr, solutions);
644 return solutions.GetNumSolutions();
651 pose_frame.M.GetRPY(roll, pitch, yaw);
652 ComputeIk(trans, &pitch, vfree.size() > 0 ? &vfree[0] :
nullptr, solutions);
653 return solutions.GetNumSolutions();
656 RCLCPP_ERROR(
getLogger(),
"Unknown IkParameterizationType! "
657 "Was the solver generated with an incompatible version of Openrave?");
662 void IKFastKinematicsPlugin::getSolution(
const IkSolutionList<IkReal>& solutions,
int i,
663 std::vector<double>& solution)
const
666 solution.resize(num_joints_);
669 const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
670 std::vector<IkReal> vsolfree(sol.GetFree().size());
671 sol.GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] :
nullptr);
673 for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id)
675 if (joint_has_limits_vector_[joint_id])
677 solution[joint_id] = enforceLimits(solution[joint_id], joint_min_vector_[joint_id], joint_max_vector_[joint_id]);
682 void IKFastKinematicsPlugin::getSolution(
const IkSolutionList<IkReal>& solutions,
683 const std::vector<double>& ik_seed_state,
int i,
684 std::vector<double>& solution)
const
687 solution.resize(num_joints_);
690 const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
691 std::vector<IkReal> vsolfree(sol.GetFree().size());
692 sol.GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] :
nullptr);
695 for (std::size_t i = 0; i < num_joints_; ++i)
697 if (joint_has_limits_vector_[i])
699 solution[i] = enforceLimits(solution[i], joint_min_vector_[i], joint_max_vector_[i]);
700 double signed_distance = solution[i] - ik_seed_state[i];
701 while (signed_distance > M_PI && solution[i] - 2 * M_PI > (joint_min_vector_[i] -
LIMIT_TOLERANCE))
703 signed_distance -= 2 * M_PI;
704 solution[i] -= 2 * M_PI;
706 while (signed_distance < -M_PI && solution[i] + 2 * M_PI < (joint_max_vector_[i] +
LIMIT_TOLERANCE))
708 signed_distance += 2 * M_PI;
709 solution[i] += 2 * M_PI;
715 double IKFastKinematicsPlugin::enforceLimits(
double joint_value,
double min,
double max)
const
718 while (joint_value > max)
720 joint_value -= 2 * M_PI;
724 while (joint_value < min)
726 joint_value += 2 * M_PI;
731 void IKFastKinematicsPlugin::fillFreeParams(
int count,
int* array)
733 free_params_.clear();
734 for (
int i = 0; i < count; ++i)
735 free_params_.push_back(array[i]);
738 bool IKFastKinematicsPlugin::getCount(
int& count,
int max_count,
int min_count)
const
742 if (-count >= min_count)
747 else if (count + 1 <= max_count)
759 if (1 - count <= max_count)
764 else if (count - 1 >= min_count)
774 bool IKFastKinematicsPlugin::getPositionFK(
const std::vector<std::string>& link_names,
775 const std::vector<double>& joint_angles,
776 std::vector<geometry_msgs::msg::Pose>& poses)
const
784 RCLCPP_ERROR(
getLogger(),
"Can only compute FK for Transform6D IK type!");
789 if (link_names.size() == 0)
791 RCLCPP_WARN_STREAM(
getLogger(),
"Link names with nothing");
795 if (link_names.size() != 1 || link_names[0] != getTipFrame())
797 RCLCPP_ERROR(
getLogger(),
"Can compute FK for %s only", getTipFrame().c_str());
803 IkReal eerot[9], eetrans[3];
805 if (joint_angles.size() != num_joints_)
807 RCLCPP_ERROR(
getLogger(),
"Unexpected number of joint angles");
811 IkReal angles[num_joints_];
812 for (
unsigned char i = 0; i < num_joints_; ++i)
813 angles[i] = joint_angles[i];
818 for (
int i = 0; i < 3; ++i)
819 p_out.p.data[i] = eetrans[i];
821 for (
int i = 0; i < 9; ++i)
822 p_out.M.data[i] = eerot[i];
825 poses[0] = tf2::toMsg(p_out);
830 bool IKFastKinematicsPlugin::searchPositionIK(
const geometry_msgs::msg::Pose& ik_pose,
831 const std::vector<double>& ik_seed_state,
double timeout,
832 std::vector<double>& solution,
833 moveit_msgs::msg::MoveItErrorCodes& error_code,
836 std::vector<double> consistency_limits;
837 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution,
IKCallbackFn(), error_code,
841 bool IKFastKinematicsPlugin::searchPositionIK(
const geometry_msgs::msg::Pose& ik_pose,
842 const std::vector<double>& ik_seed_state,
double timeout,
843 const std::vector<double>& consistency_limits,
844 std::vector<double>& solution,
845 moveit_msgs::msg::MoveItErrorCodes& error_code,
848 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution,
IKCallbackFn(), error_code,
852 bool IKFastKinematicsPlugin::searchPositionIK(
const geometry_msgs::msg::Pose& ik_pose,
853 const std::vector<double>& ik_seed_state,
double timeout,
854 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
855 moveit_msgs::msg::MoveItErrorCodes& error_code,
858 std::vector<double> consistency_limits;
859 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
863 bool IKFastKinematicsPlugin::searchPositionIK(
const geometry_msgs::msg::Pose& ik_pose,
864 const std::vector<double>& ik_seed_state,
double timeout,
865 const std::vector<double>& consistency_limits,
866 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
867 moveit_msgs::msg::MoveItErrorCodes& error_code,
874 if (free_params_.size() == 0)
876 RCLCPP_DEBUG_STREAM(
getLogger(),
"No need to search since no free params/redundant joints");
878 std::vector<geometry_msgs::msg::Pose> ik_poses(1, ik_pose);
879 std::vector<std::vector<double>> solutions;
882 if (!getPositionIK(ik_poses, ik_seed_state, solutions, kinematic_result,
options))
884 RCLCPP_DEBUG(
getLogger(),
"No solution whatsoever");
885 error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
890 std::vector<LimitObeyingSol> solutions_obey_limits;
891 for (std::size_t i = 0; i < solutions.size(); ++i)
893 double dist_from_seed = 0.0;
894 for (std::size_t j = 0; j < ik_seed_state.size(); ++j)
896 dist_from_seed += fabs(ik_seed_state[j] - solutions[i][j]);
899 solutions_obey_limits.push_back({ solutions[i], dist_from_seed });
901 std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end());
904 if (solution_callback)
906 for (std::size_t i = 0; i < solutions_obey_limits.size(); ++i)
908 solution_callback(ik_pose, solutions_obey_limits[i].value, error_code);
909 if (error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
911 solution = solutions_obey_limits[i].value;
912 RCLCPP_DEBUG(
getLogger(),
"Solution passes callback");
917 RCLCPP_DEBUG_STREAM(
getLogger(),
"Solution has error code " << error_code.val);
922 solution = solutions_obey_limits[0].value;
923 error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
932 RCLCPP_ERROR(
getLogger(),
"Kinematics not active");
933 error_code.val = error_code.NO_IK_SOLUTION;
937 if (ik_seed_state.size() != num_joints_)
940 "Seed state must have size " << num_joints_ <<
" instead of size " << ik_seed_state.size());
941 error_code.val = error_code.NO_IK_SOLUTION;
945 if (!consistency_limits.empty() && consistency_limits.size() != num_joints_)
947 RCLCPP_ERROR_STREAM(
getLogger(),
"Consistency limits be empty or must have size "
948 << num_joints_ <<
" instead of size " << consistency_limits.size());
949 error_code.val = error_code.NO_IK_SOLUTION;
957 transformToChainFrame(ik_pose, frame);
959 std::vector<double> vfree(free_params_.size());
963 double initial_guess = ik_seed_state[free_params_[0]];
964 vfree[0] = initial_guess;
968 int num_positive_increments;
969 int num_negative_increments;
970 double search_discretization = redundant_joint_discretization_.at(free_params_[0]);
972 if (!consistency_limits.empty())
976 double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess + consistency_limits[free_params_[0]]);
977 double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess - consistency_limits[free_params_[0]]);
979 num_positive_increments =
static_cast<int>((max_limit - initial_guess) / search_discretization);
980 num_negative_increments =
static_cast<int>((initial_guess - min_limit) / search_discretization);
984 num_positive_increments = (joint_max_vector_[free_params_[0]] - initial_guess) / search_discretization;
985 num_negative_increments = (initial_guess - joint_min_vector_[free_params_[0]]) / search_discretization;
991 RCLCPP_DEBUG_STREAM(
getLogger(),
"Free param is " << free_params_[0] <<
" initial guess is " << initial_guess
992 <<
", # positive increments: " << num_positive_increments
993 <<
", # negative increments: " << num_negative_increments);
994 if ((search_mode &
OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000)
995 RCLCPP_WARN_STREAM_ONCE(
getLogger(),
"Large search space, consider increasing the search discretization");
997 double best_costs = -1.0;
998 std::vector<double> best_solution;
999 int nattempts = 0, nvalid = 0;
1003 IkSolutionList<IkReal> solutions;
1004 size_t numsol = solve(frame, vfree, solutions);
1006 RCLCPP_DEBUG_STREAM(
getLogger(),
"Found " << numsol <<
" solutions from IKFast");
1010 for (
size_t s = 0; s < numsol; ++s)
1013 std::vector<double> sol;
1014 getSolution(solutions, ik_seed_state, s, sol);
1016 bool obeys_limits =
true;
1017 for (
size_t i = 0; i < sol.size(); ++i)
1019 if (joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
1021 obeys_limits =
false;
1029 getSolution(solutions, ik_seed_state, s, solution);
1032 if (solution_callback)
1034 solution_callback(ik_pose, solution, error_code);
1038 error_code.val = error_code.SUCCESS;
1041 if (error_code.val == error_code.SUCCESS)
1048 for (
unsigned int i = 0; i < solution.size(); ++i)
1050 double d = fabs(ik_seed_state[i] - solution[i]);
1054 if (costs < best_costs || best_costs == -1.0)
1057 best_solution = solution;
1068 if (!getCount(counter, num_positive_increments, -num_negative_increments))
1071 error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
1075 vfree[0] = initial_guess + search_discretization * counter;
1079 RCLCPP_DEBUG_STREAM(
getLogger(),
"Valid solutions: " << nvalid <<
'/' << nattempts);
1083 solution = best_solution;
1084 error_code.val = error_code.SUCCESS;
1089 error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
1094 bool IKFastKinematicsPlugin::getPositionIK(
const geometry_msgs::msg::Pose& ik_pose,
1095 const std::vector<double>& ik_seed_state, std::vector<double>& solution,
1096 moveit_msgs::msg::MoveItErrorCodes& error_code,
1099 RCLCPP_DEBUG_STREAM(
getLogger(),
"getPositionIK");
1103 RCLCPP_ERROR(
getLogger(),
"kinematics not active");
1107 if (ik_seed_state.size() < num_joints_)
1109 RCLCPP_ERROR_STREAM(
getLogger(),
"ik_seed_state only has " << ik_seed_state.size()
1110 <<
" entries, this ikfast solver requires "
1116 for (std::size_t i = 0; i < ik_seed_state.size(); ++i)
1119 if (joint_has_limits_vector_[i] && ((ik_seed_state[i] < (joint_min_vector_[i] -
LIMIT_TOLERANCE)) ||
1122 RCLCPP_DEBUG_STREAM(
getLogger(),
"IK seed not in limits! " <<
static_cast<int>(i) <<
" value " << ik_seed_state[i]
1123 <<
" has limit: " << joint_has_limits_vector_[i]
1124 <<
" being " << joint_min_vector_[i] <<
" to "
1125 << joint_max_vector_[i]);
1130 std::vector<double> vfree(free_params_.size());
1131 for (std::size_t i = 0; i < free_params_.size(); ++i)
1133 int p = free_params_[i];
1134 RCLCPP_ERROR(
getLogger(),
"%u is %f", p, ik_seed_state[p]);
1135 vfree[i] = ik_seed_state[p];
1139 transformToChainFrame(ik_pose, frame);
1141 IkSolutionList<IkReal> solutions;
1142 size_t numsol = solve(frame, vfree, solutions);
1143 RCLCPP_DEBUG_STREAM(
getLogger(),
"Found " << numsol <<
" solutions from IKFast");
1145 std::vector<LimitObeyingSol> solutions_obey_limits;
1149 std::vector<double> solution_obey_limits;
1150 for (std::size_t s = 0; s < numsol; ++s)
1152 std::vector<double> sol;
1153 getSolution(solutions, ik_seed_state, s, sol);
1154 RCLCPP_DEBUG(
getLogger(),
"Sol %d: %e %e %e %e %e %e",
static_cast<int>(s), sol[0], sol[1], sol[2],
1155 sol[3], sol[4], sol[5]);
1157 bool obeys_limits =
true;
1158 for (std::size_t i = 0; i < sol.size(); ++i)
1161 if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] -
LIMIT_TOLERANCE)) ||
1165 obeys_limits =
false;
1166 RCLCPP_DEBUG_STREAM(
getLogger(),
"Not in limits! " <<
static_cast<int>(i) <<
" value " << sol[i]
1167 <<
" has limit: " << joint_has_limits_vector_[i]
1168 <<
" being " << joint_min_vector_[i] <<
" to "
1169 << joint_max_vector_[i]);
1176 getSolution(solutions, ik_seed_state, s, solution_obey_limits);
1177 double dist_from_seed = 0.0;
1178 for (std::size_t i = 0; i < ik_seed_state.size(); ++i)
1180 dist_from_seed += fabs(ik_seed_state[i] - solution_obey_limits[i]);
1183 solutions_obey_limits.push_back({ solution_obey_limits, dist_from_seed });
1189 RCLCPP_DEBUG(
getLogger(),
"No IK solution");
1193 if (!solutions_obey_limits.empty())
1195 std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end());
1196 solution = solutions_obey_limits[0].value;
1197 error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
1201 error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
1205 bool IKFastKinematicsPlugin::getPositionIK(
const std::vector<geometry_msgs::msg::Pose>& ik_poses,
1206 const std::vector<double>& ik_seed_state,
1207 std::vector<std::vector<double>>& solutions,
1211 RCLCPP_DEBUG_STREAM(
getLogger(),
"getPositionIK with multiple solutions");
1215 RCLCPP_ERROR(
getLogger(),
"kinematics not active");
1220 if (ik_poses.empty())
1222 RCLCPP_ERROR(
getLogger(),
"ik_poses is empty");
1227 if (ik_poses.size() > 1)
1229 RCLCPP_ERROR(
getLogger(),
"ik_poses contains multiple entries, only one is allowed");
1234 if (ik_seed_state.size() < num_joints_)
1236 RCLCPP_ERROR_STREAM(
getLogger(),
"ik_seed_state only has " << ik_seed_state.size()
1237 <<
" entries, this ikfast solver requires "
1243 transformToChainFrame(ik_poses[0], frame);
1246 std::vector<IkSolutionList<IkReal>> solution_set;
1247 IkSolutionList<IkReal> ik_solutions;
1248 std::vector<double> vfree;
1250 std::vector<double> sampled_joint_vals;
1251 if (!redundant_joint_indices_.empty())
1254 sampled_joint_vals.push_back(ik_seed_state[redundant_joint_indices_[0]]);
1258 joint_has_limits_vector_[redundant_joint_indices_.front()])
1260 double joint_min = joint_min_vector_[redundant_joint_indices_.front()];
1261 double joint_max = joint_max_vector_[redundant_joint_indices_.front()];
1263 double jv = sampled_joint_vals[0];
1267 RCLCPP_ERROR_STREAM(
getLogger(),
"ik seed is out of bounds");
1273 if (!sampleRedundantJoint(
options.discretization_method, sampled_joint_vals))
1279 for (
unsigned int i = 0; i < sampled_joint_vals.size(); ++i)
1282 vfree.push_back(sampled_joint_vals[i]);
1283 numsol += solve(frame, vfree, ik_solutions);
1284 solution_set.push_back(ik_solutions);
1290 numsol = solve(frame, vfree, ik_solutions);
1291 solution_set.push_back(ik_solutions);
1294 RCLCPP_DEBUG_STREAM(
getLogger(),
"Found " << numsol <<
" solutions from IKFast");
1295 bool solutions_found =
false;
1301 for (
unsigned int r = 0; r < solution_set.size(); ++r)
1303 ik_solutions = solution_set[r];
1304 numsol = ik_solutions.GetNumSolutions();
1305 for (
int s = 0; s < numsol; ++s)
1307 std::vector<double> sol;
1308 getSolution(ik_solutions, ik_seed_state, s, sol);
1310 bool obeys_limits =
true;
1311 for (
unsigned int i = 0; i < sol.size(); ++i)
1314 if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] -
LIMIT_TOLERANCE)) ||
1318 obeys_limits =
false;
1319 RCLCPP_DEBUG_STREAM(
getLogger(),
"Not in limits! " << i <<
" value " << sol[i]
1320 <<
" has limit: " << joint_has_limits_vector_[i]
1321 <<
" being " << joint_min_vector_[i] <<
" to "
1322 << joint_max_vector_[i]);
1329 solutions_found =
true;
1330 solutions.push_back(sol);
1335 if (solutions_found)
1343 RCLCPP_DEBUG_STREAM(
getLogger(),
"No IK solution");
1351 std::vector<double>& sampled_joint_vals)
const
1353 int index = redundant_joint_indices_.front();
1354 double joint_dscrt = redundant_joint_discretization_.at(index);
1355 double joint_min = joint_min_vector_[index];
1356 double joint_max = joint_max_vector_[index];
1362 size_t steps = std::ceil((joint_max - joint_min) / joint_dscrt);
1363 for (
size_t i = 0; i < steps; ++i)
1365 sampled_joint_vals.push_back(joint_min + joint_dscrt * i);
1367 sampled_joint_vals.push_back(joint_max);
1372 int steps = std::ceil((joint_max - joint_min) / joint_dscrt);
1373 steps = steps > 0 ? steps : 1;
1374 double diff = joint_max - joint_min;
1375 for (
int i = 0; i < steps; ++i)
1377 sampled_joint_vals.push_back(((diff * std::rand()) / (
static_cast<double>(RAND_MAX))) + joint_min);
1386 RCLCPP_ERROR_STREAM(
getLogger(),
"Discretization method " << method <<
" is not supported");
1393 void IKFastKinematicsPlugin::transformToChainFrame(
const geometry_msgs::msg::Pose& ik_pose,
1394 KDL::Frame& ik_pose_chain)
const
1396 if (tip_transform_required_ || base_transform_required_)
1398 Eigen::Isometry3d ik_eigen_pose;
1399 tf2::fromMsg(ik_pose, ik_eigen_pose);
1400 if (tip_transform_required_)
1401 ik_eigen_pose = ik_eigen_pose * group_tip_to_chain_tip_;
1403 if (base_transform_required_)
1404 ik_eigen_pose = chain_base_to_group_base_ * ik_eigen_pose;
1406 tf2::fromMsg(tf2::toMsg(ik_eigen_pose), ik_pose_chain);
1410 tf2::fromMsg(ik_pose, ik_pose_chain);
1417 #include <pluginlib/class_list_macros.hpp>
Provides an interface for kinematics solvers.
std::function< void(const geometry_msgs::msg::Pose &, const std::vector< double > &, moveit_msgs::msg::MoveItErrorCodes &)> IKCallbackFn
Signature for a callback to validate an IK solution. Typically used for collision checking.
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
const std::string & getName() const
Get the name of the joint.
const VariableBounds & getVariableBounds(const std::string &variable) const
Get the bounds for a variable. Throw an exception if the variable was not found.
JointType getType() const
Get the type of joint.
A link from the robot. Contains the constant transform applied to the link and its geometry.
const JointModel * getParentJointModel() const
Get the joint model whose child this link is. There will always be a parent joint.
const std::string & getName() const
The name of this link.
const LinkModel * getParentLinkModel() const
Get the link model whose child this link is (through some joint). There may not always be a parent li...
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
bool hasLinkModel(const std::string &name) const
Check if a link exists. Return true if it does.
PLUGINLIB_EXPORT_CLASS(_NAMESPACE_::IKFastKinematicsPlugin, kinematics::KinematicsBase)
const double LIMIT_TOLERANCE
SEARCH_MODE
Search modes for searchPositionIK(), see there.
IkParameterizationType
The types of inverse kinematics parameterizations supported.
@ IKP_Ray4D
ray on end effector coordinate system reaches desired global ray
@ IKP_TranslationZAxisAngle4DVelocity
@ IKP_TranslationXAxisAngle4D
@ IKP_Direction3D
direction on end effector coordinate system reaches desired direction
@ IKP_TranslationYAxisAngleXNorm4DVelocity
@ IKP_Direction3DVelocity
@ IKP_TranslationZAxisAngleYNorm4DVelocity
@ IKP_VelocityDataBit
bit is set if the data represents the time-derivate velocity of an IkParameterization
@ IKP_Translation3D
end effector origin reaches desired 3D translation
@ IKP_TranslationZAxisAngleYNorm4D
@ IKP_UniqueIdMask
the mask for the unique ids
@ IKP_Transform6D
end effector reaches desired 6D transformation
@ IKP_NumberOfParameterizations
number of parameterizations (does not count IKP_None)
@ IKP_TranslationLocalGlobal6D
local point on end effector origin reaches desired 3D global point
@ IKP_TranslationLocalGlobal6DVelocity
@ IKP_TranslationXYOrientation3DVelocity
@ IKP_TranslationYAxisAngle4D
@ IKP_TranslationXAxisAngleZNorm4DVelocity
@ IKP_Translation3DVelocity
@ IKP_TranslationYAxisAngle4DVelocity
@ IKP_TranslationXY2D
2D translation along XY plane
@ IKP_TranslationZAxisAngle4D
@ IKP_TranslationDirection5DVelocity
@ IKP_Rotation3D
end effector reaches desired 3D rotation
@ IKP_Transform6DVelocity
@ IKP_TranslationXY2DVelocity
@ IKP_TranslationXYOrientation3D
@ IKP_TranslationXAxisAngleZNorm4D
@ IKP_Lookat3D
direction on end effector coordinate system points to desired 3D position
@ IKP_TranslationYAxisAngleXNorm4D
@ IKP_TranslationXAxisAngle4DVelocity
@ IKP_TranslationDirection5D
@ MULTIPLE_TIPS_NOT_SUPPORTED
@ UNSUPORTED_DISCRETIZATION_REQUESTED
Core components of MoveIt.
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
IKFAST_API int * GetFreeParameters()
IKFAST_API int GetNumJoints()
IKFAST_API int GetIkType()
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
IKFAST_API int GetNumFreeParameters()
std::vector< double > value
bool operator<(const LimitObeyingSol &a) const
A set of options for the kinematics solver.
KinematicError kinematic_error