18 September 2023 Research & Development Meeting

[Woolfrey]

Chair:

• Jon @Woolfrey

Attendees:

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

Absent:

---

Agenda:

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

Item	Tag	Responsible	Comment
Need to fix logic that missed the error with joint commands of incorrect size	N/A	@Woolfrey	Error has been fixed. Robot booked for Tuesday to test demo
Create issue and upload videos of walking controller & navigation	#111	@SimoneMic	Complete.
Continue implementation of ergocub-bimanual branch	N/A	@Woolfrey	Complete
Clean up & commit code on ergocub torso to master branch for use in future demos	N/A	@steb6 @andrearosasco	To be done Tuesday
Fine tuning of parameters for walking & bimanual control	#108	@vigisushrutha23	Integration is complete.

Additional Discussion Points:

Anything not captured in the agenda.

Discussion Point	Raised By	Tag	Outcome
QP solver implemented as independent template class	@Woolfrey	N/A	https://github.com/Woolfrey/SimpleQPSolver
Added option for primal or dual method for QP solver in bimanual control.	@Woolfrey	N/A	(See comment below)
Issues with Gazebo simulation where walking controller stops after 20 steps.	@vigisushrutha23	N/A	Might need to try using a more powerful laptop to get simulation results.
Issue with compliance of left wrist raised with icub tech	@vigisushrutha23	N/A	Awaiting feedback.

---

Next Meeting:

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

Item	Tag	Responsible
Test new singularity avoidance method with ergoCub in Gazebo	#35	@Woolfrey

[Woolfrey]

Primal vs Dual Method for QP

Primal method for a QP problem:

$$\begin{align} \min_{\mathbf{x}} \frac{1}{2}\mathbf{\left( x_\mathrm{d} - x\right)^\mathrm{T} W\left( x_\mathrm{d} - x\right)} \\\ \text{subject to: } \mathbf{Ax = y} \end{align}$$

Lagrangian:

$$\mathrm{L} = \frac{1}{2}\mathbf{x^\mathrm{T}Wx - x^\mathrm{T}Wx_\mathrm{d}} + \left(\mathbf{Ax - y}\right)^\mathrm{T}\boldsymbol{\lambda}$$

where $\boldsymbol{\lambda}$ are the Lagrange multipliers. Optimal solution:

$$\frac{\partial \mathrm{L}}{\partial\mathbf{x}} = \mathbf{0} ~\Longrightarrow \mathbf{x = x_\mathrm{d} - W^\mathrm{-1}A^\mathrm{T}}\boldsymbol{\lambda}$$

But this back in the Lagrangian to get the dual problem:

$$\mathrm{L} = -\frac{1}{2}\boldsymbol{\lambda}^\mathrm{T}\mathbf{AW^\mathrm{-1}A^\mathrm{T}}\boldsymbol{\lambda} + \boldsymbol{\lambda}^\mathrm{T}\left(\mathbf{y - Ax_\mathrm{d}}\right) - \frac{1}{2}\mathbf{x_\mathrm{d}^\mathrm{T}Wx_\mathrm{d}}$$

$dim(\boldsymbol{\lambda}) < dim(\mathbf{x})$, so optimizing over $\boldsymbol{\lambda}$ is faster:

However, it seems that the dual method is conservative in practice:

qp_dual.mp4

The primal method is better at finding a solution within joint limits:

qp_primal.mp4

[Woolfrey]

New ergocub-bimanual repository

https://github.com/Woolfrey/ergocub-bimanual

Cleaned up and removed all obsolete code (iCub2 control code, force/torque related terms).

	icub-bimanual	ergocub-bimanual*	Improvement
No. of header files	7	4	43%
No. of source files	9	6	33%
Lines of code	3,466	2,590	25%

*Additional files now outsourced to SimpleQPSolver.

Tested and working on ergoCub.