Problem about the Dual Function and Constraint

[mitsui29]

Hi, @junzengx14 @AkshayThiru, Great works! I have questions about the dual-reformulation eq.10 and constraint ||λA|| ≤ 1. I derived the formula step by step from eq.8. But there's still an extra item -1/4 λ_i^T A_i^T A_i λ_i and no cone-costraint ||λA|| ≤ 1. I checked the convex_optimization from Stephen Boyd. In the chapter 8.2.2 we can find very similar dual formulation which is based on the norm2 distance(not the case in the paper mentioned, square distance form). So, could you please explain for this? Thanks a lot!

Mitsui

[AkshayThiru]

Hi, @mitsui29. Thanks for bringing this up!

You are correct in that the cost term in Eq. (8) should be the 2-norm and not the 2-norm squared. Likewise, there should not be a square in Eqs. (17), (18), (19), and (20e).

However, the code is correctly implemented because we choose the CBF as the distance, not the distance squared: https://github.com/HybridRobotics/cbf/blob/d3412d229cd957a215b2ca562ca505fc4a4a33f8/control/dcbf_optimizer.py#L133-L138 https://github.com/HybridRobotics/cbf/blob/d3412d229cd957a215b2ca562ca505fc4a4a33f8/models/geometry_utils.py#L111

Also, the optimal solutions of the dual problem Eq. (10) and the dual of Eq. (8) with 2-norm squared are related. We explicitly make this transformation in the code: https://github.com/HybridRobotics/cbf/blob/d3412d229cd957a215b2ca562ca505fc4a4a33f8/models/geometry_utils.py#L113-L114

Please let us know if you have any follow-up questions.

[mitsui29]

Dear Dr. Thirugnanam @AkshayThiru,

Thanks for your reply！This is exactly what I was confused about and your reply cleared it up for me. I still have some questions about this work:

1. In every loop the function get_dist_region_to_region will be called to calculate the distance(cbf_curr, lamb_curr, mu_curr), is this optimization problem still computationally intensive?
2. the safety_dist is defined as safety_ratio brake_min_dist + abs(x[2]) timestep + 0.5 amax timestep 2, i wanna ask the logic behind this design. From my opinion, it's enough for safety_dist composing safety_ratio brake_min_dist. If the collision occurs, we can increase safety_ratio and let the robot know the pos of obstacles sooner. What means the second part from abs(x[2]) timestep + 0.5 amax timestep 2?
3. the constraint in eq.(20e): why this part −λ_O(k+1) b_O − λ_R(k+1) bR(x(t+k+1)|t) is implemented in the code like this ca.mtimes((ca.mtimes(mat_A, robot_T) - vec_b).T, lamb[:, i]). I saw the closed issue. But, I can't understand the code here. It's rotation related? I guess, the obstacles are in global frame and the robot is in the robot frame? If we wanna calculate the frame transformation, i mean calculate something in same frame, we should use this for robot part ca.mtimes(robot_g.T, mu[:, i]) from robot frame to global frame? the same question for eq.(20g): ca.mtimes(ca.mtimes(robot_R.T, mat_A.T), lamb[:, i]?.

I’m looking forward to your response. Thanks a lot! Mitsui

[AkshayThiru]

Hi, @mitsui29.

1. The distance and dual variable computation can be performed very quickly (this package is primarily for research and testing purposes, so we have not optimized the distance computation). For a fast solution to the distance problem, you can look at ProxQP. For an even faster method, you can look at HPP-FCL. If you want to use HPP-FCL, you will need to compute the dual optimal solutions from the witness points. Generally, the distance and dual variable computation can be performed in a few microseconds.
2. The extra terms add a 'braking distance' to the original distance CBF. The logic primarily comes from Backup CBF approaches that guarantee the persistent feasibility for the CBF-QP, i.e., you can ensure that collisions never occur (in this case you don't need to tune the safety margin). You can take a look at the following papers to get started: Safety Barrier Certificates for Collisions-Free Multirobot Systems, Backup Control Barrier Functions: Formulation and Comparative Study.
3. Here, we are transforming the obstacle (defined by (mat_A, vec_b) in the global frame) from the global frame to the robot frame. In the robot frame, the obstacle polytope is given by mat_A @ (robot_R @ z + robot_T) <= vec_b, where z is in robot frame. So, the obstacle in the body frame is given by the polytope (mat_A @ robot_R, vec_b - mat_A @ robot_T).

[mitsui29]

Dear Dr. Thirugnanam @AkshayThiru, Thanks a lot!

Mitsui