16 October 2023 Research Development Meeting

[Woolfrey]

Chair:

• Jon @Woolfrey

Attendees:

• Andrea @andrearosasco
• Stefano @steb6
• Simone @SimoneMic
• Vignesh @vigisushrutha23
• Lorenzo @lornat75

Absent:

• N/A

---

Agenda:

Note: Last week's items should become this week's agenda.

Item	Tag	Responsible	Comment
Test CBF for singularity avoidance on personal laptop	#35	@Woolfrey	Still same problems with constraint violation. Further investigation needed.
Try increasing width of virtual corridor and re-test path planning	#123	@vigisushrutha23	Problem with right hip
Follow up bimanual control with improved shoulder tracking.	#124	@Woolfrey @vigisushrutha23	Decent results with latest work.

Additional Discussion Points:

Anything not captured in the agenda.

Discussion Point	Raised By	Tag	Outcome
Implementing class for MPC	@Woolfrey	N/A	Some issues with underactuated system. Need to implement trajectory generator to improve results.

---

Next Meeting:

Note: These will become the agenda for the next meeting.

Item	Tag	Responsible
Test more parameters for singularity avoidance	#35	@Woolfrey
Work on trajectory generator for MPC	N/A	@Woolfrey
Plan for final demonstration	N/A	@Woolfrey
Implement experiments for integration paper	N/A	@vigisushrutha23

[Woolfrey]

Model Predictive Control Class

I'm working on the MPC class for the collaborative paper with Vignesh:

The objective function is:

$$\begin{align} \min_{\mathbf{u}} ~\frac{1}{2}\left(\mathbf{x_\mathrm{d} - \mathrm{x}_\mathrm{0} - Bu}\right)^\mathrm{T}\mathbf{W}\left(\mathbf{x_\mathrm{d} - \mathrm{x}_\mathrm{0} - Bu}\right) \\\ \text{subject to: } \mathbf{u}_\mathrm{min} \le \mathbf{u} \le \mathbf{u}_\mathrm{max} \\\ \frac{\partial\mathrm{h}}{\partial\mathbf{x}}\mathbf{Bu} \ge -\gamma\mathrm{h}(\mathbf{x}) \end{align}$$

and we assume that $\frac{\partial\mathbf{B}}{\partial\mathbf{u}} \approx \mathbf{0}$ so that we can solve the problem with linear least squares.

Some initial problems:

---

1. Setpoint control is not feasible:

Here is the output for a simple control problem with:

$$\mathbf{x}_0 = \begin{bmatrix} 0 & 0 & 0 \end{bmatrix}^\mathrm{T}$$

The state does not converge even through the control input diminishes:

The solver is technically working, but the control matrix is rank-deficient, so the mobile robot cannot move directly to a target point:

$$\mathbf{B(0)} = \begin{bmatrix} \cos(0) & 0 \\ \sin(0) & 0 \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} 1 & 0 \\ 0 & 0 \\ 0 & 1 \end{bmatrix}$$

We need to establish a cubic polynomial trajectory to gradually transition the state from the start to the end:

$$\mathbf{x}(\mathrm{t}) = \mathbf{a + b\mathrm{t} + c\mathrm{t^2} + d\mathrm{t^3}} \Longrightarrow \mathbf{x}(0) = \mathbf{x}_0, \quad \mathbf{x}(\mathrm{t_f}) = \mathbf{x}_\mathrm{d}.$$

---

2. Interior Point Method Fails???

Trying to drive straight in the x-direction returns 0 for some weird reason:

$$\mathbf{x}_0 = \begin{bmatrix} 0 & 0 & 0 \end{bmatrix}^\mathrm{T} \qquad \mathbf{x}_\mathrm{d} = \begin{bmatrix} 1 & 0 & 0 \end{bmatrix}^\mathrm{T}$$

The result is:

But if I removes constraints on the control input:

$$\mathbf{u} = \left(\mathbf{B^\mathrm{T}WB}\right)^{-1}\mathbf{B^\mathrm{T}W}\left(\mathbf{x_\mathrm{d} - x_\mathrm{0}}\right)$$

Then it works:

This might be related to the need for a trajectory generator above. Instead of `direct shooting', it requires a smooth transition across time.