Fixed Control law error in LQRController

[Haricharan1212]

Bugs:

• The control law used was $u = -K y$, while instead it should've been $u = - K \Delta y$.

Changes Made:

• __init__ takes moment_of_inertia as a parameter for added functionality
• The function set_goal previously had no functionality, now it sets the goal of the controller and recomputes $A$, $K$, and $S$ matrices
• The control law is changed to $u = -K (y - y_\text{goal})$

Testing:

• I tested these changes in our TMotors setup, and they are functional

[shivesh1210]

Thanks for the PR. Delta y is not needed because we perform a change of coordinates around that configuration. If you choose to do so, you have to also correct your L133 where the expression on LHS should be Delta y S Delta y.

[Haricharan1212]

It is updated now

[Haricharan1212]

Sir,

I have updated the function now. The class is now more modular and has more features. Please look into merging the changes.

Regards, Haricharan B, Second Year Undergraduate, Indian Institute of Technology, Madras

On Mon, Dec 26, 2022 at 12:05 AM Shivesh Kumar @.***> wrote:

Thanks for the PR. Delta y is not needed because we perform a change of coordinates around that configuration. If you choose to do so, you have to also correct your L133 where the expression on LHS should be Delta y S Delta y.

— Reply to this email directly, view it on GitHub https://github.com/dfki-ric-underactuated-lab/torque_limited_simple_pendulum/pull/24#issuecomment-1364722711, or unsubscribe https://github.com/notifications/unsubscribe-auth/AWI4KLTT5KE3DT2ROV7KC53WPCHY5ANCNFSM6AAAAAATI24PHE . You are receiving this because you authored the thread.Message ID: <dfki-ric-underactuated-lab/torque_limited_simple_pendulum/pull/24/c1364722711 @github.com>

[fwiebe]

Hi @Haricharan1212 Thanks for your PR. I have the following comments to your changes:

1. I think there is a mistake in the A matrix. If you introduce inertia and evaluate the matrix at an arbitrary goal, I think the lower left entry should be

   -self.mass*self.g*self.len / self.moment_of_inertia*np.cos(self.goal[0])

   instead of:

   -self.g * np.cos(self.goal[0])/(2 * self.len)

2. The angle wrapping is missing. The position error should be wrapped so that there is no discontinuity around the goal position. The angle should be wrapped like

   delta_pos = pos - self.goal[0] delta_pos_wrapped = (delta_pos + np.pi) % (2*np.pi) - np.pi

3. I would propose to rename the variable moment_of_inertia to simply inertia. This is then consistent with the other controllers and the pendulum plant.

If you can address these comments, I will gladly merge your PR.

Best Regards

[Haricharan1212]

Hi,

I think I've addressed the above comments.

Best Regards