Woolfrey
commented
1 year ago Here is the stability proof. $\mathbf{l}\in\mathbb{R}^3$ is a vector denoting the line of sight, and $\mathbf{p}\in\mathbb{R}^3$ is a point we want the robot to look at:
Define the error as $$\boldsymbol{\varepsilon} = \mathbf{l\times p}$$ Then the time derivative is:
\begin{align}
\dot{\boldsymbol{\varepsilon}} &= \mathbf{\dot{l}\times p} \\
&= \left(\boldsymbol{\omega}\times\mathbf{l}\right)\times\mathbf{p}
\end{align}Define the angular velocity for the head to be: $$\boldsymbol{\omega} \triangleq k\cdot\boldsymbol{\varepsilon}$$ for some value $k\in\mathbb{R}^+$. Then the time derivative of the error becomes:
\begin{align}
\dot{\boldsymbol{\varepsilon}} &= k\cdot\left(\boldsymbol{\varepsilon}\times\mathbf{l}\right)\times\mathbf{p} \\
&= -k\cdot\boldsymbol{\varepsilon}
\end{align}Therefore: $$\boldsymbol{\varepsilon}(t) = e^{-kt}\boldsymbol{\varepsilon}_0$$ and is asymptotically stable.
andrearosasco
xEnVrE
We need to design a proper gaze control task.
The main requirement is to define what $dx$ should be in
$$ J \dot{q} = dx $$
First proposal
We assume to have the instantaneous z axis of the realsense $\hat{l}(q)$ and the desired pointing direction
$$ d(q) = p^d - o(q) $$
where $p^d$ is the desired gaze point and $o(q)$ is the instantaneous origin of the RealSense camera frame.
In this case, the displacement vector might be defined as:
$$ dx = k \hat{l}(q) \times d(q) $$
Moreover, we should only the three downmost rows of the Jacobian, i.e. those associated to the angular velocity part of the end-effector twist.
We are not sure if this works as when the two directions are aligned, i.e. where the error is zero, the cross product is not well defined.
Second proposal
Note: this requires more work in term of implementation.
We could extend the kinematic chain with an additional fake prismatic joint to be superimposed on the gaze point $p^{d}$.
In this case, given the translational part of the Jacobian of the chan, i.e. the three topmost rows, $J(q)^{p}$, we should append an additional column containing the instantaneous z axis of the realsense $\hat{l}(q)$.
In that case, the displacement vector reduces to
$$ dx = k (p^d - p(q)) $$
where $p(q)$ is the instantaneous position of the end-effector taking into account the prismatic joint.
To implement this solution we would need to "fake" the motion of the prismatic joint in software using an "integrator".
We should understand if it is possible to add a "fake" link to the model using iDynTree APIs so that the evaluation of the augmented Jacobian and of the forward kinematics come for free.