-
Notifications
You must be signed in to change notification settings - Fork 105
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.
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
Add toleranced Cartesian waypoints to solver #354
Add toleranced Cartesian waypoints to solver #354
Conversation
trajopt/src/kinematic_terms.cpp
Outdated
Eigen::VectorXd calculateExceededTolerance(const Eigen::VectorXd& lower_tolerance, | ||
const Eigen::VectorXd& upper_tolerance, | ||
const Eigen::VectorXd& error) | ||
{ | ||
// Initialize the resultant vector | ||
Eigen::VectorXd resultant(error.size()); | ||
|
||
// Iterate through each element | ||
for (int i = 0; i < error.size(); ++i) | ||
{ | ||
// If error is within tolerances, set resultant to 0 | ||
if (error(i) >= lower_tolerance(i) && error(i) <= upper_tolerance(i)) | ||
{ | ||
resultant(i) = 0.0; | ||
} | ||
// If error is below lower tolerance, set resultant to error - lower_tolerance | ||
else if (error(i) < lower_tolerance(i)) | ||
{ | ||
resultant(i) = error(i) - lower_tolerance(i); | ||
} | ||
// If error is above upper tolerance, set resultant to error - upper_tolerance | ||
else if (error(i) > upper_tolerance(i)) | ||
{ | ||
resultant(i) = error(i) - upper_tolerance(i); | ||
} | ||
} | ||
|
||
return resultant; | ||
} | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think this already exists in Trajopt IFOPT in a vectorized, potentially faster way here
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
That comparison you referenced is doing something slightly different. I reimplemented this though in the latest commit.
In order for this to not have stability issues this cannot use the the jacobian utility to provide the jacobain because the error function is not continuous. When within the tolerance this will produce an all zero jacobian which is not correct so this will need to have its own jacobian method. This need to more or less use the error function without tolerance to calculate the jacobian. |
This is why in my tesseract_planning PR I made it so that we can simultaneously have both costs and constraints, so the cost will continue to drive it to zero while the constraint will keep it inside the bounds. However, if there's a better solution I'd definitely be open to making the changes for that. |
I would update this so the class holds a std::function which should be used to calculate the error. Then based on construction if toleranced we point it to one function and if not tolerance it points to another function. The function signature should be the same as |
Does this have anything to do with the Jacobian issue, or is the purpose of this to save on computation in the cases where there is no tolerance applied? |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Also need to figure out how to handle the jacobian issue when toleranced.
No, just computation and address the issue @marip8 pointed out. |
I added an error function option, but I'm not sure what we want to put the default values of the tolerances at now. The error function defaults to |
Just realized I don't actually use the tolerances anywhere any more. Should the tolerances just be completely dropped from these structs/classes and put the burden on a higher level function calling them to pass in an error function that captures the tolerances to calculate the error? |
Sorry for the confusion. I would not pass the error function into the constructor. I would keep the tolerance as input to the constructor and have the constructor evaluate the tolerance to determine which function to assign to the internal member variable. |
This has been working for me now and I think it addresses everything. It has been very useful in increasing motion planning success. |
5f623af
to
9a0155c
Compare
9a0155c
to
add5389
Compare
The test that is failing is comparing the numerical to the analytical Jacobian solutions, which would be expected to be different now because of the addition of Failing comparison: https://github.com/marrts/trajopt_ros/blob/9b7676069257930aca365c23adc142579a6d3e8d/trajopt/test/kinematic_costs_unit.cpp#L56 |
@marrts I will take a look at this and push a fix. |
trajopt/src/problem_description.cpp
Outdated
target_frame_offset, | ||
indices, | ||
lower_tolerance, | ||
upper_tolerance); | ||
|
||
// This is currently not being used. There is an intermittent bug that needs to be tracked down it is not used. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Is this issue solved now? Otherwise please add @todo, so it can be found more easily.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I would say this resolves it, so I'm removing those comments.
trajopt/src/problem_description.cpp
Outdated
target_frame_offset, | ||
indices, | ||
lower_tolerance, | ||
upper_tolerance); | ||
|
||
// This is currently not being used. There is an intermittent bug that needs to be tracked down it is not used. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Is this issue solved now? Otherwise please add @todo, so it can be found more easily.
trajopt/src/problem_description.cpp
Outdated
target_frame_offset, | ||
indices, | ||
lower_tolerance, | ||
upper_tolerance); | ||
|
||
// This is currently not being used. There is an intermittent bug that needs to be tracked down it is not used. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Is this issue solved now? Otherwise please add @todo, so it can be found more easily.
Could you implement this for trajopt_ifopt as well? |
This test is failing because the rotational error set to
The Jacobian results are:
The first three rows, corresponding to position, match and the last three, corresponding to rotation are different. This is the expected behavior based on the initial error vectors we have. If you rotate the target frame by 180 degrees around z this goes away and the test passes:
I see a few options for resolving this:
|
I haven't used trajopt_ifopt yet, and it's not obvious to me what changes would need to be made. Perhaps we get this one merged we make an issue to implement this in trajopt_ifopt? I do see that there is a trajopt/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h Lines 175 to 176 in 0733775
|
@marrts I would just apply your change to make the unit test pass. |
9b76760
to
566b795
Compare
988c079
into
tesseract-robotics:master
This is to support tolerances on Cartesian waypoints so that you can set constraints that don't have to match perfectly