Presentation of Research Summary

[Woolfrey]

We could make some sort of interactive presentation showing the research summary for HSP.

This could be a single presentation slide from each of us with the main research problem/contribution.

[vigisushrutha23]

List of things needed for the ppt for now
@steb6 A step-by-step video of the behavior tree when the actions are recognized + an explanation of the few shot action recognition.
@andrearosasco Info with one picture on the shape completion and a video showing grasp transforms appearing on detected objects.
@Woolfrey Sngle set of equations summarizing the optimization framework + video on ergocub

[Woolfrey]

@vigisushrutha23 , for the video you can check out #39. I'm hoping to get some more/better videos on the coming week.

For the equations there are actually two (maybe 3) stages, but the main part is where I use Gauss' principle of least constraint:

$$\begin{align} \min_{\mathbf{\ddot{q}}_\mathrm{c}}~\frac{1}{2}\left(\mathbf{\ddot{q} - \ddot{q}_\mathrm{c}}\right)^\mathrm{T}\mathbf{M}\left(\mathbf{\ddot{q} - \ddot{q}_\mathrm{c}}\right) \\\ \text{subject to: } \mathbf{J_\mathrm{c}\ddot{q}_\mathrm{c} + \dot{J}_\mathrm{c}\dot{q}_\mathrm{c} = 0} \end{align}$$

where:

• $\mathbf{\ddot{q}}\in\mathbb{R}^{n}$ is the desired joint acceleration for moving the hands,
• $\mathbf{M}\in\mathbb{R}^{n\times n}$ is the inertia matrix, and
• $\mathbf{J_\mathrm{c}\dot{q}_\mathrm{c} = 0}$ is a Pfaffian constraint.

[vigisushrutha23]

Got it. Updated the video. But the equations seem too generic to me. I shall add this on the slide. but maybe you could use like a linear flowchart of equations derivation stages so that when you will be showing the slide to the people interested, it will be useful to help them see how you came about it ? Let me know if you think that would be a good idea.

[vigisushrutha23]

Some screenshots of the two updated slides. More edits to be done

[Woolfrey]

Well, here is the full math:

The inverse dynamics is obtained from a constrained Lagrangian:

$$\begin{align} \mathrm{A = \int_0^t K(\mathbf{q,\dot{q}}) - P(\mathbf{q})~dt} \\\ \text{subject to: } \mathbf{c(q) =0} \end{align}$$

where $\mathbf{c(q) = 0}$ is a holonomic constraint on the hands. The solution is;

$$\boldsymbol{\tau} = \mathbf{M(q)\ddot{q} + h(q,\dot{q}) + J_\mathrm{c}(q)^\mathrm{T}f_\mathrm{c}}$$

where $\mathbf{M}$ is the inertia, $\mathbf{h}$ is Coriolis and gravity, $\mathbf{J_\mathrm{c} = \partial c/\partial q}$, and $\mathbf{f}_\mathrm{c}$ are the grasping forces.

The hand motion $\mathbf{\ddot{x}}$ is related to the object motion $\mathbf{\ddot{x}_\mathrm{o}}$ via the grasp matrix $\mathbf{G}$:

$$\mathbf{\ddot{x}} = \mathbf{G^\mathrm{T}\ddot{x}_\mathrm{o} + \dot{G}^\mathrm{T}\dot{x}_\mathrm{o} = J(q)\ddot{q} + \dot{J}(q,\dot{q})\dot{q}}$$

First I solve the control task for the hands using Gauss' principle:

$$\begin{align} \min_{\mathbf{\ddot{q}}} \frac{1}{2} \left(\mathbf{M^\mathrm{-1}\boldsymbol{\tau}_\mathrm{3} - \ddot{q}}\right)^\mathrm{T}\mathbf{M}\left(\mathbf{M^\mathrm{-1}\boldsymbol{\tau}_\mathrm{3} - \ddot{q}}\right) \\\ \text{subject to: } \mathbf{\ddot{x} - J\ddot{q} - \dot{J}\dot{q} = 0} \end{align}$$

where $\boldsymbol{\tau}_3$ is some third-level redundant task. And the solution is:

$$\boldsymbol{\tau}_\mathrm{2} = \mathbf{M\ddot{q}} = \mathbf{J^\mathrm{T}\boldsymbol{\Lambda}\left(\ddot{x} - \dot{J}\dot{q}\right) + N\mathrm{_M^T}\boldsymbol{\tau}_\mathrm{3}}$$

where $\boldsymbol{\Lambda} = \left(\mathbf{JM^\mathrm{-1}J^\mathrm{T}}\right)^{-1}$ and $\mathbf{N}\mathrm{_M^T} = \mathbf{I - J^\mathrm{T}\boldsymbol{\Lambda}JM^\mathrm{-1}}$.

Then I apply a Pfaffian constraint $\mathbf{J_\mathrm{c}\dot{q} = 0}$ so the hands cannot move apart, again using Gauss' principle:

$$\begin{align} \min_{\mathbf{\ddot{q}}} \frac{1}{2}\left(\mathbf{M^\mathrm{-1}\boldsymbol{\tau}_\mathrm{2} - \ddot{q}}\right)^\mathrm{T}\mathbf{M}\left(\mathbf{M^\mathrm{-1}\boldsymbol{\tau}_\mathrm{2} - \ddot{q}}\right) \\\ \text{subject to: } \mathbf{J_\mathrm{c}\ddot{q} + \dot{J}_\mathrm{c}\dot{q} = 0} \end{align}$$

where I get the solution:

$$\mathbf{M\ddot{q} = N_\mathrm{c}^\mathrm{T}\boldsymbol{\tau}_\mathrm{2} - h_\mathrm{c}}$$

where:

$$\mathbf{h}_\mathrm{c} = \mathbf{J}\mathrm{_c^T}\left(\mathbf{J}_\mathrm{c}\mathbf{M}^{-1}\mathbf{J}\mathrm{_c^T}\right)^{-1}\mathbf{\dot{J}}_\mathrm{c}\mathbf{\dot{q}}$$

and

$$\mathbf{N}\mathrm{_c^T} = \mathbf{I - J\mathrm{_c^T}\left(J_\mathrm{c}M^\mathrm{-1}J\mathrm{_c^T}\right) J_\mathrm{c}M^\mathrm{-1}}.$$

Then we put this all back in to the inverse dynamics to get:

$$\boldsymbol{\tau} = \mathbf{J\mathrm{_c^T}f_\mathrm{c} + N\mathrm{_c^T}\boldsymbol{\tau}_\mathrm{2} + h - h_\mathrm{c}}.$$

I don't think this is all going to fit in to one slide ;)

I think maybe I can go with the constrained dynamics, inverse dynamics showing grasping / constraint forces, then Gauss' principle to solve the manipulation task.

Unfortunately for this kind of problem it's an all-or-nothing explanation...

[Woolfrey]

@vigisushrutha23 maybe slides like this will suffice:

If people want more details they can ask me questions.

ICRA_Slides.pptx

[andrearosasco]

I uploaded videos and screen recordings here.

The grasp is performed well in Take 6. Only problems is that the screen recordings for Take 6 are broken for some reason. Maybe we could get away with editing in a different screen recording.

[xEnVrE]

Hi @andrearosasco, I cannot access them

[andrearosasco]

I should have added you now. @xEnVrE

[vigisushrutha23]

most relevant materials have been uploaded to the teams folders