How to add terminal condition constraints in `dcbf_optimizer.py`

[Highlight123]

Dear Dr. Thirugnanam: I have problems in adding terminal condition constraints in dcbf optimizer such as the desired heading angle at goal position. Very disturbing! Thank you very much!

[AkshayThiru]

Hi @Highlight123. Thank you for the question.

We did not implement terminal constraints in the dcbf optimizer as it can lead to infeasibility. We have the parameter terminal_weight https://github.com/HybridRobotics/cbf/blob/d3412d229cd957a215b2ca562ca505fc4a4a33f8/control/dcbf_optimizer.py#L20 that adjusts how closely the actual trajectory follows the reference trajectory. So to impose soft terminal constraints, you can adjust the parameters and cost matrices here: https://github.com/HybridRobotics/cbf/blob/d3412d229cd957a215b2ca562ca505fc4a4a33f8/control/dcbf_optimizer.py#L79-L81

However, if you want hard terminal constraints, you can write a function similar to add_initial_condition_constraint https://github.com/HybridRobotics/cbf/blob/d3412d229cd957a215b2ca562ca505fc4a4a33f8/control/dcbf_optimizer.py#L38-L39 and calling the function in the setup function. https://github.com/HybridRobotics/cbf/blob/d3412d229cd957a215b2ca562ca505fc4a4a33f8/control/dcbf_optimizer.py#L203

Please let us know if you have additional questions.

[Highlight123]

Thanks for your help！I have added hard terminal constraints on the desired heading angle at goal position as your suggested. def add_end_condition_constraint(self, param): self.opti.subject_to(self.variables["x"][:, param.horizon] == np.transpose(np.array([1.875, 0.075, 0, 0]))) and self.add_end_condition_constraint(param) in the setup function. Error: RuntimeError: Error in Opti::solve [OptiNode] at .../casadi/core/optistack.cpp:159: .../casadi/core/optistack_internal.cpp:999: Assertion "return_success(accept_limit)" failed: Solver failed. You may use opti.debug.value to investigate the latest values of variables. return_status is 'Maximum_Iterations_Exceeded'

Is this error caused by this terminal constraint not working in every param.horizon? This terminal constraint may be added only close to the terminal time. what is the connection between the whole navigation_time and param.horizon? How to end simulation once feasible trajectory is obtained or self._robot._system._time >= navigation_time?(cbf/sim/simulation.py Line 69)

[AkshayThiru]

The problem is due to the infeasibility of the MPC optimization close to the terminal time. This might be happening because of the following possible reasons: 1) the terminal constraint was added too early, 2) the MPC horizon param.horizon is not long enough, or 3) the terminal state is not reachable at all. Adding the terminal constraint only close to the terminal time should be fine for the solver. I recommend adding the terminal constraint as a soft constraint by adding a terminal cost, as discussed in my previous comment. However, if you would like to continue using a hard terminal constraint, I suggest increasing the horizon length param.horizon or adding the terminal constraint later than you currently do.

The navigation time used in the SingleAgentSimulation class is mostly unrelated to the MPC horizon length param.horizon.

We currently don't have the functionality to stop the simulation once the terminal state is achieved. You could implement this by adding a function in the Robot class, https://github.com/HybridRobotics/cbf/blob/d3412d229cd957a215b2ca562ca505fc4a4a33f8/sim/simulation.py#L23 to check if the current position is close to the goal position.

Please let us know if you have more questions.

[Highlight123]

Dear Dr. Thirugnanam: Thanks for your continued help ! Terminal condition constraints can be solved well as your suggestions. I want to improve warm start to accelerate calculation. 1. Why self.add_warm_start(param, system) is not from self._robot.run_global_planner or self._robot.run_local_planner() in simulation.py? 2. how to choose warm start in add_obstacle_avoidance_constraint ( Line 178- Line 180) ? 3. What is the connection between self.add_warm_start(param, system) and the warm start ( Line 178- Line 180) ?

[AkshayThiru]

Hi @Highlight123. It's good to hear you solved your terminal constraints issue.

1. The trajectories obtained from the global and local planners are not dynamically feasible. This is why they are not used as the warm start solution. Instead, we use a braking controller for the warm start to ensure that the optimized trajectory is safe, even if it is not optimal.
2. The warm start solution for the dual variables is chosen as the initial value of dual variables. You could try other solutions, such as setting them equal to zero. But I'm not sure how this will affect the solution time.
3. The two warm starts are for different optimization variables. The warm start for the dual variables is written in the function of definition for convenience.