# How to formulate tracking pose control as an optimization problem

Let $$q\in\mathbb{R}^n,x\in\mathbb{R}^m,J(q)\in\mathbb{R}^{m\times n}$$, the differential kinematic equation is $$\dot{x} = J(q) \dot{q} \tag{1}$$ To formulate the control task as an optimization problem, we need an objective function which in this case is obtained directly from Eq(1) (i.e. $$\dot{x} - J(q) \dot{q} =0$$); therefore, we get $$\min_{\dot{q}} \| \dot{x} - J(q) \dot{q} \|$$ My question is how can I choose the objective function so that $$x\rightarrow x_d,\dot{x}\rightarrow \dot{x} _d$$ as $$t\rightarrow \infty$$, with $$\dot{q}$$ is chosen as the control input (i.e. $$\dot{q} \triangleq u$$). I'm aware of the Jacobian pseudoinverse control scheme which selects the control input as $$u = J^\dagger (\dot{x}_d + K(x_d-x))$$ which if I plug it in Eq(1), I get the error dynamic equation as \begin{align} \dot{x} - J \dot{q} &= 0,\\ \dot{x} - J (J^\dagger (\dot{x}_d + K(x_d-x))) &= 0,\\ (\dot{x} - \dot{x}_d) + K(x-x_d) &= 0,\\ \dot{e} + Ke &= 0. \tag{2} \end{align} Eq(2) provides me with some hints. My attempt to formulate the tracking pose task as an optimization problem is considering Eq(1),Eq(2), $$\min_{u} \| Ju - \dot{x}_d + Ke \|$$ Is this correct objective function for the tracking pose problem?

• As far as I can tell that looks correct. You'd probably want to be careful with how you compute the orientation error, as its not exactly $\mathbf{x} - \mathbf{x}_{d}$. But in general, the idea looks good to me. Jan 24 at 17:12