Support controller parameters tuning pipeline

[xiangfuli]

The tuner is built based on the sensitivity propagation optimization, the related paper can be found in here.

To use the tuner, four components must be defined by the user:

1. The dynamic model
2. The linear/non-linear controller
3. The desired states for the model
4. The function to get the error between desired states and current states

[aerogjy]

First round of review, please see the detailed review in the code. Several major issues: 1. Use LieTensor rather than re-write all the conversion by yourself. 2.Compute the partial derivative for u_k to accommodate the general loss L instead of only for x_k for sensitivity propogation. 3. Re-organize the modules , put the controller and tuner to modules rather than examples or optim. 4. Documentation should have all the core algorithms and equations with properly cited papers. 5. Some coding style really needs improved. Let's fix these before further review.

[xiangfuli]

According to the comments, several revises have been made:

• Use the skew2vec, vec2skew function provided in the pypose. But still keeping the quaternion_2_rotation_matrix and rotation_matrix_2_quaternion function since pypose does not provide such function.
• Add the controller inputs $u_k$ as the penalty in the loss function
• Have moved the controllers: dubincar controller and geometric controller to the directory pypose.module.controllers, also have moved ControllerParametersTuner into the directory pypose.module
• Parts of the codes have been polished.

ToDo:

• Working on the documentation.

[xiangfuli]

Documentation has been updated on Wednesday.

[xiangfuli]

@wang-chen Hello Chen, In this PR, I am trying to implement the function skew2vec(also mentioned in the #255) in the file pypose/lietensor/basics.py. When I was trying to implement the corresponding unit tests, there is no test_basics.py under the test folder.

Should I also create the unit test for the skew2vec function? Thanks.

[wang-chen]

Yes, you may do that. Thank you!

[xiangfuli]

ToDo:

• Add simple result plots in the new dynamic system examples.
• Add result plots in the tuner examples. （Finished 07.09.23)

[aerogjy]

I can successfully run all the examples and the test files. Several major issues: 1. bad file organization in the pypose/module. 2. The controller is too problem specific. We are expecting the general PID controller, etc so that people can pass through different problem (dynamics). 3. Many of the API design are not good. Please think about it from the user perspective. 4. Some of the coding style is really not ideal. Please refer to our developer guideline SERIOUSLY and CAREFULLY.

[aerogjy]

The API design is not very idea. Think about when someone is initiating a controller, what argments you need to define a controller? some parameters like PID gains, right? So I suggest put the controller parameters in the init, put the current state and ref_state in the get_control.

[aerogjy]

The following update rule doesn't tell you are projecting the new parameters into the feasible set. It's just the normal gradient descent.

[xiangfuli]

Setting the pid parameters as the PID controller's internal variables would cause some inconvenience when calculating the gradient inside the parameter tuner.

We use the torch.autograd.functional.jacobian function provided by the PyTorch to get the gradient. When calculating the gradient of controller output w.r.t. the controller parameters, we have to let the controller parameters used as the function input parameters like this: dhdparam_func = lambda params: controller.get_control(parameters = params, state = system_state, ....). Then use the jacobin function to get the gradient. jacobian(dhdparam_func, controller_parameters).

If we set pid parameters as PID's internal properties, we cannot set the controller_parameters as the input like this jacobian(dhdparam_func, controller_parameters) which causes us cannot use the jacobian function anymore.

There is one way to get the gradients properly but it looks much more complex than using the jacobian function and needs to modify the existing tuner codes a lot.

Here is the code example (just for showing the new way to get the gradient if set the controller parameters as PID's internal parameters):

#!/usr/bin/python3
import torch
from torch.autograd.functional import jacobian

class PID:
    def __init__(self, params) -> None:
        self.params = params
        self.params.requires_grad=True

    def get_control(self, state):
        return state + self.params * 2

parameters = torch.ones(4, requires_grad=True)
pid = PID(parameters)

print("Gradient compution using parameters tensors inside the pid controller: ")
dhdx_func = lambda state: pid.get_control(state)
dhdparam_func = lambda params: pid.get_control(state = torch.ones(4) * 3.5)
print(jacobian(dhdparam_func, parameters))

print("Gradient compution using undetached parameters tensors: ")
new_state = pid.get_control(state = torch.ones(4) * 3.5)
new_state[0].backward(retain_graph=True)
print(pid.params.grad)
new_state[1].backward(retain_graph=True)
print(pid.params.grad)


print("Gradient compution using detached parameters tensors: ")
new_param = torch.clone(parameters).detach()
pid.params = new_param
pid.params.requires_grad=True
new_state0 = pid.get_control(state = torch.ones(4) * 3.5)
new_state0[0].backward(retain_graph=True)
print(pid.params.grad)
new_param = torch.clone(parameters).detach()
pid.params = new_param
pid.params.requires_grad=True
new_state1 = pid.get_control(state = torch.ones(4) * 3.5)
new_state1[1].backward(retain_graph=True)
print(pid.params.grad)


print("Gradient compution using autograd.grad ")
new_parameters = torch.ones(4, requires_grad=True)
new_pid = PID(new_parameters)
new_state = new_pid.get_control(state = torch.ones(4) * 3.5)
for i in range(0, len(new_state)):
  print(torch.autograd.grad(new_state[i], new_pid.params, retain_graph=True))