Fix mppi bidirectional settings #4954
Conversation
Signed-off-by: mini-1235 <[email protected]>
Codecov ReportAll modified and coverable lines are covered by tests ✅
|
| float max_delta_vx = model_dt_ * control_constraints_.ax_max; | ||
| float min_delta_vx = model_dt_ * control_constraints_.ax_min; | ||
|
|
||
| float max_delta_vx = 0.0; |
There was a problem hiding this comment.
Assigning here is meaningless, we have moved it into the for loop, I can do it like this or float max_delta_vx; if you prefer
There was a problem hiding this comment.
Agreed its meaningless, but vy and wz didn't have this change, so we should make it consistent. Remove all or revert the vx ones please
| } else { | ||
| max_delta_vx = -control_constraints_.ax_min * model_dt_; | ||
| min_delta_vx = control_constraints_.ax_min * model_dt_; | ||
| } |
There was a problem hiding this comment.
I think this is also missing for Y direction as well
There was a problem hiding this comment.
I didn't see the ay_min as well as az_min at first, so I only deal with the X direction, are we assuming that ay_min = - ay_max?
There was a problem hiding this comment.
It looks like ay_min is accidentally missing and should be added. If ax_max is max acceleration and ax_min is max deceleration, then the Y axis should have the same as another translational motion axis.
The max acceleration should be applied for moving full-left and full-right (with appropriate sign changes to accomodate); while the deceleration is applied to slowing down movements both left and right.
Accelerating forward is ax_max, decelerating forward is ax_min. Accelerating in reverse is -ax_max and decelerating in reverse is -ax_min.
There was a problem hiding this comment.
So, I think that really this should only be looking at the sign of vx_last to know what to populate for min_delta_vx and max_delta_vx. I don't see why a comparison with cvx_curr or cvx_curr * vx_last >= 0.0 is necessary here. Does that make sense?
Yes, the sign of vx_last should be enough to determine the delta values.
There was a problem hiding this comment.
It looks like ay_min is accidentally missing and should be added.
When I was working on adding constraints, there was some discussion around it, as I remember.
#4352 (comment)
#4352 (comment)
| // Decelerating otherwise | ||
| if (abs(cvx_curr) >= abs(vx_last) && cvx_curr * vx_last >= 0.0) { | ||
| max_delta_vx = control_constraints_.ax_max * model_dt_; | ||
| min_delta_vx = -control_constraints_.ax_max * model_dt_; |
There was a problem hiding this comment.
Shouldn't this be ax_min not max? We are setting the bounds of max acceleration, and max deceleration when we're currently accelerating.
I think this should just be
min_delta_vx = control_constraints_.ax_min * model_dt_;
(the usual)
Ditto issue in the else condition. I think that should be the one inverting the signs and/or the value positions
There was a problem hiding this comment.
I am actually following the convention here
navigation2/nav2_velocity_smoother/src/velocity_smoother.cpp
Lines 263 to 268 in 443915c
shouldn't we stay consistent for that?
There was a problem hiding this comment.
Let me look over this again, admittedly I'm a bit sick and may have just misinterpreted this
There was a problem hiding this comment.
Writing this out as part of the process. Accelerations min/max are signed (i.e. a_max is positive, a_min is negative).
I see the following cases, centered around how we find the viable bounds to place on cvx_curr for what is dynamically feasible. We start with our current velocity from the last iteration, vx_last and we want to find the values above and below it to threshold current control step cvx_curr for:
- Moving forward, meaning
vx_lastwhich is positive
- +ax_max * model_dt for maximum forward velocity possible while accelerating (numerically highest value)
- +ax_min * model_dt for minimum forward velocity possible while decelerating (numerically lowest value)
- Thus we clamp in the low-high range of (vx_last + ax_min * model_dt, vx_last + ax_max * model_dt)
- Moving in reverse, meaning
vx_lastwhich is negative
- -ax_max * model_dt for maximum reverse velocity possible while accelerating (numerically lowest value)
- -ax_min * model_dt for minimum reverse velocity possible while decelerating (numerically highest value)
- Thus we clamp in the low-high range of (vx_last - ax_max * model_dt, vx_last - ax_min * model_dt)
If the robot is moving forward but the next velocity is in reverse due to random sampling, that should cover the scope as the bounds are already placed.
So, I think that really this should only be looking at the sign of vx_last to know what to populate for min_delta_vx and max_delta_vx. I don't see why a comparison with cvx_curr or cvx_curr * vx_last >= 0.0 is necessary here. Does that make sense?
I believe what is done in the velocity smoother is different. It is putting the bounds on the acceleration part of the term to allow in the velocity smoother between iterations when we have already identified if we're accelerating or decelerating. Note the dv is v_cmd - v_curr and we're clamping on the accelerating part of the term, not the full velocity (which v_curr is added - right @doisyg?
| @@ -86,6 +87,15 @@ class MotionModel | |||
| for (unsigned int j = 0; j != n_rows; j++) { | |||
| float vx_last = state.vx(j, i - 1); | |||
| float & cvx_curr = state.cvx(j, i - 1); | |||
| // Accelerating if magnitude of cvx is greater than vx_last and they have the same sign | |||
| // Decelerating otherwise | |||
There was a problem hiding this comment.
I don't believe this is true. There are four potential states:
- Accelerating forward
- Decelerating forward
- Accelerating in reverse
- Decelerating in reverse
I don't think this properly handles all 4 cases.
@Ayush1285 do you mind taking a look / reviewing this as well? Your recent changes touch this part of code as well
Basic Info
Description of contribution in a few bullet points
Description of documentation updates required from your changes
None
Description of how this change was tested
Tested on sim, wrote some new unit tests
Future work that may be required in bullet points
Maybe more unit tests? Looking for some suggestions before proceeding
For Maintainers: