No! It is differentiable, but it zeros when \(\|s_e(q)-s_d\|^2\) is an integer. Therefore, it zeros, for example, when \(\|s_e(q)-s_d\|^2 = 1\) (i.e., when the distance between the current and desired positions is 1).

Back

No! If \(r_T(q)\) is a task function for \(\mathcal{T}\), then it is not difficult to see that \(r_1(q)=r_T(q)\) and \(r_2(q)=-r_T(q)\) are task functions for \(\mathcal{T}\).

But \(r_1(q)+r_2(q)\) is the zero vector. Therefore, it is not a task function for \(\mathcal{T}\).

Back

Yes! First, if \(f(q)\) is differentiable, \(\|f(q)\|^2\) is also differentiable. Furthermore, the sum of differentiable functions is differentiable. Therefore, \(r_3(q)\) is differentiable.

Moreover, \(r_3(q) = 0\) if and only if \(r_1(q)\) and \(r_2(q)\) are zero. Since \(r_1,r_2\) are task functions for \(\mathcal{T}\), both are zero if and only if \(q \in \mathcal{T}\).

Back

No. In fact, \(\det(Q(q)-Q_d) = 0\) for any rotation matrices \(Q\) and \(Q_d\).

To see this, use (i) to write \(r = \det((QQ_d^T - I_{3 \times 3})Q_d\). Using (ii) and the fact that \(\det(Q_d)=1\), we have \(r = \det(QQ_d^T- I_{3 \times 3})\). Note that \(R = QQ_d\) is also a rotation matrix, and therefore (using (iii)) always has an eigenvalue of 1. Therefore, \(\det(R-\lambda I_{3 \times 3}) = 0\) for \(\lambda=1\). This shows that \(r=0\).

Back

Yes. Note that \(\|M\|_F^2 = 0\) if and only if the matrix \(M\) is the zero matrix.

So \(r(q) = 0\) if and only if \(I_{3 \times 3} - Q(q)Q_d^T=0_{3 \times 3}\). Manipulating this equation, it is equivalent to the equation \(Q(q)=Q_d\). This shows that \(r(q)=0\) if and only if \(q \in \mathcal{T}\).

Since \(Q(q)\) is a differentiable function in \(q\), \(\|M\|_F^2\) is a differentiable function in the components \(M\) and the composition of differentiable functions is also differentiable, \(r(q)\) is also differentiable.

Back

Using \(\|w\|^2 = w^Tw\) and differentiating \(r(q,t)\) with respect to time \(t\), we have:

$$\frac{d}{dt}r(q,t) = 2(s_e(q){-}s_d(t))^T\left(\frac{d}{dt}s_e(q){-}\frac{d}{dt}s_d(t)\right) .$$

Since \(\frac{d}{dt}s_e(q) = J_v(q) \dot{q}\), we have, after separating the terms:

$$\frac{d}{dt}r(q,t) = 2(s_e(q){-}s_d(t))^TJ_v(q) \dot{q} -2(s_e(q){-}s_d(t))^T\dot{s}_d(t).$$

Comparing with the result of the chain rule, \(\dot{r} = J_r\dot{q} + \frac{\partial r}{\partial t}\), we have:

$$J_r(q,t) = 2(s_e(q){-}s_d(t))^TJ_v(q) \ \ , \ \ \frac{\partial r}{\partial t}(q,t) = -2(s_e(q){-}s_d(t))^T\dot{s}_d(t).$$

Back

Solving the equation \(\dot{r} = -Kr\), we have \(r(q(t)) = e^{-Kt}r(q(0))\).

Since \(r = s_e-s_d\), this implies \(s_e = s_d + e^{-Kt}(s_{e0}-s_{d})\) where \(s_{e0} = s_e(q(0))\).

Although \(p(t) = s_e(q(t))\) is a nonlinear function of \(t\), when we plot this expression parameterized in time, we have a straight line in the space \(p \in \mathbb{R}^3\). One way to see this is that the velocity vector of the trajectory, \(\frac{d}{dt} p = -Ke^{-Kt}(s_{e0}-s_{d})\), is a vector with constant direction and sense (although not magnitude), since \(Ke^{-Kt}\) is a scalar.

Back

Considering the equation \(\frac{dr}{dt} = -K\sqrt{r}\), we can solve it by separating the variable (since \(r\) is a scalar).

Thus, \(\int_0^t \frac{dr(s)}{\sqrt{r(s)}} = \int_0^t(-Kds)\). And therefore:

$$(2\sqrt{r(s)}) |_{s=0}^{s=t} = (-Ks) |_{s=0}^{s=t} \ \rightarrow \ 2\sqrt{r} - 2\sqrt{r_0} = -Kt.$$

Thus, \(\sqrt{r(t)} = \sqrt{r_0} - \frac{K}{2}t\). Since \(\sqrt{r} \geq 0\), this equation is valid for \(t \leq 2\frac{\sqrt{r_0}}{K}\). For \(t > \frac{\sqrt{r_0}}{K}\), by continuity, \( \sqrt{r(t)} = 0\). Thus:

$$r(q(t)) = \left\{ \begin{array}{ll} (\sqrt{r_0} - \frac{K}{2}t)^2 & \mbox{if } t \leq 2\frac{\sqrt{r_0}}{K} \\ 0 & \mbox{if } t > 2\frac{\sqrt{r_0}}{K} \end{array} \right.$$

Then, \(r(q(t)\) falls with a parabola until \(r=0\) and then remains constant at \(0\).

The convergence time is therefore \(t_{con} = 2\frac{\sqrt{r_0}}{K}\).

Back

Let \(F(r) = (F_1 \ \ F_2 \ \ F_3)^T\). We note that \(J_r(q) = J_v(q)\). Using the expression of the geometric Jacobian obtained in Lecture 3, we have that the equation \(J_r(q)^TF = 0_{3 \times 1}\) can be written as:

$$\LARGE{\Big(}\normalsize{}\begin{array}{ccc} -s_1{-}s_{12} & 0 & c_1{+}c_{12} \\ -s_{12} & 0 & c_{12} \end{array}\LARGE{\Big)}\normalsize{}\LARGE{\Bigg(}\normalsize{}\begin{array}{c} F_1 \\ F_2 \\ F_3 \end{array}\LARGE{\Bigg)}\normalsize{} = \LARGE{\Big(}\normalsize{}\begin{array}{c} 0 \\ 0 \end{array}\LARGE{\Big)}\normalsize{}.$$

Note that the variable \(F_2\) is irrelevant. The previous equation is therefore equivalent to the \(2 \times 2\) system:

$$\LARGE{\Big(}\normalsize{}\begin{array}{cc} -s_1{-}s_{12} & c_1{+}c_{12} \\ -s_{12} & c_{12} \end{array}\LARGE{\Big)}\normalsize{}\LARGE{\Big(}\normalsize{}\begin{array}{c} F_1 \\ F_3 \end{array}\LARGE{\Big)}\normalsize{} = \LARGE{\Big(}\normalsize{}\begin{array}{c} 0 \\ 0 \end{array}\LARGE{\Big)}\normalsize{}.$$

We are interested in solutions where \(r(q) \not= 0_{3 \times 1}\). Since \(F(r)=-r\), this means that we are interested in solutions where \(F \not= 0_{3 \times 1}\). Given the previous system, there are only non-zero solutions for \((F_1 \ \ F_3)^T\) (and therefore for \(F\)) if and only if the determinant of the 2x2 matrix is 0.

Calculating the determinant and using the sine addition rule, we find that this occurs when \(\sin(q_2)=0\). Using \(q_2=0\) and substituting into the previous 2x2 equation, as well as using the value of \(F(r)=-r\), \(r=s_e-s_d\) and the robot's forward kinematics (which allows us to calculate \(s_e\)), we have:

$$\LARGE{\Big(}\normalsize{}\begin{array}{cc} -2s_1 & 2c_1 \\ -s_1 & c_1 \end{array}\LARGE{\Big)}\normalsize{}\LARGE{\Big(}\normalsize{}\begin{array}{c} -2c_1 \\ -(2s_1-1) \end{array}\LARGE{\Big)}\normalsize{} = \LARGE{\Big(}\normalsize{}\begin{array}{c} 0 \\ 0 \end{array}\LARGE{\Big)}\normalsize{}.$$

Developing the equation, we conclude that it has a solution if and only if \(\cos(q_1)=0\).

Therefore, we have two configurations where the controller fails: \(q_A = (\frac{\pi}{2} \ \ 0)^T\) and \(q_B = (-\frac{\pi}{2} \ \ 0)^T\). That is, if the robot is fully stretched vertically (up or down).

If we start exactly in one of these configurations, the controller will fail. However, if we run the simulation, we will find that any small perturbation will cause the robot to converge to the target.

Back