[Dragon] Implement solver for the nonlinear allocation from control input to the target cog wrench

[tongtybj]

What is this

A strict solver for following optimization problem with a nonlinear allocation:

min  sqart_sum(lambda) 
s.t. w = Q(theta, phi) lambda

w: cog wrench
theta, phi: 2-DoF vectoring angles
lambda: thrust forces

Details

This nonlinear allocation is to get the target control input (lambda, theta, phi) from the target cog wrench w. We provide three different algorithm to solve this problem:

1. Static allocation method: Proposed by this artical, Eq. (10)~ Eq. (17), along with the Alg. 1. This main idea is to interatively solve a static allocation of Eq. (14) ~ Eq. (17) .
2. SQP-based method: solve the above optimiation problem with SQP method. Please check the cost funtion, and the jacobian of the equation constraint (nonlinear allocation)
3. Gradient descent method: solve static allocation once, and the use the jacobian of the nonlinear allocation to iteratively reduce the error of the equation constraints based on gradient descent method. (To be appear in IJRR 2022).

Supplementary Materials

static allocation:

Introduce a three-dimensional force f_i := f(theta_i, phi_i, lambda_i), we then can rewrite the nonlinear allocation as follows:

w = Q_bash F

w: cog wrench
F: the vector of the introduced three-dimensional forces

[tongtybj]

@Takuzumi240

This is the PR about the strict solution for the nonlinear allocation between CoG wrench and the control input

[tongtybj]

@Takuzumi240

mathematical material for derivative. differential_of_nonlinear_wrench_allocation.pdf