We know how to describe the pose of the end-effector relative to a reference frame \(\mathcal{F}_0\).

Forward kinematics maps the "position" of the joints (configuration) to the "position" of the rigid body (pose).

We are now interested in mapping not the "positions" but the "velocities". This is the goal of differential kinematics.

Before proceeding, we need to introduce an important concept:

The name "S" comes from skew-symmetric, which is the English term for "anti-symmetric".

This matrix has interesting properties:

There is a profound relationship between the \(S\) operator/function and time-varying rotation matrices:

By the theorem, \( S\big(\omega(t)\big) = \dot{Q}(t)Q(t)^T \).

Consider the following rotation (rotation around the \(z\) axis by an angle \(2t\)). Calculate \(\omega(t)\):

$$Q(t) = \Large{\Bigg(}\normalsize{}\begin{array}{ccc} \cos(2t) & -\sin(2t) & 0 \\ \sin(2t) & \cos(2t) & 0 \\ 0 & 0 & 1 \end{array}\Large{\Bigg)}\normalsize{}. $$

So:

$$\dot{Q}(t) = \Large{\Bigg(}\normalsize{}\begin{array}{ccc} -2\sin(2t) & -2\cos(2t) & 0 \\ 2\cos(2t) & -2\sin(2t) & 0 \\ 0 & 0 & 0 \end{array}\Large{\Bigg)}\normalsize{}. $$

Using the property that \(\cos(\theta)^2+\sin(\theta)^2=1\):

$$\dot{Q}(t)Q(t)^T = \Large{\Bigg(}\normalsize{}\begin{array}{ccc} 0 & -2 & 0 \\ 2 & 0 & 0 \\ 0 & 0 & 0 \end{array}\Large{\Bigg)}\normalsize{}. $$

From the definition of \(S\), we can extract that:

$$\omega(t) = \Large{\Bigg(}\normalsize{}\begin{array}{c} 0 \\ 0 \\ 2 \end{array}\Large{\Bigg)}\normalsize{}. $$

In this example, the vector \(\omega(t)\) is constant, but this is not always the case, as we will see.

Consider the following rotation (a complex rotation). Calculate \(\omega(t)\):

$$Q(t) = \Large{\Bigg(}\normalsize{}\begin{array}{ccc} \cos(2t) & -\sin(4t)/2 & \sin(2t)^2 \\ \sin(2t) & \cos(2t)^2 & -\sin(4t)/2 \\ 0 & \sin(2t) & \cos(2t) \end{array}\Large{\Bigg)}\normalsize{}. $$

With a bit of trigonometry, we can show that:

$$\dot{Q}(t)Q(t)^T = \Large{\Bigg(}\normalsize{}\begin{array}{ccc} 0 & -2 & 2\sin(2t) \\ 2 & 0 & -2\cos(2t) \\ -2\sin(2t) & 2\cos(2t) & 0 \end{array}\Large{\Bigg)}\normalsize{}. $$

Therefore, using the definition of the \(S\) operator, we can see that:

$$\omega(t) = \Large{\Bigg(}\normalsize{}\begin{array}{c} 2\cos(2t) \\ 2\sin(2t) \\ 2 \end{array}\Large{\Bigg)}\normalsize{}. $$

Now the vector \(\omega(t)\) is not constant.

The vector \(\omega(t)\) has an important interpretation in mechanics, as we will see in due course.

In UAIBot we use the function Utils.S to calculate the \(S\) matrix of a vector.

import numpy as np
import uaibot as ub

print(ub.Utils.S([1,2,3]))
                        

Now we will learn how to describe the velocity of rigid bodies, without yet being linked to the context of robotic manipulators.

A particle in 3D space with position vector \(p(t)\) written in a reference frame \(\mathcal{F}_0\) has its velocity described by the vector \(v(t) = \dot{p}(t)\), another 3D vector.

A velocity description for rigid bodies must contain the information necessary to describe the velocity of all the particles that make up a rigid body.

Consider a rigid body with a reference frame \(\mathcal{F}_{obj}\) attached to it.

Important fact: every particle \(P\) that makes up this rigid body has its coordinates described in the reference frame \(\mathcal{F}_{obj}\) by a three-dimensional vector constant \(p^{P}_{obj}\).

This vector is, in a way, an "identity card" of the particle, as it is unique for each particle and, regardless of the movement of the rigid body, this vector will be constant over time for each particle.

Let \(H_{0}^{obj}(t)\) be the HTM that represents the pose of the object relative to a reference frame \(\mathcal{F}_0\) at time \(t\).

We can extract from it the rotation matrices \(Q(t)\) and translation vector \(s(t)\).

Then, the position of the particle \(P\) in the reference frame \(\mathcal{F}_{0}\), \(p^{P}_0(t)\), is time-varying, and is given by:

$$p^{P}_0(t) = Q(t)p^{P}_{obj} + s(t)$$

We just apply the rigid transformation corresponding to the HTM \(H_{0}^{obj}(t)\) to its local description.

Let's differentiate both sides of this equation with respect to time:

$$\dot{p}^{P}_0(t) = \dot{Q}(t)p^{P}_{obj} + \dot{s}(t).$$

Using the theorem that relates the derivative of the rotation matrix to the \(S\) matrix:

$$\dot{p}^{P}_0(t) = S\big(\omega(t)\big)Q(t)p^{P}_{obj} + \dot{s}(t).$$

But \(Q(t)p^{P}_{obj} = p^{P}_0(t)-s(t)\), so:

$$\dot{p}^{P}_0(t) = S\big(\omega(t)\big)\big(p^{P}_0(t)-s(t)\big) + \dot{s}(t).$$

Using Property 2 of the \(S\) matrix, we arrive at a very important result:

$$\dot{p}^{P}_0(t) = \omega(t) \times \big(p^{P}_0(t)-s(t)\big) + \dot{s}(t).$$

You have probably seen a simplified version of this formula in high school...

A disk of radius \(R\) rotates clockwise with constant angular velocity \(\Omega\) while moving to the right with constant linear velocity \(V\). Calculate the magnitude of the instantaneous velocity vector of particle \(P\) at the time shown on the side.

The velocity vector of the particle is the result of the sum of two components (vectors): one due to the velocity of the translating disk, a vector to the right, and another due to the rotation of the disk, which, at the instant shown on the side, at the particle in question, has the same direction and sense as the velocity vector due to translation.

The magnitude of the velocity vector due to translation is V.

The magnitude of the velocity vector due to rotation is \(\Omega R\).

Since the two vectors are in the same direction and sense, the magnitude of the resulting velocity vector is: $$\Omega R + V$$

Compare the two formulas:

$$\Omega R + V$$

$$ \omega \times \big(p^{P}_0-s\big) + \dot{s}$$

The second is a generalization of the first!

• The vector \(\omega\) plays the role of the scalar \(\Omega\), and is called the angular velocity vector;
• The vector \(\dot{s}\) plays the role of the scalar \(V\), and is called the linear velocity vector, we will use the notation \(v\);
• The vector \(p^{P}_0-s\) plays the role of the scalar \(R\), and is the distance vector between the position of the particle and the center of the object's reference frame.

All these quantities are measured with respect to the reference frame \(\mathcal{F}_0\)!

Thus, the formula for the velocity vector of particle \(P\), measured in \(\mathcal{F}_0\):

$$v^P_0 = \omega \times \big(p^{P}_0-s\big) + v$$

can be used in generic situations, with even the three elements varying over time.

The formula:

$$v^P_0 = \omega \times \big(p^{P}_0-s\big) + v$$

makes it clear that to characterize the velocity of any particle, we need the linear velocity \(v(t)\) and angular velocity \(w(t)\) of the rigid body at the instant of time \(t\).

Important: note that these two vectors are properties of the rigid body as a whole, and not of separate particles. Thus, it does not make sense to say "linear velocity of the particle" or "angular velocity of the particle". The particle has its velocity vector, calculated from \(v\), \(\omega\) and the distance vector using the formula above.

How to calculate the vectors \(v(t)\) and \(\omega(t)\) of a rigid body?

Simple, in possession of its motion description as an HTM, \(H_{0}^{obj}(t)\), we extract the respective rotation \(Q(t)\) and translation \(s(t)\).

The linear velocity is \(v(t) = \dot{s}(t)\).

The angular velocity is found by the equation \(S(\omega(t)) = \dot{Q}(t)Q(t)^T\).

I repeat that the two vectors will be measured in the reference frame \(\mathcal{F}_0\).

The angular velocity vector does not depend on how the object's reference frame, \(\mathcal{F}_{obj}\), is attached to it.

The linear velocity depends on how this is done.

In the two animations below, we have a rigid body with the same motion, but with the object's reference frames attached differently. You can observe that the angular velocity is the same, but the linear velocity changes...

In any case, the formula for the velocity of a particle in the body will give the same result, regardless of how the reference frame is attached.

Example: the point at the center of the parallelepiped in the example below has zero velocity in both cases...

Interpreting the linear velocity vector \(v\) is not very difficult.

However, the angular velocity vector \(\omega\) is a bit more complex.

We can think of it as follows:

• Its direction and sense form an "instantaneous axis of rotation" around the center of the object's reference frame.
• Its magnitude gives the instantaneous rotation speed (in rad/s, for example) at that moment.

So, if a rigid body rotates around a single axis all the time with a constant speed \(\Omega\) (see the previous example, on the right), its angular velocity vector is a constant vector with the direction and sense of the axis of rotation and intensity \(\Omega\).

Finally, we can solve our differential kinematics problem.

The goal is to calculate the \(v\) and \(\omega\) (written in the reference frame \(\mathcal{F}_0\)) of the end-effector at a given instant of time.

As mentioned in forward kinematics, since all the movement of the manipulator is the result of a respective movement in its joints, that is, in its configuration \(q\), it must be possible to relate the configuration velocity \(\dot{q}=\frac{d}{dt}q\) with \(v\) and \(\omega\).

In fact, although it may seem a bit counterintuitive, we also need to know what the current configuration \(q\) is for this calculation. In other words, in general, providing only the instantaneous configuration velocity at a moment is not enough to calculate \(v\) and \(\omega\).

We are looking for functions \(g_v\) and \(g_{\omega}\) such that

$$v = g_v(q,\dot{q}),$$

$$\omega = g_\omega(q,\dot{q}).$$

Let's get a feeling/intuition of how this is possible with an example.

In the forward kinematics class, we calculated the forward kinematics for the robot on the side.

We concluded that the vector \(s\), the position of the center of the end-effector's reference frame relative to the reference frame, \(\mathcal{F}_0\), is:

$$s(q) = \Large{\Bigg(}\normalsize{}\begin{array}{c} 0.5\cos(q_1)+0.5\cos(q_1+q_2) \\ 0 \\ 0.5\sin(q_1)+0.5\sin(q_1+q_2) \end{array}\Large{\Bigg)}\normalsize{}.$$

As we have seen, to calculate the linear velocity \(v\), we just differentiate \(s\) with respect to time.

We have to use the chain rule, because we will differentiate with respect to \(t\), \(s=s(q)\) depends on \(q\) and the configuration \(q\) depends on \(t\), \(q=q(t)\):

$$v = \frac{d}{dt} s(q) = \Large{\Bigg(}\normalsize{}\begin{array}{c} -0.5\sin(q_1)\dot{q}_1-0.5\sin(q_1+q_2)(\dot{q}_1+\dot{q}_2) \\ 0 \\ 0.5\cos(q_1)\dot{q}_1+0.5\cos(q_1+q_2)(\dot{q}_1+\dot{q}_2) \end{array}\Large{\Bigg)}\normalsize{}.$$

Using the notation \(c_1,s_1,c_{12},s_{12}\) already used previously, we can write the last equation in a matrix form:

$$v =\Large{\Bigg(}\normalsize{}\begin{array}{cc} -0.5s_1-0.5s_{12} & -0.5s_{12} \\ 0 & 0 \\ 0.5c_1+0.5c_{12} & 0.5c_{12} \end{array}\Large{\Bigg)}\normalsize{} \Large{\Big(}\normalsize{}\begin{array}{c} \dot{q}_1 \\ \dot{q}_2 \end{array}\Large{\Big)}\normalsize{}.$$

Let's call this \(3 \times 2\) matrix \(J_v(q)\). Note that it depends only on \(q\) (not on \(\dot{q}\)) and that it is a nonlinear function of \(q\).

With this notation:

$$v = J_v(q)\dot{q}.$$

Now let's move on to the angular velocity. We remember that the rotation matrix corresponding to the HTM of the forward kinematics is:

$$ Q(q) = \LARGE{\Bigg(}\normalsize{}\begin{array}{ccc} \cos(q_1+q_2) & 0 & -\sin(q_1+q_2) \\ 0 & 1 & 0 \\ \sin(q_1+q_2) & 0 & \cos(q_1+q_2) \end{array}\LARGE{\Bigg)}\normalsize{}$$

So:

$$ \frac{d}{dt} Q(q) = \LARGE{\Bigg(}\normalsize{}\begin{array}{ccc} -\sin(q_1{+}q_2)(\dot{q}_1{+}\dot{q}_2) & 0 & -\cos(q_1{+}q_2)(\dot{q}_1{+}\dot{q}_2). \\ 0 & 0 & 0 \\ \cos(q_1{+}q_2)(\dot{q}_1{+}\dot{q}_2) & 0 & \sin(q_1{+}q_2)(\dot{q}_1{+}\dot{q}_2) \end{array}\LARGE{\Bigg)}\normalsize{}.$$

We use the relation \(S(\omega) = (\frac{d}{dt} Q(q))Q(q)^T\). With a bit of trigonometry, we have that:

$$ S(\omega) = \LARGE{\Bigg(}\normalsize{}\begin{array}{ccc} 0 & 0 & -(\dot{q}_1+\dot{q}_2) \\ 0 & 0 & 0 \\ (\dot{q}_1+\dot{q}_2) & 0 & 0 \end{array}\LARGE{\Bigg)}\normalsize{}.$$

And therefore:

$$\omega = \Large{\Bigg(}\normalsize{}\begin{array}{c} 0 \\ -(\dot{q}_1+\dot{q}_2) \\ 0 \end{array}\Large{\Bigg)}\normalsize{}.$$

We can rewrite the last equation as:

$$\omega =\Large{\Bigg(}\normalsize{}\begin{array}{cc} 0 & 0 \\ -1 & -1 \\ 0 & 0 \end{array}\Large{\Bigg)}\normalsize{} \Large{\Big(}\normalsize{}\begin{array}{c} \dot{q}_1 \\ \dot{q}_2 \end{array}\Large{\Big)}\normalsize{}.$$

Let's call this \(3 \times 2\) matrix \(J_\omega(q)\). Note that it does not depend on \(\dot{q}\) or \(q\), but there is no mathematical error in saying that it is a (constant) function of \(q\)...

With this notation:

$$\omega = J_\omega(q)\dot{q}.$$

Suppose that at a given instant of time we have \(q_1=\frac{\pi}{4}\), \(q_2=0\) and \(\dot{q}_1=\dot{q}_2=1 rad/s\).

Using the two formulas:

$$ v = \Large{\Bigg(}\normalsize{}\begin{array}{c} -\sqrt{2} \\ 0 \\ \sqrt{2} \end{array}\Large{\Bigg)}\normalsize{} m/s \ , \ \omega = \Large{\Bigg(}\normalsize{}\begin{array}{c} 0 \\ -2 \\ 0 \end{array}\Large{\Bigg)}\normalsize{} rad/s.$$

Both vectors are written in the reference frame \(\mathcal{F}_0\).

See the linear velocity vector on the side, in yellow, and observe that it is consistent with what we would expect in terms of instantaneous linear velocity for the end-effector in this situation.

And the end-effector will rotate 2rad/s around the \(y\) axis in the counterclockwise direction (negative direction of \(y\), in this case).

Let's calculate the same relationship for the second example from the previous class...

The procedure is the same. We start by differentiating \(s\) with respect to time to obtain:

$$v = \frac{d}{dt} s(q) = \Large{\Bigg(}\normalsize{}\begin{array}{c} -0.5s_1\dot{q}_1{-}(0.25{+}q_3)s_{12}(\dot{q}_1{+}\dot{q}_2)+c_{12}\dot{q}_3 \\ 0 \\ 0.5c_1\dot{q}_1{-}(0.25{+}q_3)c_{12}(\dot{q}_1{+}\dot{q}_2)+s_{12}\dot{q}_3 \end{array}\Large{\Bigg)}\normalsize{}.$$

As in the previous example, we can rewrite it in a matrix form:

$$v =\Large{\Bigg(}\normalsize{}\begin{array}{ccc} -0.5s_1{-}(0.25{+}q_3)s_{12} & -(0.25{+}q_3)s_{12} & c_{12} \\ 0 & 0 & 0 \\ 0.5c_1{+}(0.25{+}q_3)c_{12} & (0.25{+}q_3)c_{12} & s_{12} \end{array}\Large{\Bigg)}\normalsize{} \Large{\Bigg(}\normalsize{}\begin{array}{c} \dot{q}_1 \\ \dot{q}_2 \\ \dot{q}_3 \end{array}\Large{\Bigg)}\normalsize{}.$$

Let's call this \(3 \times 3\) matrix \(J_v(q)\). Note that it depends only on \(q\) (not on \(\dot{q}\)) and that it is a nonlinear function of \(q\).

With this notation:

$$v = J_v(q)\dot{q}.$$

The angular velocity is the same as in the previous example, being independent of the third configuration:

$$\omega =\Large{\Bigg(}\normalsize{}\begin{array}{ccc} 0 & 0 & 0 \\ -1 & -1 & 0\\ 0 & 0 & 0\end{array}\Large{\Bigg)}\normalsize{} \Large{\Bigg(}\normalsize{}\begin{array}{c} \dot{q}_1 \\ \dot{q}_2 \\ \dot{q}_3 \end{array}\Large{\Bigg)}\normalsize{}.$$

Let's call this \(3 \times 3\) matrix \(J_\omega(q)\). Note that it does not depend on \(\dot{q}\) or \(q\), but there is no mathematical error in saying that it is a (constant) function of \(q\)...

With this notation:

$$\omega = J_\omega(q)\dot{q}.$$

So, for both examples, there are \(3 \times n\) matrices \(J_v(q)\) and \(J_{\omega}(q)\) (where \(n\) is the number of joints in the respective example) such that:

$$v = J_v(q)\dot{q},$$

$$\omega = J_\omega(q)\dot{q}.$$

Is something similar true for a general manipulator?

Yes! in the general case for a manipulator with \(n\) joints, we can show that there are \(3 \times n\) matrices \(J_v(q)\), \(J_{\omega}(q)\), generally nonlinear functions of \(q\), such that:

$$v = J_v(q)\dot{q},$$

$$\omega = J_\omega(q)\dot{q}.$$

When we "stack" the two matrices into a single \(6 \times n\) matrix, we have the (very important) Geometric Jacobian:

$$J_{geo}(q) = \left(\begin{array}{c} J_v(q) \\ J_{\omega}(q) \end{array}\right).$$

From the equations:

$$v = g_v(q,\dot{q}) = J_v(q)\dot{q},$$

$$\omega = g_\omega(q,\dot{q}) = J_\omega(q)\dot{q}.$$

we can conclude that the two velocities are (generally) nonlinear functions of the current configuration, \(q\), but always linear in \(\dot{q}\).

There is a "recipe" for constructing the geometric Jacobian of the end-effector's reference frame using the Denavit-Hartenberg convention.

For this, we use the matrices \(H_0^e(q)\) (HTM from the reference frame \(\mathcal{F}_0\) to \(\mathcal{F}_e\)) and \(H_0^{DHi}(q) = H_0^{DH0} H_{DH0}^{DHi}(q)\) (HTM from the reference frame \(\mathcal{F}_0\) to \(\mathcal{F}_{DHi}\)).

Let \(s_{DHi}(q)\) and \(z_{DHi}(q)\) be the translation and \(z\) axis of the HTM \(H_0^{DHi}(q)\), respectively (last and penultimate column of the matrices, respectively, without the last element).

Let \(s_{e}(q)\) be the translation of the HTM \(H_0^{e}(q)\).

We will assemble the two matrices column-by-column, for \(j=1,2,...,n\):

In UAIBot we use the function .jac_geo to calculate the geometric Jacobian.

import numpy as np
import uaibot as ub

robot = ub.Robot.create_kuka_kr5()
#Calculate the geometric Jacobian for a given configuration
#The HTM of the forward kinematics (fk) comes as a by-product
Jg, fk = robot.jac_geo(q=[0,1,-1,2,3,4])
#If we do not specify the configuration, it assumes the current one (robot.q)
Jg, fk = robot.jac_geo()
#Assuming the robot has an instantaneous velocity of 1 rad/s for each joint in the current configuration, let's calculate the linear and angular velocity vectors
qdot = np.matrix([1,1,1,1,1,1]).reshape((6,1)) #It has to be a column vector
v = Jg[0:3,:] * qdot
w = Jg[3:6,:] * qdot
                        

The geometric Jacobian matrix in a configuration \(q\) allows us to perform a series of interesting analyses about the manipulability of the manipulator in that configuration.

Essentially, it can tell us how difficult (or if it is impossible) to perform a given movement in that configuration \(q\).

The first question is: given that we are in a configuration \(q\), is it possible to perform (simultaneously) any desired linear velocity \(v_{d}\) and angular velocity \(\omega_d\) by manipulating the joint velocity \(\dot{q}\)?

From a mathematical point of view, we want to know if the linear system of equations:

has a solution \(\dot{q}\) for any vector \(\xi_d \in \mathbb{R}^6\).

A vector with the linear and angular velocity "stacked", like \(\xi_d\), is simply called velocity.

Let's recall a bit of Linear Algebra:

Let \(A\) be a matrix with \(m\) rows and \(n\) columns. We want to know if for every \(b \in \mathbb{R}^m\) the equation \(Au=b\) has a solution \(u \in \mathbb{R}^n\).

To answer this question, we need to remember the concept of rank of a matrix:

Some results:

Another important definition:

Other important definitions:

Note: it is called "\(\texttt{Ker}\)" because in English we call it "Kernel".

We have an important result:

Therefore, the equation \(J_{geo}(q)\dot{q} = \xi_d\) has a solution for any velocity \(\xi_d \in \mathbb{R}^6\) if and only if \(n \geq 6\) (the robot has at least 6 joints) and \(J_{geo}(q)\) has full rank (in this case, 6).

Furthermore, Theorem 4 tells us that the smaller the rank of \(J_{geo}(q)\), the smaller the dimension of achievable velocities \(\xi_d\) in the configuration \(q\), that is, where it is possible to choose \(\dot{q}\) that gives us this velocity \(\xi_d\).

Therefore, the space of achievable velocities in the configuration \(q\) is \(\texttt{Im}(J_{geo}(q))\).

Considering a robot with at least 6 joints, we say that a configuration \(q\) is singular if the rank of \(J_{geo}(q)\) is not full.

In a singular configuration, it is not possible to perform any \(v_d, \omega_d\) velocity, which limits the robot's movement.

These are difficult configurations to work with, and control engineers should avoid them whenever possible. The reason will become clearer when we discuss robot control in more detail.

If a manipulator cannot perform any velocity \(\xi_d\) in a configuration \(q\), which velocities can it not perform in that configuration?

To find some infeasible velocities, we can find vectors \(\xi_d\) such that \(J_{geo}(q)^T\xi_d = 0_{n \times 1}\).

Theorem 7 provides ways to find some velocities that are not feasible, but there are infeasible velocities that do not satisfy \(J_{geo}(q)^T\xi_d = 0_{n \times 1}\).

The space of infeasible velocities is not a vector space!

Considering only exactly singular configurations as problematic is not practical.

In practice, there are configurations \(q\) that are not exactly singular, but are practically singular, because to perform a velocity \(\xi_d\) we need an instantaneous joint velocity \(\dot{q}\) that is extremely high (and therefore infeasible, in practice).

These are configurations where the matrix \(J_{geo}(q)\) has singular values very close to zero, but not exactly zero.

Due to the continuity of the matrix \(J_{geo}(q)\) in the configuration \(q\), near-singular configurations are configurations close to singular configurations.

Consider the Kuka KR5 robot on the side. The configuration on the side (which is its initial configuration) is singular.

It cannot perform the vectors (in cyan and yellow, respectively) \(v_d = (1.78 \ 0 \ 0)^T\) and \(\omega_d = (0 \ 0 \ 1)^T\) simultaneously, for example.

There are many others that it cannot perform...

In fact, if \(h_{ne}\) is an infeasible velocity and \(h_{e}\) is feasible, then \(h_{ne}+h_{e}\) is not feasible.

Let's now perform an analysis that is, in a way, complementary to the reachability analysis.

Suppose that in a configuration \(q\) there is a non-zero vector \(\dot{q}\) such that \(J_{geo}(q)\dot{q} = 0_{6 \times 1}\).

This means that there is an "instantaneous motion" (relative to the instantaneous joint velocity \(\dot{q}\)) that, although it moves parts of the robot, keeps the end-effector exactly as it is!

Therefore, the space of joint velocities that are innocuous to the end-effector in the configuration \(q\) is \(\texttt{Ker}(J_{geo}(q))\).

If the robot has more than 6 joints, for every configuration \(q\) there are non-zero vectors \(\dot{q}\) such that \(J_{geo}(q)\dot{q} = 0_{6 \times 1}\).

See the KUKA LBR IIWA on the side, which has 7 joints. It can always make movements that do not change the pose of the end-effector...

In summary, for a robot with \(n\) joints:

• The space of achievable velocities in the configuration \(q\) is \(\texttt{Im}(J_{geo}(q))\), and has dimension rank(\(J_{geo}(q)\))...
• The space of joint velocities that are innocuous to the end-effector in the configuration \(q\) is \(\texttt{Ker}(J_{geo}(q))\), and has dimension \(n\)-rank(\(J_{geo}(q)\))...

There are also interpretations for \(\texttt{Im}(J_{geo}(q)^T)\) and \(\texttt{Ker}(J_{geo}(q)^T)\), but they will be seen at another opportunity.

We are now interested in actually solving the equation

given a configuration \(q\) and a desired velocity \(\xi_d\).

This is the problem of Inverse Differential Kinematics!

This problem, and similar problems, is a fundamental piece for most control strategies of manipulators.

Only one of three possibilities can happen with a linear system \(Au=b\) of the form:

• ...it has a unique solution...
• ...it has infinitely many solutions...
• ...it has no solution.

It cannot happen that it has, for example, exactly 4 solutions (and no more)!

It is reasonable to think the following:

• If \(Au=b\) has only one solution, we want to find it.
• If \(Au=b\) has infinitely many solutions, we want to find the best one according to some criterion.
• If \(Au=b\) has no solution, we want to find the vector \(u\) that best approximates solving the equation.

Let's formalize the concept of "best" in the second and third situations.

There are several ways to formalize the concept of "best", but the following way is very convenient mathematically:

• If \(Au=b\) has only one solution, we want to find it.
• If \(Au=b\) has infinitely many solutions, we want to find the one that minimizes \(\|u\|^2\) (solution with the smallest norm).
• If \(Au=b\) has no solution, we want to find the one that minimizes \(\|Au-b\|^2\) (best solution in terms of least squares).

What we are looking for is an algorithm that considers all three cases and obtains the respective solution.

There is an elegant way to implement this solution. Consider the following optimization problem:

We will now convince ourselves that solving this problem for a very small, but positive \(\epsilon\) (\(\lim_{\epsilon \rightarrow +0}\)) is equivalent to implementing the three-part solution we wanted...

The "trick" is to observe that the objective function \(\|Au-b\|^2 + \epsilon \|u\|^2\) weighs two objectives, which may be conflicting: minimizing \(\|Au-b\|^2\) and minimizing \(\|u\|^2\).

Considering a very small \(\epsilon\), minimizing \(\|Au-b\|^2\) has much more priority than minimizing \(\|u\|^2\), and the second term will only be considered if we can completely minimize \(\|Au-b\|^2\), that is, if \(Au=b\).

• If \(Au=b\) has only one solution, we will completely minimize \(\|Au-b\|^2\) without worrying about minimizing \(\|u\|^2\). The result will be the unique \(u\) that solves \(Au=b\).
• If we have infinitely many solutions, we will completely minimize \(\|Au-b\|^2\). Among those \(u\) where \(Au=b\), the problem will choose the one that minimizes the lower priority objective function, \(\|u\|^2\), just as we wanted.
• If we have none, the problem will only minimize \(\|Au-b\|^2\) as best as possible, without worrying about minimizing \(\|u\|^2\).

To be precise, the third case has a slightly more complicated analysis, as there may be infinitely many \(u\) that minimize \(\|Au-b\|^2\) without having \(Au=b\). In this case, the second objective function acts again.

The optimization problem:

can be solved analytically by taking the gradient of the objective function with respect to the variable \(u\) and setting it to zero.

The solution is:

We can show that, for \(\epsilon > 0\), the matrix \(A^TA + \epsilon I_{n \times n}\) is always invertible.

Let's introduce the notation:

Then, our solution, with \(\epsilon \rightarrow +0\), is:

$$u = \lim_{\epsilon \rightarrow +0} A^{\dagger(\epsilon)} b = \left(\lim_{\epsilon \rightarrow +0} A^{\dagger(\epsilon)}\right)b.$$

The matrix \(A^{\dagger} = \left(\lim_{\epsilon \rightarrow +0} A^{\dagger(\epsilon)}\right)\) is called the pseudoinverse of the matrix \(A\), while \(A^{\dagger(\epsilon)}\) is called the damped pseudoinverse of the matrix \(A\).

Finally, we have our solution:

Unfortunately, this solution is not very good.

When \(A\) is close to being singular, \(A^{\dagger}\), and therefore \(u\), tends to "explode" (have very large values).

This is not practical when our \(u\) represents the joint velocity, for example.

For this reason, we work with the damped pseudoinverse, \(u = A^{\dagger(\epsilon)}b\), with a small, but not zero, \(\epsilon\).

We have a trade-off between our original desire for \(u\) and having a more reasonable solution (with smaller values) for the problem.

Given a target velocity in the configuration \(q\) and at time \(t\), \(\xi_d(q,t)\), and a damping factor \(\epsilon\), we can calculate the respective joint velocity:

If the robot has an interface to send the joint velocity, we can use this equation to perform control!

The challenge is precisely to choose \(\xi_d\) that will allow us to perform the desired task.

In UAIBot we use the function Utils.dp_inv to calculate the damped pseudoinverse:

import numpy as np
import uaibot as ub

robot = ub.Robot.create_kuka_kr5()
#Calculate the geometric Jacobian for the current configuration
Jg, fk = robot.jac_geo()
#Define the desired velocity
xi_d = np.matrix([1,0,0,0,0,0]).reshape((6,1))
#Calculate the joint velocity
eps=0.001
qdot = ub.Utils.dp_inv(Jg, eps) * xi_d
                        

The name comes from the fact that in English we call the matrix damped pseudoinverse.

Let's make the end-effector go up and down, without changing its orientation. To do this, just choose something like \(v_d(t) = (0 \ 0 \ 0.2\cos(t))^T\) and \(\omega_d(t) = (0 \ 0 \ 0)^T\).

import numpy as np
import uaibot as ub

robot = ub.Robot.create_kuka_kr5()
sim = ub.Simulation([robot])
#Parameters
dt=0.01
eps=0.001

for i in range(2000): #Simulate for 20 seconds
    #Calculate the discrete time from the continuous time
    t = i*dt
    #Calculate the desired velocity
    xi_d = np.matrix([0,0,0.2*np.cos(t),0,0,0]).reshape((6,1))
    #Calculate the geometric Jacobian (in the current configuration)
    Jg, fk = robot.jac_geo()
    #Calculate what the joint velocity should be
    qdot = ub.Utils.dp_inv(Jg, eps) * xi_d
    #Here, we simulate the robot's movement according to this instantaneous joint velocity.
    #We use a first-order Euler integration:
    #next configuration = current configuration + joint velocity *dt
    qprox = robot.q + qdot * dt
    robot.add_ani_frame(t, q = qprox)

sim.run()




                        

With the geometric Jacobian, we can map velocities to joint velocities.

This is very useful because it is often easier to plan control in velocity, but the robot receives the command in joint velocity. This mapping solves the problem.

In the next class, we will see that the geometric Jacobian will be used as the main ingredient for calculating other Jacobians, related to a space where planning velocities is even simpler than in the velocity space.

In this space, we will plan our controller.