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

Modify ikfast_template for getPositionIK single solution results #537

Merged
merged 8 commits into from
Jul 28, 2017
Merged

Modify ikfast_template for getPositionIK single solution results #537

Changes from 1 commit
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
98e52fe
Modifeid ikfast_template for getPositionIK single solution case
nsnitish Jun 22, 2017
d08803e
Corrected the harmonize definition
nsnitish Jun 23, 2017
0fc8697
Formated according to style guide and few typos
nsnitish Jun 23, 2017
a6cc992
Sort the solutions before returning closest
nsnitish Jun 29, 2017
0230511
Removed the dependency of getPositionIK from harmonize
nsnitish Jul 1, 2017
370b01f
Removed the dependency of getPositionIK from harmonize
nsnitish Jul 1, 2017
a5f1d19
Used std::sort for solution sorting
nsnitish Jul 22, 2017
85a3afb
Changed struct name for storing and sorting the solutions
nsnitish Jul 26, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Formated according to style guide and few typos
  • Loading branch information
@nsnitish
nsnitish committed Jun 23, 2017
commit 0fc8697c22e96b3a2ddd2bffb1c313cac24e2294
85 changes: 45 additions & 40 deletions moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -201,7 +201,7 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase
* @return True if a valid set of solutions was found, false otherwise.
*/
bool getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
std::vector<std::vector<double> >& solutions, kinematics::KinematicsResult& result,
std::vector<std::vector<double>>& solutions, kinematics::KinematicsResult& result,
const kinematics::KinematicsQueryOptions& options) const;

/**
Expand Down Expand Up @@ -576,26 +576,29 @@ void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal>& solutions
// ROS_ERROR("%f %d",solution[2],vsolfree.size());
}


double IKFastKinematicsPlugin::harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const
double IKFastKinematicsPlugin::harmonize(const std::vector<double>& ik_seed_state, std::vector<double>& solution) const
{
double dist_abs = 0;
std::vector<double> ss = ik_seed_state;
for(size_t i=0; i< ik_seed_state.size(); ++i)
for (size_t i = 0; i < ik_seed_state.size(); ++i)
{
while(ss[i] > M_PI) {
ss[i] -= 2*M_PI;
}
while(ss[i] < -1*M_PI) {
ss[i] += 2*M_PI;
}
while(solution[i] > M_PI) {
solution[i] -= 2*M_PI;
}
while(solution[i] < -1*M_PI) {
solution[i] += 2*M_PI;
}
dist_abs += fabs(ss[i] - solution[i]);
while (ss[i] > M_PI)
{
ss[i] -= 2 * M_PI;
}
while (ss[i] < -1 * M_PI)
{
ss[i] += 2 * M_PI;
}
while (solution[i] > M_PI)
{
solution[i] -= 2 * M_PI;
}
while (solution[i] < -1 * M_PI)
{
solution[i] += 2 * M_PI;
}
dist_abs += fabs(ss[i] - solution[i]);
}
return dist_abs;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

could you elaborate in your PR description what these changes are for? was this harmonize() function not being used previously? was it this broken?

Copy link
Contributor Author

@nsnitish nsnitish Jun 26, 2017 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I will add the comment explaining this. Actually, this function harmonize wasn't written correctly. I think the the solution output from solve(frame, vfree, solutions) as well as seed input can be something out of range (not between -pi to pi). So, this function transforms the solution in the range(-pi to pi) and finds its L1 norm from the transformed seed value.
Originally, this function was not being used. But I found it useful in my implementation. So, I finally modified this because it was running on wrong definition.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@nsnitish: could you take a look at moveit/moveit_ikfast#53?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is exactly what I am talking about. It had wrong definition. Now it makes more sense and I have also added a new PR which tests for the getPositionIK call for closest solution to seed. So, Indirectly this test is testing the harmonize function as well. But adding an explicit test for this can be helpful.

}
Expand Down Expand Up @@ -1049,8 +1052,9 @@ bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, c
if (obeys_limits)
{
// All elements of thi solution obey limits
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

typo "// All elements of thi solution obey limits`"

getSolution(solutions,s,solution_obey_limits);
solutions_obey_limits.push_back(solution_obey_limits);
solution_found = true;
getSolution(solutions, s, solution_obey_limits);
solutions_obey_limits.push_back(solution_obey_limits);
}
}
}
Expand All @@ -1059,40 +1063,41 @@ bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, c
ROS_DEBUG_STREAM_NAMED("ikfast", "No IK solution");
}

// Among the solutions under limits, find the one that is closest to ik_seed_state
// Among the solutions under limits, find the one that is closest to ik_seed_state
if (solutions_obey_limits.size() != 0)
{
double mindist = DBL_MAX;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

in moveit we prefer std::numeric_limits<double>::max()

int minindex = -1;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

can you make these variables use underscore e.g. min_index?

std::vector<double> sol;

double mindist = DBL_MAX;
int minindex = -1;
std::vector<double> sol;

for(size_t i=0; i < solutions_obey_limits.size(); ++i)
{
sol = solutions_obey_limits[i];
double dist = harmonize(ik_seed_state, sol);
ROS_INFO_STREAM_NAMED("ikfast","Dist " << i << " dist " << dist);
for (size_t i = 0; i < solutions_obey_limits.size(); ++i)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

std::size_t please

{
sol = solutions_obey_limits[i];
double dist = harmonize(ik_seed_state, sol);
ROS_INFO_STREAM_NAMED("ikfast", "Dist " << i << " dist " << dist);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this should probably be ROS_DEBUG right?


if(minindex == -1 || dist<mindist){
minindex = i;
mindist = dist;
if (minindex == -1 || dist < mindist)
{
minindex = i;
mindist = dist;
}
Copy link
Contributor

@jrgnicho jrgnicho Jun 30, 2017 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think your sorting code would look cleaner if you were to use the sort algorithm with a predicate function that compares the distances of each argument to the seed. This will do though.

}
if (minindex >= 0)
{
solution = solutions_obey_limits[minindex];
harmonize(ik_seed_state, solution);
}
}
if(minindex >= 0){
solution = solutions_obey_limits[minindex];
harmonize(ik_seed_state, solution);
}
error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
return true;
}

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'd approach this by sorting the solutions in ascending order using sort and returning the first element from the sorted solutions vector. This could be useful in the getPositionIK that returns multiple solutions. This is only a suggestion, I'm OK with your method too.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hi @jrgnicho ! What are the expectations from the getPositionIK call for multiple solutions? Am I being over-concerned about scanning the list of solutions in O(n) and not sorting them in n*logn?
I mean does this complexity make any difference? Because this was the only reason I didn't try sorting the elements. If it doesn't matter, I think your approach will be really beneficial and I will do that.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Since we're only going to be sorting a handful of solution I don't think the performance hit is of much concern.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Added the sort for solutions for getPositionIK with single solution but haven't modified the getPositionIK that returns multiple solutions. Lets leave this for some other PR

error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
return false;
}

bool IKFastKinematicsPlugin::getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses,
const std::vector<double>& ik_seed_state,
std::vector<std::vector<double> >& solutions,
std::vector<std::vector<double>>& solutions,
kinematics::KinematicsResult& result,
const kinematics::KinematicsQueryOptions& options) const
{
Expand Down Expand Up @@ -1130,7 +1135,7 @@ bool IKFastKinematicsPlugin::getPositionIK(const std::vector<geometry_msgs::Pose
tf::poseMsgToKDL(ik_poses[0], frame);

// solving ik
std::vector<IkSolutionList<IkReal> > solution_set;
std::vector<IkSolutionList<IkReal>> solution_set;
IkSolutionList<IkReal> ik_solutions;
std::vector<double> vfree;
int numsol = 0;
Expand Down