search

tesseract-robotics / trajopt

Trajectory Optimization Motion Planner for ROS
381 stars 103 forks source link

Fully integrate support for LVS collision constraints #260

Closed Levi-Armstrong closed 2 years ago

Levi-Armstrong commented 3 years ago

@mpowelson I am getting closer but still have two unit tests failing. The first is the numerical ik test which has the same results for several iterations but then they go in different directions so there may be a difference in the trust region solver which I may need a little help tracking down.

Levi-Armstrong commented 3 years ago

Also, It took me a while to figure out how to use the SquaredCost with a constraint which requires you to call LinkWithVariableSet on the constraint before passing it to the SquaredCost. Is there a reason not to pass the problem into the constructor of the SquaredCost and let it call LinkWithVariableSet?

mpowelson commented 3 years ago

I think that would be fine. I guess you have to have a problem in order to call getCost anyway right?

mpowelson commented 3 years ago

@mpowelson I am getting closer but still have two unit tests failing. The first is the numerical ik test which has the same results for several iterations but then they go in different directions so there may be a difference in the trust region solver which I may need a little help tracking down.

Let me know if I can help out.

Levi-Armstrong commented 3 years ago

I think that would be fine. I guess you have to have a problem in order to call getCost anyway right?

Yea, I don't think there is a case where you would not already have one. Another option is see if the maintainer would be willing to make the method LinkWithVarables virtual then we could overload it. Do you think it is worth proposing?

mpowelson commented 3 years ago

I think that would be fine. I guess you have to have a problem in order to call getCost anyway right?

Yea, I don't think there is a case where you would not already have one. Another option is see if the maintainer would be willing to make the method LinkWithVarables virtual then we could overload it. Do you think it is worth proposing?

If we can fix it just by changing our interface, I would probably just do that to limit the amount of code we have to write. Whichever you think is cleaner is fine with me though.

Levi-Armstrong commented 3 years ago

@mpowelson I am getting closer but still have two unit tests failing. The first is the numerical ik test which has the same results for several iterations but then they go in different directions so there may be a difference in the trust region solver which I may need a little help tracking down.

Let me know if I can help out.

So I have tracked down one difference. The updateNLPVariableBounds produces different bounds than the original OSQP interface. I am trying to track down how this is done in the original, but if you have time to take look also you may be more familiar with the code.

Levi-Armstrong commented 3 years ago

Well, so it was not an issue with the updateNLPVariableBounds. The issue was I was not setting the bounds on the JointPositionVariable. After fixing the issue in the test I was able to go further comparing the results and found that the new TrustRegionSQPSolver is producing a different result when entering the trust-region loop code. The OSQP Eigen library produces an error when we stepOptimization(nlp) is called a second time within this loop because it calls initSolver a second time after it has already been initialized. After this, it diverges. @mpowelson Do you know how we should address the issue? It is in trust_region_sqp_solver.cpp around line 100.

mpowelson commented 3 years ago

the new TrustRegionSQPSolver is producing a different result when entering the trust-region loop code. The OSQP Eigen library produces an error when we stepOptimization(nlp) is called a second time within this loop because it calls initSolver a second time after it has already been initialized.

I seem to remember I had issues with this as well. I have a comment there about trying to remove the qp_solver->clear(). I guess you need to move all of the clear and all of the qp solver setup inside the loop. Or preferably, figure out why we have to clear the workspace each time and fix that.

Levi-Armstrong commented 3 years ago

@mpowelson Added the absolute cost along with unit tests for the current cost wrappers. I think the only remaining thing on this PR is to figure out how best to handle the error calculations for the different methods used to combine the gradient.

mpowelson commented 3 years ago

@mpowelson Added the absolute cost along with unit tests for the current cost wrappers. I think the only remaining thing on this PR is to figure out how best to handle the error calculations for the different methods used to combine the gradient.

We are trying to reduce a multiple row constraint down to a single row. Thinking about how to reduce these constraints, it seems we currently have 3 options 1) Sum - Makes sense, but it could blow up if you have lots of links (e.g. a dense mesh) 2) Average - Doesn't make sense unless this is absolute value average. You don't want them cancelling out. However, this divides by the number of collisions, so it doesn't blow up when lots of links are in collision at once 3) Max - This also makes sense. I just think it is not the most efficient. It ignores perpendicular axes. If row 1 is only effected by j1 and row 2 is only effected by j2, we should adjust both in the same step. We shouldn't just adjust the row that is bigger.

So I think my intuition is that the average violation (which is >0) is the best default value.

Now, we also square some of them currently. This does make sense in that it will weight bigger violations higher. I think we might consider handling this in its own flag or as a couple of functions (or class?) you pass in. One could imagine having other scaling factors as well such as exponential scaling as well.

Levi-Armstrong commented 3 years ago

@mpowelson I believe this is ready for review. There are a lot of changes to make the collision evaluators work within the new IFOPT framework. The only remaining change is the current constraints are not excluding the results at the previous and post state which I am working on fixing now but should not change much.

Levi-Armstrong commented 3 years ago

@mpowelson The continuous IFOPT constraint is now working and should be ready for you to review.