Mechanical subjects: robotics control

ROBOT CONTROL JACOBIANS, VELOCITY, AND FORCE

Jacobian matrices are a super useful tool, and heavily used throughout robotics and control theory. Basically, a Jacobian defines the dynamic relationship between two different representations of a system. For example, if we have a 2-link robotic arm, there are two obvious ways to describe its current position: 1) the end-effector position and orientation (which we will denote $\textbf{x}$ ), and 2) as the set of joint angles (which we will denote $\textbf{q}$ ). The Jacobian for this system relates how movement of the elements of $\textbf{q}$ causes movement of the elements of $\textbf{x}$ . You can think of a Jacobian as a transform matrix for velocity.
Formally, a Jacobian is a set of partial differential equations:

$\textbf{J} = \frac{\partial \textbf{x}}{\partial \textbf{q}}$ .

With a bit of manipulation we can get a neat result:

$\textbf{J} = \frac{\partial \textbf{x}}{\partial t} \; \frac{\partial t}{\partial \textbf{q}} \rightarrow \frac{\partial \textbf{x}}{\partial \textbf{t}} = \textbf{J} \frac{\partial \textbf{q}}{\partial t}$ ,

$\dot{\textbf{x}} = \textbf{J} \; \dot{\textbf{q}}$ ,

where $\dot{\textbf{x}}$ and $\dot{\textbf{q}}$ represent the time derivatives of $\textbf{x}$ and $\textbf{q}$ . This tells us that the end-effector velocity is equal to the Jacobian, $\textbf{J}$ , multiplied by the joint angle velocity.
Why is this important? Well, this goes back to our desire to control in operational (or task) space. We’re interested in planning a trajectory in a different space than the one that we can control directly. Iin our robot arm, control is effected through a set of motors that apply torque to the joint angles, BUT what we’d like is to plan our trajectory in terms of end-effector position (and possibly orientation), generating control signals in terms of forces to apply in $(x,y,z)$ space. Jacobians allow us a direct way to calculate what the control signal is in the space that we control (torques), given a control signal in one we don’t (end-effector forces). The above equivalence is a first step along the path to operational space control. As just mentioned, though, what we’re really interested in isn’t relating velocities, but forces. How can we do this?
Energy equivalence and Jacobians
Conservation of energy is a property of all physical systems where the amount of energy expended is the same no matter how the system in question is being represented. The planar two-link robot arm shown below will be used for illustration.

Let the joint angle positions be denoted $\textbf{q} = [q_0, q_1]^T$ , and end-effector position be denoted $\textbf{x} = [x, y, 0]^T$ .
Work is the application of force over a distance

$\textbf{W} = \int \textbf{F}^T \textbf{v} \; dt,$

where $\textbf{W}$ is work, $\textbf{F}$ is force, and $\textbf{v}$ is velocity.
Power is the rate at which work is performed

$\textbf{P} = \frac{d}{d t} \textbf{W},$

where $\textbf{P}$ is power.
Substituting in the equation for work into the equation for power gives:

$\textbf{P} = \frac{d}{d t} \textbf{F}^T \textbf{v}.$

Because of energy equivalence, work is performed at the same rate regardless of the characterization of the system. Rewriting this terms of end-effector space gives:

$\textbf{P} = \textbf{F}_\textbf{x}^T \dot{\textbf{x}},$

where $\textbf{F}_\textbf{x}$ is the force applied to the hand, and $\dot{\textbf{x}}$ is the velocity of the hand. Rewriting the above in terms of joint-space gives:

$\textbf{P} = \textbf{F}_\textbf{q}^T \dot{\textbf{q}},$

where $\textbf{F}_\textbf{q}$ is the torque applied to the joints, and $\dot{\textbf{q}}$ is the angular velocity of the joints. Setting these two equations (in end-effector and joint space) equal to each other and substituting in our equation for the Jacobian gives:

$\textbf{F}_{q_{hand}}^T \dot{\textbf{q}} = \textbf{F}_\textbf{x}^T \dot{\textbf{x}},$

$\textbf{F}_{q_{hand}}^T \dot{\textbf{q}} = \textbf{F}_\textbf{x}^T \textbf{J}_{ee}(\textbf{q}) \; \dot{\textbf{q}},$

$\textbf{F}_{q_{hand}}^T \textbf{F}_\textbf{x}^T \textbf{J}_{ee}(\textbf{q}),$

$\textbf{F}_{q_{hand}} = \textbf{J}_{ee}^T(\textbf{q}) \textbf{F}_\textbf{x}.$

where $\textbf{J}_{ee}(\textbf{q})$ is the Jacobian for the end-effector of the robot, and $\textbf{F}_{q_{hand}}$ represents the forces in joint-space that affect movement of the hand. This says that not only does the Jacobian relate velocity from one state-space representation to another, it can also be used to calculate what the forces in joint space should be to effect a desired set of forces in end-effector space.
Building the Jacobian
First, we need to define the relationship between the $(x,y,z)$ position of the end-effector and the robot’s joint angles, $(q_0, q_1)$ . However will we do it? Well, we know the distances from the shoulder to the elbow, and elbow to the wrist, as well as the joint angles, and we’re interested in finding out where the end-effector is relative to a base coordinate frame…OH MAYBE we should use those forward transformation matrices from the previous post. Let’s do it!
The forward transformation matrix
Recall that transformation matrices allow a given point to be transformed between different reference frames. In this case, the position of the end-effector relative to the second joint of the robot arm is known, but where it is relative to the base reference frame (the first joint reference frame in this case) is of interest. This means that only one transformation matrix is needed, transforming from the reference frame attached to the second joint back to the base.
The rotation part of this matrix is straight-forward to define, as in the previous section:

$\textbf{R}^1_0 = \left[ \begin{array}{ccc} cos(q_0) & -sin(q_0) & 0 \\ sin(q_0) & cos(q_0) & 0 \\ 0 & 0 & 1 \end{array} \right].$

The translation part of the transformation matrices is a little different than before because reference frame 1 changes as a function of the angle of the previous joint’s angles. From trigonometry, given a vector of length $r$ and an angle $q$ the $x$ position of the end point is defined $r \; cos(q)$ , and the $y$ position is $r \; sin(q)$ . The arm is operating in the $(x,y)$ plane, so the $z$ position will always be 0.
Using this knowledge, the translation part of the transformation matrix is defined:

$\textbf{D}^1_0 = \left[ \begin{array}{c} L_0 cos(q_0) \\ L_0 sin(q_0) \\ 0 \end{array} \right].$

Giving the forward transformation matrix:

$\textbf{T}^1_0 = \left[ \begin{array}{cc} \textbf{R}^1_0 & \textbf{D}^1_0 \\ \textbf{0} & \textbf{1} \end{array} \right] = \left[ \begin{array}{cccc} cos(q_0) & -sin(q_0) & 0 & L_0 cos(q_0) \\ sin(q_0) & cos(q_0) & 0 & L_0 sin(q_0)\\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array} \right],$

which transforms a point from reference frame 1 (elbow joint) to reference frame 0 (shoulder joint / base).
The point of interest is the end-effector which is defined in reference frame 1 as a function of joint angle, $q_1$ and the length of second arm segment, $L_1$ :

$\textbf{x} = \left[ \begin{array}{c} L_1 cos(q_1) \\ L_1 sin(q_1) \\ 0 \\ 1 \end{array} \right].$

To find the position of our end-effector in terms of the origin reference frame multiply the point $\textbf{x}$ by the transformation $\textbf{T}^1_0$ :

$\textbf{T}^1_0 \; \textbf{x} = \left[ \begin{array}{cccc} cos(q_0) & -sin(q_0) & 0 & L_0 cos(q_0) \\ sin(q_0) & cos(q_0) & 0 & L_0 sin(q_0)\\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array} \right] \; \left[ \begin{array}{c} L_1 cos(q_1) \\ L_1 sin(q_1) \\ 0 \\ 1 \end{array} \right],$

$\textbf{T}^1_0 \textbf{x} = \left[ \begin{array}{c} L_1 cos(q_0) cos(q_1) - L_1 sin(q_0) sin(q_1) + L_0 cos(q_0) \\ L_1 sin(q_0) cos(q_1) + L_1 cos(q_0) sin(q_1) + L_0 sin(q_0) \\ 0 \\ 1 \end{array} \right]$

where, by pulling out the $L_1$ term and using the trig identities

$cos(\alpha)cos(\beta) - sin(\alpha)sin(\beta) = cos(\alpha + \beta),$

and

$sin(\alpha)cos(\beta) + cos(\alpha)sin(\beta) = sin(\alpha + \beta),$

the position of our end-effector can be rewritten:

$\left[ \begin{array}{c} L_0 cos(q_0) + L_1 cos(q_0 + q_1) \\ L_0 sin(q_0) + L_1 sin(q_0 + q_1) \\ 0 \end{array} \right],$

which is the position of the end-effector in terms of joint angles. As mentioned above, however, both the position of the end-effector and its orientation are needed; the rotation of the end-effector relative to the base frame must also be defined.
Accounting for orientation
Fortunately, defining orientation is simple, especially for systems with only revolute and prismatic joints (spherical joints will not be considered here). With prismatic joints, which are linear and move in a single plane, the rotation introduced is 0. With revolute joints, the rotation of the end-effector in each axis is simply a sum of rotations of each joint in their respective axes of rotation.
In the example case, the joints are rotating around the $z$ axis, so the rotation part of our end-effector state is

$\left[ \begin{array}{c} \omega_x \\ \omega_y \\ \omega_z \end{array} \right] = \left[ \begin{array}{c} 0 \\ 0 \\ q_0 + q_1 \end{array} \right],$

where $\omega$ denotes angular rotation. If the first joint had been rotating in a different plane, e.g. in the $(x, z)$ plane around the $y$ axis instead, then the orientation would be

$\left[ \begin{array}{c} \omega_x \\ \omega_y \\ \omega_z \end{array} \right] = \left[ \begin{array}{c} 0 \\ q_0 \\ q_1 \end{array} \right].$

Partial differentiation
Once the position and orientation of the end-effector have been calculated, the partial derivative of these equations need to be calculated with respect to the elements of $\textbf{q}$ . For simplicity, the Jacobian will be broken up into two parts, $J_v$ and $J_\omega$ , representing the linear and angular velocity, respectively, of the end-effector.
The linear velocity part of our Jacobian is:

$\textbf{J}_v(\textbf{q}) = \left[ \begin{array}{cc} \frac{\partial x}{\partial q_0} & \frac{\partial x}{\partial q_1} \\ \frac{\partial y}{\partial q_0} & \frac{\partial y}{\partial q_1} \\ \frac{\partial z}{\partial q_0} & \frac{\partial z}{\partial q_1} \end{array} \right] = \left[ \begin{array}{cc} -L_0 sin(q_0) - L_1 sin(q_0 + q_1) & - L_1 sin(q_0 + q_1) \\ L_0 cos(q_0) + L_1 cos(q_0 + q_1) & L_1 cos(q_0 + q_1) \\ 0 & 0 \end{array} \right].$

The angular velocity part of our Jacobian is:

$\textbf{J}_\omega(\textbf{q}) = \left[ \begin{array}{cc} \frac{\partial \omega_x}{\partial q_0} & \frac{\partial \omega_x}{\partial q_1} \\ \frac{\partial \omega_y}{\partial q_0} & \frac{\partial \omega_y}{\partial q_1} \\ \frac{\partial \omega_z}{\partial q_0} & \frac{\partial \omega_z}{\partial q_1} \end{array} \right] = \left[ \begin{array}{cc} 0 & 0 \\ 0 & 0 \\ 1 & 1 \end{array} \right].$

The full Jacobian for the end-effector is then:

$\textbf{J}_{ee}(\textbf{q}) = \left[ \begin{array}{c} \textbf{J}_v(\textbf{q}) \\ \textbf{J}_\omega(\textbf{q}) \end{array} \right] = \left[ \begin{array}{cc} -L_0 sin(q_0) - L_1 sin(q_0 + q_1) & - L_1 sin(q_0 + q_1) \\ L_0 cos(q_0) + L_1 cos(q_0 + q_1) & L_1 cos(q_0 + q_1) \\ 0 & 0 \\ 0 & 0 \\ 0 & 0 \\ 1 & 1 \end{array} \right].$

Analyzing the Jacobian
Once the Jacobian is built, it can be analysed for insight about the relationship between $\dot{\textbf{x}}$ and $\dot{\textbf{q}}$ .
For example, there is a large block of zeros in the middle of the Jacobian defined above, along the row corresponding to linear velocity along the $z$ axis, and the rows corresponding to the angular velocity around the $x$ and $y$ axes. This means that the $z$ position, and rotations $\omega_x$ and $\omega_y$ are not controllable. This can be seen by going back to the first Jacobian equation:

$\dot{\textbf{x}} = \textbf{J}_{ee}(\textbf{q})\;\dot{\textbf{q}}.$

No matter what the values of $\dot{\textbf{q}}$ , it is impossible to affect $\omega_x$ , $\omega_y$ , or $z$ , because the corresponding rows during the above multiplication with the Jacobian are $\textbf{0}$ . These rows of zeros in the Jacobian are referred to as its `null space’. Because these variables can’t be controlled, they will be dropped from both $\textbf{F}_\textbf{x}$ and $\textbf{J}(\textbf{q})$ .
Looking at the variables that can be affected it can be seen that given any two of $x, y, \omega_z$ the third can be calculated because the robot only has 2 degrees of freedom (the shoulder and elbow). This means that only two of the end-effector variables can actually be controlled. In the situation of controlling a robot arm, it is most useful to control the $(x,y)$ coordinates, so $\omega_z$ will be dropped from the force vector and Jacobian.
After removing the redundant term, the force vector representing the controllable end-effector forces is

$\textbf{F}_\textbf{x} = \left[ \begin{array}{c}f_x \\ f_y\end{array} \right],$

where $f_x$ is force along the $x$ axis, $f_y$ is force along the $y$ axis, and the Jacobian is written

$\textbf{J}_{ee}(\textbf{q}) = \left[ \begin{array}{cc} -L_0 sin(q_0) - L_1 sin(q_0 + q_1) & - L_1 sin(q_0 + q_1) \\ L_0 cos(q_0) + L_1 cos(q_0 + q_1) & L_1 cos(q_0 + q_1) \end{array} \right].$

If instead $f_{\omega_z}$ , i.e. torque around the $z$ axis, were chosen as a controlled force then the force vector and Jacobian would be (assuming force along the $x$ axis was also chosen):

$\textbf{F}_\textbf{x} = \left[ \begin{array}{c} f_x \\ f_{\omega_z}\end{array} \right],$
$\textbf{J}_{ee}(\textbf{q}) = \left[ \begin{array}{cc} -L_0 sin(q_0) - L_1 sin(q_0 + q_1) & - L_1 sin(q_0 + q_1) \\ 1 & 1 \end{array} \right].$

But we’ll stick with control of the $x$ and $y$ forces instead, as it’s a little more straightforward.
Using the Jacobian
With our Jacobian, we can find out what different joint angle velocities will cause in terms of the end-effector linear and angular velocities, and we can also transform desired $(x,y)$ forces into $(\theta_0, \theta_1)$ torques. Let’s do a couple of examples. Note that in the former case we’ll be using the full Jacobian, while in the latter case we can use the simplified Jacobian specified just above.
Example 1
Given known joint angle velocities with arm configuration

$\textbf{q} = \left[ \begin{array}{c} \frac{\pi}{4} \\ \frac{3 \pi}{8} \end{array}\right] \;\;\;\; \dot{\textbf{q}} = \left[ \begin{array}{c} \frac{\pi}{10} \\ \frac{\pi}{10} \end{array} \right]$

and arm segment lengths $L_i = 1$ , the $(x,y)$ velocities of the end-effector can be calculated by substituting in the system state at the current time into the equation for the Jacobian:

$\dot{\textbf{x}} = \textbf{J}_{ee}(\textbf{q}) \; \dot{\textbf{q}},$

$\dot{\textbf{x}} = \left[ \begin{array}{cc} - sin(\frac{\pi}{4}) - sin(\frac{\pi}{4} + \frac{3\pi}{8}) & - sin(\frac{\pi}{4} + \frac{3\pi}{8}) \\ cos(\frac{\pi}{4}) + cos(\frac{\pi}{4} + \frac{3\pi}{8}) & cos(\frac{\pi}{4} + \frac{3\pi}{8}) \\ 0 & 0 \\ 0 & 0 \\ 0 & 0 \\ 1 & 1 \end{array} \right] \; \left[ \begin{array}{c} \frac{\pi}{10} \\ \frac{\pi}{10} \end{array} \right],$