RoboJackets
Velocity Profile bug
Description
The velocity profile code doesn't effectively constrain the acceleration. the output velocity profile has points where acceleration is much higher (4x maybe more?) than motionConstraints.maxAcceleration
Steps to reproduce
paste this into RRTPlanner.cpp generateVelocityPath() function at the end. right before the return statement.
for(int i = 1; i < entries.size(); i++) { double accel = (entries[i].vel.linear() - entries[i-1].vel.linear()).mag() / RJ::Seconds(entries[i].time - entries[i-1].time).count(); assert(accel < motionConstraints.maxAcceleration*2); }
Also paste this into the CMakeLists.txt to view the acceleration (it seems to get optimized out otherwise :| )
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -O0") set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -O0")
Result: the assertion fails as soon as Basic122 is started
- [ ] This bug has been reproduced by someone else
Additional information
I think this happens because we assume tangential acceleration and normal acceleration are constant in oneStepLimitAcceleration(). but they aren't.
When fixing this it might be worth adding in the constraints on wheel acceleration instead of world acceleration since we would already be in the land of annoying differential eqns
@egordon9dev the reason we haven't done that in the past is that at the point of that pass, you don't have angle information. Unless that's changed (which would require you to do the velocity profile pass multiple times or plan 3d xyh paths instead of 2d paths in the bezier fitting step).
