Fixup TOTG #3427
Fixup TOTG #3427
Conversation
max_velocity and max_acceleration need to have same size as waypoint vectors, i.e. group->getVariableCount()
rhaschke
requested review from
henningkayser
and removed request for
tylerjw and
mlautman
|
@AndyZe, I noticed that the unit test is rather slow: it takes more than a minute to finish the single time parameterization test on my side. I have seen that your code essentially iteratively adapts the acceleration bounds based on observed torques. Looks like many iterations are required to converge. Did you analyze the behavior? How can the performance be further improved? |
|
I didn't notice that but I'll test it when I review your code. It should be limited to 10 iterations and they should be very fast since TOTG and the fwd dynamics are all very fast.
|
|
Looks like I tested with a DEBUG build. With a release build, the time is down to 7s, which is still some time but maybe acceptable. |
|
I know what the problem is: This should be a shorter distance. There are a ton of waypoints right now. |
|
Indeed, reducing the amount of joint motion brings the test duration down to 3ms. Is this ready to merge? |
lol. Reviewing now. |
moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
Show resolved
Hide resolved
moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
Outdated
Show resolved
Hide resolved
| "<joint name=\"joint_a\"/>" | ||
| "</group>" | ||
| "</robot>"; | ||
| const std::string srdf = R"(<?xml version="1.0" ?> |
There was a problem hiding this comment.
Is this R necessary? Seems strange:
R"(<?xml version="1.0" ?>
There was a problem hiding this comment.
Read about raw string literals (since C++11).
moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp
Outdated
Show resolved
Hide resolved
Codecov ReportPatch coverage:
Additional details and impacted files@@ Coverage Diff @@
## master #3427 +/- ##
==========================================
+ Coverage 61.69% 61.74% +0.05%
==========================================
Files 385 385
Lines 34095 34035 -60
==========================================
- Hits 21033 21012 -21
+ Misses 13062 13023 -39
☔ View full report in Codecov by Sentry. |
| <joint name="joint_a"/> | ||
| <joint name="joint_b"/> | ||
| </group> | ||
| "</robot>)"; |
There was a problem hiding this comment.
Looks like an extra quote here?
There was a problem hiding this comment.
Well spotted. Wasn't noticed, because the extra quote goes as text in the XML and is simply ignored.
This fixes several issues introduced in #3412. Replaces #3426.
Apart from a lot of code redundancy introduced in #3412, this PR also fixes a severe bug introduced there:
Because the TOTG algorithm considers all group variables (including mimic joints), the lookup of velocity and acceleration limits cannot be restricted to active joint variables only. Differently sized vectors might lead to segfaults!
To be honest, I am somewhat disappointed about the poor quality of PRs and reviews from approved maintainers in the last few months. Please improve!