2024; Robot Control: Kinematic Redundancy

About the Project

I took a course called EE683: Robot Control by Professor Min Jun Kim at KAIST during my 2nd semester. One good thing about the course was the bi-weekly assignment which had one specific part where the students were free to implement any algorithm and provide simulation results. I ended up implementing a lot of the techniques and models learned in the course. These are a series of blog posts that cover some of those sections since I felt like it was worth sharing.

Note that all of these are implemented from scratch, with very few sections requiring MATLAB toolboxes. Also note that just because the specific part was in the report, it does not mean that it worked perfectly; given more time, I would've probably improved it, especially with regards to gain tuning for controllers.

All the work was done in MATLAB and the files will be shared soon on Github. If I have more time, I will also reexplain everything in simpler words.

You can download this full report here:

Kinematic Redundancy

Consider your own hands trying to hold a door knob. Since your body has several degrees of freedom and the task to be performed is just hold a position (x, y, z), you know you can hold the door knob in several different ways. The same goes for robots which have an n-dimensional configuration space (space of all possible ways a robot can be in) and the task to perform is m-dimensional (m < n), like holding a position. It means that the robot can perform the task and also have remaining configurations that are accessible without disturbing the task at hand. This is the essence of kinematic redundancy introduced in this part.

Why kinematic redundancy exists?

Consider a robot with state q in the joint space (say 7-dof) and end-effector configuration p in the task space. We know that the Jacobian J(q) (velocity level inverse kinematics) provides the mapping between the velocities in these two spaces as ˙p = J(q) ˙q. Now, if p is m-dimensional, q is n-dimensional and m < n, J(q) is of size m × n which is a fat matrix. Such matrices can be thought of as a transformation that also performs dimension reduction to the output space, like from 3D space to a 2D plane (2 × 3 matrix). This means that there exists regions of the ˙q space which must go to 0 in the output, i.e. ∃ ˙q_n s.t. J(q) ˙q_n = 0. This is called the null space of the Jacobian matrix, where the motions of the robot has practically 0 effect on the task at hand (given the spaces are orthogonal).

The aim of this section is to express this null-space and form the dynamics wrt task and null motion, that allows us to perform the task, which performing additional motion in the null-space configurations.

Decomposition of joint space

For a linear transformation (matrix) J ∈ R^m×n with rank r = n − m, there exists the row space, column space and null space of the matrix. The r-dim row space is the set of linear combinations of the row vectors given as J^Ty ∀ y, r-dim column space for column vectors Jx ∀ x, and the m-dim null space is the set of solutions to Jx = 0. For such matrices, there exists a pseudo-inverse J^{+} = J^T (JJ^T )^{−1} s.t. JJ^{+} = I. Then, the set of all row space components of the input ˙q can be given by J^{+}J and its orthogonal complement, I − J^{+}J forms the null space component.

It is better to represent this null space component using another matrix Z(q) such that JZ(q)^T = 0, i.e. the row space of Z is orthogonal to the row space of J which is the null space (Z^{+}Z = I − J^{+}J). We can also include a symmetric positive definite weight matrix W (q) to Z component which allows us to write the null space velocities n in output space as

where

One can see that J^{W+} is the weighted pseudo-inverse of J, while W^{−1}Z^{W#} is the weighted pseudo-inverse of Z (one can also say Z^{W#} is right inverse of ZW). This allows us to separate the task variable rates and null motion rates into separate components. This new n × n matrix [J(q); Z(q)W (q)] is non-singular as long as the original system has no kinematic singularities.

Task space dynamics with null motion

Given the system M ¨q + C ˙q + g(q) = τ , we can convert it into the task space wrt p and n. Using weight matrix W(q) = M(q), our dynamics can be written as

which separates the task dynamics and null dynamics. That is, our robot manipulator can perform tasks using f in task space, while using η for other tasks without disturbing task-dynamics in acceleration level.

Extensions to null motion parameterization

The resulting joint torque can also be written as

where τ_null is the joint torque that does not affect the task variables. This matrix [I − J^T {J^{W +}}^T ] is called the projector (to null space) matrix. Now, this method can be used to extend to perform different tasks:

1. Given PD in task space as f = K_P (p_d − p) − K_D (˙p) + ζ_p(q), one can also perform τ_null = D ˙q, which allows us to add joint damping without affecting task dynamics.

2. Another option is to design τ_null = κ∇V (q) where V(q) defines the secondary task that we want to achieve using null-motion. We will try using this method in this work.

Kinematic Redundancy on KUKA IIWA 7

First, the robot is constructed using Robotic Systems Toolbox in MATLAB. Since the robot is 7-dof and my end-effector task p = [ϵ, r] is 6-dof I have kinematic redundancy.

I use simulink blocks for forward dynamics to get joint acclerations and integrate them to obtain the joint configuration q and velocity ˙q. There are additional blocks to generate animations. The external forces acting on each link is set to 0.

Control input

We know that the joint torque with task space motion and null space motion can be written as

Here, the task force f can be constructed with a PD controller with gravity compensation as f = K_P (p_d −p)−K_D ( ˙p)+ζ_p(q) where ζ_p(q) is the gravity compensation in task space, given by ζ_p(q) = {J^{M +}}^T g(q).

In Simulink we can obtain J(q), M (q) and g(q) matrices can be found from the robot’s information. p here is written as [ϵ, r]^T since the toolbox calculates J(q) for ˙p = [ω, v]^T, which is constructed from the homogeneous transformation matrix T between world frame and end-effector frame. ϵ is in axis-angle representation for exponential coordinates. The null space input is constructed by finding the projector matrix and then setting τ_null = D ˙q to provide joint damping.

The blocks shown above perform the computation for J^Tf and [I − J^T {J^{M +}}^T ]τ_null. Also, the W(q) matrix can be chosen by the user as either identity I or the mass matrix M(q).

Other null-space tasks

I also tested building another task to prevent ground contact. Consider z position of end-effector as z = f (q), given the kinematics. Then, we can take V (q) = 1/2 k_z f(q)^2 = 1/2 k_z z^2 for gain k_z. The gradient is given as ∇V = k_z z J_z^T , where J_z is the 1 × n z position component of the Jacobian matrix. Then τ_null = κ (k_z z J_z^T ). This is essentially a simple null motion hierarchical control with a lower priority task that prevents z = 0 condition.

Results

The gains are set as K_P = 500*eye(6), K_D = 50*eye(6), D= 100*eye(6). The results

are shown in RGB order for angles and then positions. The required rotation was [pi/4, pi/4, pi/4] and required position was [0.3, 0.3, 0.3].

1. No null-space control: The result shows oscillations at the end.

   

2. Damping with null-space: By providing joint damping through the null-space input, we can easily see that the end-effector moves in a more damped manner.

   

3. Changing weight matrix: For weight matrix as identity, the behavior was almost the same. This weight matrix is used in the null projection matrix calculations. It seems since my task is a fixed end-effector pose, identity weight matrix seems to work well enough.

   

4. Seconday task: For the V (q) function designed to push the system away from z = 0, we can see that the end-effector moves as expected, staying at a minimum of 0.1m away from the ground.

   

Robot motion visual is given below. However, this cannot really display the difference between the approaches where all of them behaved almost similarly: