How to add new --psi_stop ( angle car when stop)

[esprtk]

Hello Sir. First i want to say thank you so much for shared this project. This is helping me alot in my research and learn about automous car. My question is how (or where) to add angle car when stop beside X_Y Stop argument ? I want to make the car can deal with any angle of parking.

[amirhosseinh77]

Hi there,

I'm glad you found this repo helpful. It's important to note that the controller currently operates based on the desired x and y coordinates. To incorporate the psi angle into the control, you'll need to modify the controller accordingly.

Here is the MPC:

        for i in range(self.horiz):
            state_dot = mpc_car.move(u_k[0,i], u_k[1,i])
            **mpc_car.update_state(state_dot)**

            z_k[:,i] = [mpc_car.x, mpc_car.y]
            cost += np.sum(self.R@(u_k[:,i]**2))
            cost += np.sum(self.Q@((desired_state[:,i]-z_k[:,i])**2))
            if i < (self.horiz-1):     
                cost += np.sum(self.Rd@((u_k[:,i+1] - u_k[:,i])**2))
        return cost

So you need to optimize the last step based on z_k[:,i] = [mpc_car.x, mpc_car.y, mpc_car.psi] or to have all the psi for the trajectory which is hard to achieve.

There are other changes needed as well, but this modification is the most critical.

[esprtk]

Hello Sir. Thank you so much for your guide . I will try to change the code and try it. Thank you again for fast respond.