[helper]: add_convex_to_convex_constraint code understanding

[Highlight123]

Dear Dr. Thirugnanam I have some code understanding problems. Very disturbing! Thank you very much! dcbf_optimizer.py add_convex_to_convex_constraint function What is the meaning and function of the following codes? line 136 np.dot(robot_G, self.state.rotation().T) line 137 np.dot(np.dot(robot_G, self.state.rotation().T), self.state.translation()) + robot_g line 147 robot_R line 163 robot_T line 167 ca.mtimes((ca.mtimes(mat_A, robot_T) - vec_b).T, lamb[:, i]) line 171 ca.mtimes(ca.mtimes(robot_R.T, mat_A.T), lamb[:, i]) line 174 self.opti.subject_to(ca.mtimes(temp.T, temp) <= 1)

[AkshayThiru]

Hi @Highlight123. Thank you for the question.

I will refer to the arXiv paper in the explanation.

1. The current rotation matrix and translation vector are given by self.state.rotation() and self.state.translation(), respectively. The robot polytope at the origin with no rotation is defined as robot_G @ z <= robot_g. When the robot is rotated and translated, its polytope is robot_G(x) @ z <= robot_g(x), where x is the state, robot_G(x) is given by L136, and robot_g(x) is given by L137. L133-L138 then calculates the distance between obstacle i and the robot. See Eq. (7) and Eq. (8) in the paper.
2. In L147-L162, the variable robot_R is the rotation matrix corresponding to the rotation of the robot, self.variables["x"][3, i + 1], at time i+1. Similarly, robot_T in L163 is the translation vector of the robot at time i+1.
3. L170-L172 and L174 correspond to Eq. (20g) in the paper. L166-L169 is Eq. (22) in the paper (you will also need Eq. (20g) for this line).

Please let us know if any other questions persist.

[Highlight123]

Dear Dr. Thirugnanam Thanks for your help！I have read your paper carefully, "Safety-Critical Control and Planning for Obstacle Avoidance between Polytopes with Control Barrier Functions". outstanding and innovative work! I have another four questions:

1. Page 288 in paper multiple DCBF constraints for each hi , how to implement multiple DCBF constraints in code especially when robot_shape == "lshape"( two or more geometry_regions)?
2. Are self.state.rotation() and self.state.translation() applicable to the rotation of x and y planes of all robot systems?
3. what is the difference between robot_R and robot_T ? What is the application scope of robot_R and robot_T
   respectively?
4. how to calculate b^R(Xt+k|t) in Eq. (22) and A^R(Xt+k|t) in Eq. (20g) ?

[AkshayThiru]

Hi @Highlight123. Thank you for the question.

1. I am not sure I understand the question entirely. I assume you are referring to Eq. (6e) in the paper, where we apply the DCBF constraint for multiple horizons in the MPC formulation. If so, the parameter N_{CBF} in Eq. (6e) in the paper corresponds to the parameter horizon_dcbf. This parameter is independent of the multiple geometry region implementation. To change horizon_dcbf, edit dcbf_optimizer.py#L12. To see how to add multiple geometries to the robot, see kinematic_car.py#L105-L120.
2. Our implementation assumes a single robot and multiple obstacles. Then self.state.rotation() and self.state.translation() apply to the robot. Note that the single robot can be composed of multiple convex shapes, such as the L-shaped robot.
3. robot_R and robot_T are the rotation and translation of the robot at some time in the MPC horizon. Both are optimization variables and represent the predicted values of the rotation and translation.
4. Firstly, A^R and b^R are represented in the code by robot_G and robot_g respectively. Then, robot_G(x_{t+i|t}) = robot_G @ robot_R.T and robot_g(x_{t+i|t}) = robot_G @ robot_R.T @ robot_T + robot_g, where robot_R and robot_T are the rotation and translation variables at time i (as implemented in dcbf_optimizer.py#L147-L162 and dcbf_optimizer.py#L163).

[Highlight123]

Dear Dr. Thirugnanam Thanks for your help！Eq. (9) in the paper and enforcing multiple DCBF constraints for each hi is equivalent to enforcing a single DCBF constraint on h(x) := mini{ hi(x) }, how to implement it in code? Is all hi(x) stored in self.costs["decay_rate_relaxing"] ( as implemented in dcbf_optimizer.py#L176) ?

[AkshayThiru]

The DCBF h_i(x) is computed between each pair of robot geometry and obstacle. In code h_i(x) is given by cbf_curr in L133. We don't store the DCBFs into an array, instead use it directly.

If you want to implement the DCBF constraint on h(x) = min_i {h_i(x)}, you can try the following:

1. Move L133-L138 to before L192 and store the CBF values into an array.
2. Compute the minimum value in this array and use the DCBF constraint function add_convex_to_convex_constraint for the corresponding pair of obstacle and robot geometry.