47 #include <Eigen/Geometry> 65 #define IKFAST_NO_MAIN // Don't include main() from IKFast 156 std::vector<std::string> joint_names_;
166 const std::string IKFAST_TIP_FRAME_ =
"flange";
167 const std::string IKFAST_BASE_FRAME_ =
"base_link";
170 std::string link_prefix_;
185 const std::string name_{
"ikfast" };
187 const std::vector<std::string>& getJointNames()
const override 202 srand(time(
nullptr));
219 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state, std::vector<double>& solution,
220 moveit_msgs::MoveItErrorCodes& error_code,
238 bool getPositionIK(
const std::vector<geometry_msgs::Pose>& ik_poses,
const std::vector<double>& ik_seed_state,
250 bool searchPositionIK(
251 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
252 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
264 bool searchPositionIK(
265 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
266 const std::vector<double>& consistency_limits, std::vector<double>& solution,
267 moveit_msgs::MoveItErrorCodes& error_code,
278 bool searchPositionIK(
279 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
280 std::vector<double>& solution,
const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
293 bool searchPositionIK(
294 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
295 const std::vector<double>& consistency_limits, std::vector<double>& solution,
296 const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
307 bool getPositionFK(
const std::vector<std::string>& link_names,
const std::vector<double>& joint_angles,
308 std::vector<geometry_msgs::Pose>& poses)
const override;
319 void setSearchDiscretization(
const std::map<unsigned int, double>& discretization);
324 bool setRedundantJoints(
const std::vector<unsigned int>& redundant_joint_indices)
override;
328 const std::string& base_frame,
const std::vector<std::string>& tip_frames,
329 double search_discretization)
override;
345 void getSolution(
const IkSolutionList<IkReal>& solutions,
const std::vector<double>& ik_seed_state,
int i,
346 std::vector<double>& solution)
const;
351 double enforceLimits(
double val,
double min,
double max)
const;
353 void fillFreeParams(
int count,
int* array);
354 bool getCount(
int& count,
const int& max_count,
const int& min_count)
const;
365 bool computeRelativeTransform(
const std::string& from,
const std::string& to, Eigen::Isometry3d& transform,
366 bool& differs_from_identity);
373 void transformToChainFrame(
const geometry_msgs::Pose& ik_pose,
KDL::Frame& ik_pose_chain)
const;
376 bool IKFastKinematicsPlugin::computeRelativeTransform(
const std::string& from,
const std::string& to,
377 Eigen::Isometry3d& transform,
bool& differs_from_identity)
379 RobotStatePtr robot_state;
380 robot_state.reset(
new RobotState(robot_model_));
381 robot_state->setToDefaultValues();
383 auto* from_link = robot_model_->getLinkModel(from);
384 auto* to_link = robot_model_->getLinkModel(to);
385 if (!from_link || !to_link)
388 if (robot_model_->getRigidlyConnectedParentLinkModel(from_link) !=
389 robot_model_->getRigidlyConnectedParentLinkModel(to_link))
395 transform = robot_state->getGlobalLinkTransform(from_link).inverse() * robot_state->getGlobalLinkTransform(to_link);
396 differs_from_identity = !transform.matrix().isIdentity();
401 const std::string& base_frame,
const std::vector<std::string>& tip_frames,
402 double search_discretization)
404 if (tip_frames.size() != 1)
410 storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization);
411 if (!lookupParam(
"link_prefix", link_prefix_, std::string(
"")))
426 if (!robot_model.
hasLinkModel(link_prefix_ + IKFAST_TIP_FRAME_))
428 <<
"' does not exist. " 429 "Please check your link_prefix parameter.");
430 if (!robot_model.
hasLinkModel(link_prefix_ + IKFAST_BASE_FRAME_))
432 <<
"' does not exist. " 433 "Please check your link_prefix parameter.");
439 if (!computeRelativeTransform(tip_frames_[0], link_prefix_ + IKFAST_TIP_FRAME_, group_tip_to_chain_tip_,
440 tip_transform_required_) ||
441 !computeRelativeTransform(link_prefix_ + IKFAST_BASE_FRAME_, base_frame_, chain_base_to_group_base_,
442 base_transform_required_))
450 if (free_params_.size() > 1)
455 else if (free_params_.size() == 1)
457 redundant_joint_indices_.clear();
458 redundant_joint_indices_.push_back(free_params_[0]);
459 KinematicsBase::setSearchDiscretization(search_discretization);
472 while (link && link != base_link)
475 link_names_.push_back(link->
getName());
481 joint_names_.push_back(joint->
getName());
490 if (joint_names_.size() != num_joints_)
492 ROS_FATAL_NAMED(name_,
"Joint numbers of RobotModel (%zd) and IKFast solver (%zd) do not match",
493 joint_names_.size(), num_joints_);
497 std::reverse(link_names_.begin(), link_names_.end());
498 std::reverse(joint_names_.begin(), joint_names_.end());
499 std::reverse(joint_min_vector_.begin(), joint_min_vector_.end());
500 std::reverse(joint_max_vector_.begin(), joint_max_vector_.end());
501 std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
503 for (
size_t joint_id = 0; joint_id < num_joints_; ++joint_id)
505 << joint_max_vector_[joint_id] <<
" " 506 << joint_has_limits_vector_[joint_id]);
512 void IKFastKinematicsPlugin::setSearchDiscretization(
const std::map<unsigned int, double>& discretization)
514 if (discretization.empty())
516 ROS_ERROR(
"The 'discretization' map is empty");
520 if (redundant_joint_indices_.empty())
526 if (discretization.begin()->first != redundant_joint_indices_[0])
528 std::string redundant_joint = joint_names_[free_params_[0]];
530 << discretization.begin()->first <<
", only joint '" << redundant_joint
531 <<
"' with index " << redundant_joint_indices_[0] <<
" is redundant.");
535 if (discretization.begin()->second <= 0.0)
541 redundant_joint_discretization_.clear();
542 redundant_joint_discretization_[redundant_joint_indices_[0]] = discretization.begin()->second;
545 bool IKFastKinematicsPlugin::setRedundantJoints(
const std::vector<unsigned int>& redundant_joint_indices)
551 size_t IKFastKinematicsPlugin::solve(
KDL::Frame& pose_frame,
const std::vector<double>& vfree,
558 trans[0] = pose_frame.
p[0];
559 trans[1] = pose_frame.
p[1];
560 trans[2] = pose_frame.
p[2];
574 vals[0] = mult(0, 0);
575 vals[1] = mult(0, 1);
576 vals[2] = mult(0, 2);
577 vals[3] = mult(1, 0);
578 vals[4] = mult(1, 1);
579 vals[5] = mult(1, 2);
580 vals[6] = mult(2, 0);
581 vals[7] = mult(2, 1);
582 vals[8] = mult(2, 2);
585 ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] :
nullptr, solutions);
595 ComputeIk(trans, direction.
data, vfree.size() > 0 ? &vfree[0] :
nullptr, solutions);
604 ROS_ERROR_NAMED(name_,
"IK for this IkParameterizationType not implemented yet.");
610 ROS_ERROR_NAMED(name_,
"IK for this IkParameterizationType not implemented yet.");
617 ROS_ERROR_NAMED(name_,
"IK for this IkParameterizationType not implemented yet.");
621 double roll, pitch, yaw;
625 pose_frame.
M.
GetRPY(roll, pitch, yaw);
626 ComputeIk(trans, &yaw, vfree.size() > 0 ? &vfree[0] :
nullptr, solutions);
633 pose_frame.
M.
GetRPY(roll, pitch, yaw);
634 ComputeIk(trans, &roll, vfree.size() > 0 ? &vfree[0] :
nullptr, solutions);
641 pose_frame.
M.
GetRPY(roll, pitch, yaw);
642 ComputeIk(trans, &pitch, vfree.size() > 0 ? &vfree[0] :
nullptr, solutions);
647 "Was the solver generated with an incompatible version of Openrave?");
653 std::vector<double>& solution)
const 656 solution.resize(num_joints_);
660 std::vector<IkReal> vsolfree(sol.
GetFree().size());
661 sol.
GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] :
nullptr);
663 for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id)
665 if (joint_has_limits_vector_[joint_id])
667 solution[joint_id] = enforceLimits(solution[joint_id], joint_min_vector_[joint_id], joint_max_vector_[joint_id]);
673 const std::vector<double>& ik_seed_state,
int i,
674 std::vector<double>& solution)
const 677 solution.resize(num_joints_);
681 std::vector<IkReal> vsolfree(sol.
GetFree().size());
682 sol.
GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] :
nullptr);
685 for (std::size_t i = 0; i < num_joints_; ++i)
687 if (joint_has_limits_vector_[i])
689 solution[i] = enforceLimits(solution[i], joint_min_vector_[i], joint_max_vector_[i]);
690 double signed_distance = solution[i] - ik_seed_state[i];
693 signed_distance -= 2 *
M_PI;
694 solution[i] -= 2 *
M_PI;
698 signed_distance += 2 *
M_PI;
699 solution[i] += 2 *
M_PI;
705 double IKFastKinematicsPlugin::enforceLimits(
double joint_value,
double min,
double max)
const 708 while (joint_value > max)
710 joint_value -= 2 *
M_PI;
714 while (joint_value < min)
716 joint_value += 2 *
M_PI;
721 void IKFastKinematicsPlugin::fillFreeParams(
int count,
int* array)
723 free_params_.clear();
724 for (
int i = 0; i < count; ++i)
725 free_params_.push_back(array[i]);
728 bool IKFastKinematicsPlugin::getCount(
int& count,
const int& max_count,
const int& min_count)
const 732 if (-count >= min_count)
737 else if (count + 1 <= max_count)
749 if (1 - count <= max_count)
754 else if (count - 1 >= min_count)
764 bool IKFastKinematicsPlugin::getPositionFK(
const std::vector<std::string>& link_names,
765 const std::vector<double>& joint_angles,
766 std::vector<geometry_msgs::Pose>& poses)
const 774 ROS_ERROR_NAMED(name_,
"Can only compute FK for Transform6D IK type!");
779 if (link_names.size() == 0)
785 if (link_names.size() != 1 || link_names[0] != getTipFrame())
787 ROS_ERROR_NAMED(name_,
"Can compute FK for %s only", getTipFrame().c_str());
793 IkReal eerot[9], eetrans[3];
795 if (joint_angles.size() != num_joints_)
801 IkReal angles[num_joints_];
802 for (
unsigned char i = 0; i < num_joints_; i++)
803 angles[i] = joint_angles[i];
808 for (
int i = 0; i < 3; ++i)
809 p_out.
p.
data[i] = eetrans[i];
811 for (
int i = 0; i < 9; ++i)
812 p_out.
M.
data[i] = eerot[i];
820 bool IKFastKinematicsPlugin::searchPositionIK(
const geometry_msgs::Pose& ik_pose,
821 const std::vector<double>& ik_seed_state,
double timeout,
822 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
825 std::vector<double> consistency_limits;
826 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution,
IKCallbackFn(), error_code,
830 bool IKFastKinematicsPlugin::searchPositionIK(
const geometry_msgs::Pose& ik_pose,
831 const std::vector<double>& ik_seed_state,
double timeout,
832 const std::vector<double>& consistency_limits,
833 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
836 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution,
IKCallbackFn(), error_code,
840 bool IKFastKinematicsPlugin::searchPositionIK(
const geometry_msgs::Pose& ik_pose,
841 const std::vector<double>& ik_seed_state,
double timeout,
842 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
843 moveit_msgs::MoveItErrorCodes& error_code,
846 std::vector<double> consistency_limits;
847 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
851 bool IKFastKinematicsPlugin::searchPositionIK(
const geometry_msgs::Pose& ik_pose,
852 const std::vector<double>& ik_seed_state,
double timeout,
853 const std::vector<double>& consistency_limits,
854 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
855 moveit_msgs::MoveItErrorCodes& error_code,
862 if (free_params_.size() == 0)
866 std::vector<geometry_msgs::Pose> ik_poses(1, ik_pose);
867 std::vector<std::vector<double>> solutions;
870 if (!getPositionIK(ik_poses, ik_seed_state, solutions, kinematic_result, options))
873 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
878 std::vector<LimitObeyingSol> solutions_obey_limits;
879 for (std::size_t i = 0; i < solutions.size(); ++i)
881 double dist_from_seed = 0.0;
882 for (std::size_t j = 0; j < ik_seed_state.size(); ++j)
884 dist_from_seed += fabs(ik_seed_state[j] - solutions[i][j]);
887 solutions_obey_limits.push_back({ solutions[i], dist_from_seed });
889 std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end());
892 if (!solution_callback.empty())
894 for (std::size_t i = 0; i < solutions_obey_limits.size(); ++i)
896 solution_callback(ik_pose, solutions_obey_limits[i].value, error_code);
897 if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
899 solution = solutions_obey_limits[i].value;
910 solution = solutions_obey_limits[0].value;
911 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
921 error_code.val = error_code.NO_IK_SOLUTION;
925 if (ik_seed_state.size() != num_joints_)
928 << ik_seed_state.size());
929 error_code.val = error_code.NO_IK_SOLUTION;
933 if (!consistency_limits.empty() && consistency_limits.size() != num_joints_)
935 ROS_ERROR_STREAM_NAMED(name_,
"Consistency limits be empty or must have size " << num_joints_ <<
" instead of size " 936 << consistency_limits.size());
937 error_code.val = error_code.NO_IK_SOLUTION;
945 transformToChainFrame(ik_pose, frame);
947 std::vector<double> vfree(free_params_.size());
951 double initial_guess = ik_seed_state[free_params_[0]];
952 vfree[0] = initial_guess;
956 int num_positive_increments;
957 int num_negative_increments;
958 double search_discretization = redundant_joint_discretization_.at(free_params_[0]);
960 if (!consistency_limits.empty())
964 double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess + consistency_limits[free_params_[0]]);
965 double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess - consistency_limits[free_params_[0]]);
967 num_positive_increments =
static_cast<int>((max_limit - initial_guess) / search_discretization);
968 num_negative_increments =
static_cast<int>((initial_guess - min_limit) / search_discretization);
972 num_positive_increments = (joint_max_vector_[free_params_[0]] - initial_guess) / search_discretization;
973 num_negative_increments = (initial_guess - joint_min_vector_[free_params_[0]]) / search_discretization;
979 ROS_DEBUG_STREAM_NAMED(name_,
"Free param is " << free_params_[0] <<
" initial guess is " << initial_guess
980 <<
", # positive increments: " << num_positive_increments
981 <<
", # negative increments: " << num_negative_increments);
982 if ((search_mode &
OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000)
985 double best_costs = -1.0;
986 std::vector<double> best_solution;
987 int nattempts = 0, nvalid = 0;
992 size_t numsol = solve(frame, vfree, solutions);
998 for (
size_t s = 0;
s < numsol; ++
s)
1001 std::vector<double> sol;
1002 getSolution(solutions, ik_seed_state,
s, sol);
1004 bool obeys_limits =
true;
1005 for (
size_t i = 0; i < sol.size(); i++)
1007 if (joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
1009 obeys_limits =
false;
1017 getSolution(solutions, ik_seed_state,
s, solution);
1020 if (!solution_callback.empty())
1022 solution_callback(ik_pose, solution, error_code);
1026 error_code.val = error_code.SUCCESS;
1029 if (error_code.val == error_code.SUCCESS)
1032 if (search_mode & OPTIMIZE_MAX_JOINT)
1036 for (
unsigned int i = 0; i < solution.size(); i++)
1038 double d = fabs(ik_seed_state[i] - solution[i]);
1042 if (costs < best_costs || best_costs == -1.0)
1045 best_solution = solution;
1056 if (!getCount(counter, num_positive_increments, -num_negative_increments))
1059 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
1063 vfree[0] = initial_guess + search_discretization * counter;
1069 if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0)
1071 solution = best_solution;
1072 error_code.val = error_code.SUCCESS;
1077 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
1082 bool IKFastKinematicsPlugin::getPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
1083 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
1094 if (ik_seed_state.size() < num_joints_)
1097 <<
" entries, this ikfast solver requires " << num_joints_);
1102 for (std::size_t i = 0; i < ik_seed_state.size(); i++)
1105 if (joint_has_limits_vector_[i] && ((ik_seed_state[i] < (joint_min_vector_[i] -
LIMIT_TOLERANCE)) ||
1108 ROS_DEBUG_STREAM_NAMED(name_,
"IK seed not in limits! " << static_cast<int>(i) <<
" value " << ik_seed_state[i]
1109 <<
" has limit: " << joint_has_limits_vector_[i]
1110 <<
" being " << joint_min_vector_[i] <<
" to " 1111 << joint_max_vector_[i]);
1116 std::vector<double> vfree(free_params_.size());
1117 for (std::size_t i = 0; i < free_params_.size(); ++i)
1119 int p = free_params_[i];
1120 ROS_ERROR(
"%u is %f", p, ik_seed_state[p]);
1121 vfree[i] = ik_seed_state[p];
1125 transformToChainFrame(ik_pose, frame);
1128 size_t numsol = solve(frame, vfree, solutions);
1131 std::vector<LimitObeyingSol> solutions_obey_limits;
1135 std::vector<double> solution_obey_limits;
1136 for (std::size_t
s = 0;
s < numsol; ++
s)
1138 std::vector<double> sol;
1139 getSolution(solutions, ik_seed_state,
s, sol);
1140 ROS_DEBUG_NAMED(name_,
"Sol %d: %e %e %e %e %e %e", static_cast<int>(
s), sol[0], sol[1], sol[2], sol[3],
1143 bool obeys_limits =
true;
1144 for (std::size_t i = 0; i < sol.size(); i++)
1147 if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] -
LIMIT_TOLERANCE)) ||
1151 obeys_limits =
false;
1153 <<
" has limit: " << joint_has_limits_vector_[i]
1154 <<
" being " << joint_min_vector_[i] <<
" to " 1155 << joint_max_vector_[i]);
1162 getSolution(solutions, ik_seed_state,
s, solution_obey_limits);
1163 double dist_from_seed = 0.0;
1164 for (std::size_t i = 0; i < ik_seed_state.size(); ++i)
1166 dist_from_seed += fabs(ik_seed_state[i] - solution_obey_limits[i]);
1169 solutions_obey_limits.push_back({ solution_obey_limits, dist_from_seed });
1179 if (!solutions_obey_limits.empty())
1181 std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end());
1182 solution = solutions_obey_limits[0].value;
1183 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
1187 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
1191 bool IKFastKinematicsPlugin::getPositionIK(
const std::vector<geometry_msgs::Pose>& ik_poses,
1192 const std::vector<double>& ik_seed_state,
1193 std::vector<std::vector<double>>& solutions,
1206 if (ik_poses.empty())
1213 if (ik_poses.size() > 1)
1215 ROS_ERROR_NAMED(name_,
"ik_poses contains multiple entries, only one is allowed");
1220 if (ik_seed_state.size() < num_joints_)
1223 <<
" entries, this ikfast solver requires " << num_joints_);
1228 transformToChainFrame(ik_poses[0], frame);
1231 std::vector<IkSolutionList<IkReal>> solution_set;
1233 std::vector<double> vfree;
1235 std::vector<double> sampled_joint_vals;
1236 if (!redundant_joint_indices_.empty())
1239 sampled_joint_vals.push_back(ik_seed_state[redundant_joint_indices_[0]]);
1243 joint_has_limits_vector_[redundant_joint_indices_.front()])
1245 double joint_min = joint_min_vector_[redundant_joint_indices_.front()];
1246 double joint_max = joint_max_vector_[redundant_joint_indices_.front()];
1248 double jv = sampled_joint_vals[0];
1264 for (
unsigned int i = 0; i < sampled_joint_vals.size(); i++)
1267 vfree.push_back(sampled_joint_vals[i]);
1268 numsol += solve(frame, vfree, ik_solutions);
1269 solution_set.push_back(ik_solutions);
1275 numsol = solve(frame, vfree, ik_solutions);
1276 solution_set.push_back(ik_solutions);
1280 bool solutions_found =
false;
1286 for (
unsigned int r = 0;
r < solution_set.size();
r++)
1288 ik_solutions = solution_set[
r];
1290 for (
int s = 0;
s < numsol; ++
s)
1292 std::vector<double> sol;
1293 getSolution(ik_solutions, ik_seed_state,
s, sol);
1295 bool obeys_limits =
true;
1296 for (
unsigned int i = 0; i < sol.size(); i++)
1299 if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] -
LIMIT_TOLERANCE)) ||
1303 obeys_limits =
false;
1305 << joint_has_limits_vector_[i] <<
" being " 1306 << joint_min_vector_[i] <<
" to " << joint_max_vector_[i]);
1313 solutions_found =
true;
1314 solutions.push_back(sol);
1319 if (solutions_found)
1335 std::vector<double>& sampled_joint_vals)
const 1337 int index = redundant_joint_indices_.front();
1338 double joint_dscrt = redundant_joint_discretization_.at(index);
1339 double joint_min = joint_min_vector_[index];
1340 double joint_max = joint_max_vector_[index];
1346 size_t steps = std::ceil((joint_max - joint_min) / joint_dscrt);
1347 for (
size_t i = 0; i < steps; i++)
1349 sampled_joint_vals.push_back(joint_min + joint_dscrt * i);
1351 sampled_joint_vals.push_back(joint_max);
1356 int steps = std::ceil((joint_max - joint_min) / joint_dscrt);
1357 steps = steps > 0 ? steps : 1;
1358 double diff = joint_max - joint_min;
1359 for (
int i = 0; i < steps; i++)
1361 sampled_joint_vals.push_back(((diff * std::rand()) / (static_cast<double>(RAND_MAX))) + joint_min);
1377 void IKFastKinematicsPlugin::transformToChainFrame(
const geometry_msgs::Pose& ik_pose,
KDL::Frame& ik_pose_chain)
const 1379 if (tip_transform_required_ || base_transform_required_)
1381 Eigen::Isometry3d ik_eigen_pose;
1383 if (tip_transform_required_)
1384 ik_eigen_pose = ik_eigen_pose * group_tip_to_chain_tip_;
1386 if (base_transform_required_)
1387 ik_eigen_pose = chain_base_to_group_base_ * ik_eigen_pose;
const std::string & getName() const
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
KinematicError kinematic_error
Eigen::Isometry3d chain_base_to_group_base_
const std::string & getName() const
number of parameterizations (does not count IKP_None)
std::size_t getVariableCount() const
PLUGINLIB_EXPORT_CLASS(prbt_manipulator::IKFastKinematicsPlugin, kinematics::KinematicsBase)
#define ROS_INFO_NAMED(name,...)
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
const std::vector< std::string > & getLinkNames() const override
ROSCONSOLE_DECL void initialize()
virtual size_t GetNumSolutions() const
returns the number of solutions stored
IKFAST_API int GetNumJoints()
IkParameterizationType
The types of inverse kinematics parameterizations supported.
direction on end effector coordinate system reaches desired direction
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
end effector origin reaches desired 3D translation
UNSUPORTED_DISCRETIZATION_REQUESTED
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
bool tip_transform_required_
end effector reaches desired 3D rotation
The discrete solutions are returned in this structure.
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
ray on end effector coordinate system reaches desired global ray
#define ROS_INFO_STREAM_NAMED(name, args)
std::vector< int > free_params_
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...
IKFAST_API int GetIkType()
void transformEigenToKDL(const Eigen::Affine3d &e, KDL::Frame &k)
bit is set if the data represents the time-derivate velocity of an IkParameterization ...
const JointModel * getParentJointModel() const
std::vector< bool > joint_has_limits_vector_
#define ROS_DEBUG_NAMED(name,...)
const double LIMIT_TOLERANCE
Eigen::Isometry3d group_tip_to_chain_tip_
std::vector< double > value
#define ROS_WARN_STREAM_ONCE_NAMED(name, args)
end effector reaches desired 6D transformation
void fromMsg(const A &, B &b)
IKFAST_API int GetNumFreeParameters()
IKFAST_API int * GetFreeParameters()
std::vector< double > joint_max_vector_
DiscretizationMethod discretization_method
std::vector< std::string > link_names_
the mask for the unique ids
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
bool operator<(const LimitObeyingSol &a) const
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
bool base_transform_required_
direction on end effector coordinate system points to desired 3D position
2D translation along XY plane
Default implementation of IkSolutionListBase.
#define ROS_FATAL_NAMED(name,...)
#define ROS_ERROR_NAMED(name,...)
MULTIPLE_TIPS_NOT_SUPPORTED
bool hasLinkModel(const std::string &name) const
const LinkModel * getParentLinkModel() const
const VariableBounds & getVariableBounds(const std::string &variable) const
JointType getType() const
local point on end effector origin reaches desired 3D global point
#define ROS_WARN_STREAM_NAMED(name, args)
std::vector< double > joint_min_vector_
SEARCH_MODE
Search modes for searchPositionIK(), see there.