Bug for informally calculating safety distance cooperated with margin

[junzengx14]

https://github.com/HybridRobotics/cbf/blob/d8a1b376e7e910de71df60cdf3619f68c40ab3ed/control/dcbf_optimizer.py#L168

Mentioned by Sha, it shall be correctly implemented as follows on the right hand side,

omega_i * param.gamma^i *(cbf_curr - param.margin_dist) + param.margin_dist

I'm agreeing with it, how do you think about it? @AkshayThiru

[AkshayThiru]

I agree with (cbf_curr - param.margin_dist) part. But I think param.gamma should still be raised to (i+1) and not i.

The same change would also apply to this line, I think. https://github.com/HybridRobotics/cbf/blob/d8a1b376e7e910de71df60cdf3619f68c40ab3ed/control/dcbf_optimizer.py#L119

[junzengx14]

@AkshayThiru Agreed with you as mentioned above, not too sure what you meant for "But I think param.gamma should still be raised to (i+1) and not i." Would you mind doing a quick fix and sending a PR for it?

[yswhynot]

Hi Jun and Akshay,

Thank you for following up on this issue. I was mostly just following the equation 20e in the ICRA2022 paper. It's k+1 on the left hand side and k for gamma on the right hand side.

[AkshayThiru]

Hi Yisha,

Thank you for pointing out the bug. For the code, we are using the exponential DCBF controller, which is equation 22, with a constant value of gamma. So I believe param.gamma ** (i+1) is correct.

[yswhynot]

Thank you for confirming!

[yswhynot]

Hi Akshay,

Another small question on this equation. In the left hand side of this equation -ca.mtimes(robot_g.T, mu[:, i]) + ca.mtimes((ca.mtimes(mat_A, robot_T) - vec_b).T, lamb[:, i]), it seems to be using robot poses at time i+1 but lamb and mu at time i. Shouldn't they match with each other (from my understanding all should be i+1 but I might be wrong)?

I know this may not affect the performance much. But I still want to make sure I understand the code and the paper precisely to avoid possible future problems. Thank you again for your help!

[AkshayThiru]

Hi Yisha,

It is just due to array indices starting from 0 (in the paper we assume indices to be starting from 1). We want to enforce the constraint h(x_{i+1}) >= gamma^(i+1) h(x_0) for different values of i >= 0. mu[:, 0] and lamb[:, 0] correspond to the first constraint h(x_1) >= gamma h(x_0), which is why mu[:, i] and lamb[:, i] are associated with x_{i+1} on the LHS.

[yswhynot]

Ohh I get it now. Thank you!!