Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[IKFastTemplate] Expand solutions to full joint range in searchPositionIK #598

Merged
merged 2 commits into from Sep 21, 2017
Merged

[IKFastTemplate] Expand solutions to full joint range in searchPositionIK #598

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3376006
Changed searchPositionIK to return the closest solution to the seed i…
hartmanndennis Aug 22, 2017
14a8ac9
Added new function getSolution with extra parameter ik_seed_state tha…
hartmanndennis Sep 11, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
48 changes: 43 additions & 5 deletions moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp
View file
Open in desktop
Expand Up @@ -312,6 +312,12 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase
*/
void getSolution(const IkSolutionList<IkReal>& solutions, int i, std::vector<double>& solution) const;

/**
* @brief Gets a specific solution from the set with joints rotated 360° to be near seed state where possible
*/
void getSolution(const IkSolutionList<IkReal>& solutions, const std::vector<double>& ik_seed_state, int i,
std::vector<double>& solution) const;

double harmonize(const std::vector<double>& ik_seed_state, std::vector<double>& solution) const;
// void getOrderedSolutions(const std::vector<double> &ik_seed_state, std::vector<std::vector<double> >& solslist);
void getClosestSolution(const IkSolutionList<IkReal>& solutions, const std::vector<double>& ik_seed_state,
Expand Down Expand Up @@ -588,6 +594,38 @@ void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal>& solutions
// ROS_ERROR("%f %d",solution[2],vsolfree.size());
}

void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal>& solutions,
const std::vector<double>& ik_seed_state, int i,
std::vector<double>& solution) const
{
solution.clear();
solution.resize(num_joints_);

// IKFast56/61
const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
std::vector<IkReal> vsolfree(sol.GetFree().size());
sol.GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL);

// rotate joints by +/-360° where it is possible and useful
for (std::size_t i = 0; i < num_joints_; ++i)
{
if (joint_has_limits_vector_[i])
{
double signed_distance = solution[i] - ik_seed_state[i];
while (signed_distance > M_PI && solution[i] - 2 * M_PI > (joint_min_vector_[i] - LIMIT_TOLERANCE))
{
signed_distance -= 2 * M_PI;
solution[i] -= 2 * M_PI;
}
while (signed_distance < -M_PI && solution[i] + 2 * M_PI < (joint_max_vector_[i] + LIMIT_TOLERANCE))
{
signed_distance += 2 * M_PI;
solution[i] += 2 * M_PI;
}
}
}
}

double IKFastKinematicsPlugin::harmonize(const std::vector<double>& ik_seed_state, std::vector<double>& solution) const
{
double dist_sqr = 0;
Expand Down Expand Up @@ -947,7 +985,7 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose
{
nattempts++;
std::vector<double> sol;
getSolution(solutions, s, sol);
getSolution(solutions, ik_seed_state, s, sol);

bool obeys_limits = true;
for (unsigned int i = 0; i < sol.size(); i++)
Expand All @@ -962,7 +1000,7 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose
}
if (obeys_limits)
{
getSolution(solutions, s, solution);
getSolution(solutions, ik_seed_state, s, solution);

// This solution is within joint limits, now check if in collision (if callback provided)
if (!solution_callback.empty())
Expand Down Expand Up @@ -1083,7 +1121,7 @@ bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, c
for (std::size_t s = 0; s < numsol; ++s)
{
std::vector<double> sol;
getSolution(solutions, s, sol);
getSolution(solutions, ik_seed_state, s, sol);
ROS_DEBUG_NAMED("ikfast", "Sol %d: %e %e %e %e %e %e", (int)s, sol[0], sol[1], sol[2], sol[3], sol[4],
sol[5]);

Expand All @@ -1105,7 +1143,7 @@ bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, c
if (obeys_limits)
{
// All elements of this solution obey limits
getSolution(solutions, s, solution_obey_limits);
getSolution(solutions, ik_seed_state, s, solution_obey_limits);
double dist_from_seed = 0.0;
for (std::size_t i = 0; i < ik_seed_state.size(); ++i)
{
Expand Down Expand Up @@ -1236,7 +1274,7 @@ bool IKFastKinematicsPlugin::getPositionIK(const std::vector<geometry_msgs::Pose
for (int s = 0; s < numsol; ++s)
{
std::vector<double> sol;
getSolution(ik_solutions, s, sol);
getSolution(ik_solutions, ik_seed_state, s, sol);

bool obeys_limits = true;
for (unsigned int i = 0; i < sol.size(); i++)
Expand Down