Inverse Kinematics with Orientations

Question

Let's say I have a robot consisting of a base $B$ and a manipulator $M$. The pose of the manipulator can be expressed as the transformation matrix $T_{BM}$. I also have a goal $G$ which I want to move the manipulator to, which can be expressed as the transformation matrix $T_{BG}$. My task is to compute the robot's joint angles $\theta$ which will set the manipulator to the goal pose.

To do this, I can use inverse kinematics. Now, I have some code which will take in the robot's URDF, and give me the Jacobian. This Jacobian tells me the rate of change of $M$ with respect to the rate of change of $\theta$. In other words, $J=\frac{d T_{BM}}{d \theta}$. And so to find the joint angles for the goal pose, I can use $d \theta = J^{-1} d T_{BM}$. This gives me the change in joint angles needed to reach the goal pose. (In practice, the Jacobian is just a local linearisation of the derivative, so I would perform this step multiple times).

My question is follows. The Jacobian is a 6-by-N matrix (for an N-DOF arm), and so I need a 6-by-1 vector to represent $d T_{BM}$. What should I use for $d T_{BM}$, given that all I know are the poses $T_{BM}$ and $T_{BG}$?

Here are my thoughts so far. For the linear position, it seems easy enough to just find the linear position difference between $T_{BM}$ and $T_{BG}$. But what I am confused about, is how to calculate the orientation component of $d T_{BM}$. The Jacobian defines the rate of change of rotation about the {x, y, z} axes, and so I need to express $d T_{BM}$ in terms of rotations about these axes. But how do I get this from $T_{BM}$ and $T_{BG}$? I know how to calculate $T_{MG}$ from these, and then convert the rotation into Euler angles. But it doesn't seem right that these Euler angles are what I should use for $d T_{BM}$, because the Jacobian expresses the instantaneous rate of change around all three axes simultaneously, whereas Euler angles describe a rotation by applying each of the rotations sequentially.

So, any help in understanding this would be great!

Answer 1

The Jacobian is a 6x6 matrix and there are multiple ways it can be derived. The concept of the Jacobian has its roots in Mathematics, where it is not stricly tied to any problem, it is just a matrix of partial derivates of an equation system.

As such it is obtained as:

$$ J = \frac{\partial f}{\partial \theta} = \begin{bmatrix} \frac{\partial f_{\text{1}}}{\partial \theta_1}, & \frac{\partial f_{\text{1}}}{\partial \theta_2} & ..., \frac{\partial f_{\text{1}}}{\partial \theta_n} \\ .\\ .\\ .\\ \frac{\partial f_{\text{n}}}{\partial \theta_1}, & \frac{\partial f_{\text{n}}}{\partial \theta_2} & ..., \frac{\partial f_{\text{n}}}{\partial \theta_n} \end{bmatrix} $$

When definee like this: $$ \dot X = J \times \dot Q$$ $\dot X = [\dot x, \dot y, \dot z, \dot A, \dot B, \dot C]^T$ and $\dot Q = [\dot q_1, \dot q_2, \dot q_3, \dot q_4, \dot q_5, \dot q_6]^T$

There are other ways to obtain this matrix, but the other ways are just applying specifics of the robotics domain (to be more precise, specifics of a robot stucture in combination with specifics of the Denavit Hartenberg convention) to obtain the same result.

If we use the form above we can better identify the equations in question. $f_1$ represents the equation used to calculate the coordinate $x$ of the end-effector. it can be written as:

$$ x =f_1(\theta_1, \theta_2, ... \theta_n)$$

If we consider a 6 DOF robot

$$ x =f_1(\theta_1, \theta_2, ... \theta_6)$$ $$ y =f_2(\theta_1, \theta_2, ... \theta_6)$$ $$ z =f_3(\theta_1, \theta_2, ... \theta_6)$$ $$ A =f_4(\theta_1, \theta_2, ... \theta_6)$$ $$ B =f_5(\theta_1, \theta_2, ... \theta_6)$$ $$ C =f_6(\theta_1, \theta_2, ... \theta_6)$$

Now you can see from here, that these are the forward kinematics equations, and the Jacobi matrix obtained this way will be dependent on $\theta_1, ... \theta_6$. Furthermore, it can be observed, that these equations are in fact the ones which determine in which frame are the orientations expressed and in which order. Any combination is possible, in any order, as long as the mathematical equality stands. So it is important to notice that the concept of the Jacobi matrix itself is agnostic of how the rotations are expressed. It is the underlying equations which define the rotation order and the frame in which the rotations are expressed in.

Moving a robot can be very simple and very complex and anywhere in between.

Simplest is to use the inverse kinematics function, calculate the joint space coordinate of the target pose and use those as reference values for the position controller, no Jacobi involved, but it needs inverse kinematics.

The Jacobi matrix converts velocities, not position. In order to use it, one must task space (Cartesian space) velocities and feed those to a velocity controller. Here care must be taken to end up with 0 velocity at the target point in order to stay at the target position. See the answers to this question for more details about this.