Controlling the pose of a manipulator using inverse kinematics

Question

I want to control a robotic manipulator in Cartesian space, using inverse kinematics. I know that I can do this using $\omega=J^{-1}v$, where $\omega$ is a vector of joint velocities, $J$ is the Jacobian, and $v$ is a vector of {position, orientation} velocities in Cartesian space. Here, the Jacobian expresses how the Cartesian {positions, orientations} change with respect to the joint angles.

My question is as follows. What I have, is two poses of the manipulator in Cartesian space, pose $A$ and pose $B$. And I want to move the manipulator from $A$ to $B$. How do I create the vector $v$ needed for the above inverse kinematics equation?

I am confused, because currently $A$ and $B$ are transformation matrices, expressing the pose of the manipulator in the fixed world coordinates. So, somehow I need to find the difference between the two poses, and then I need to convert this "difference" into the velocity vector $v$. Can anybody help me out please?

Answer 1

As you already have the Jacobian vector, I assume you also solved the inverse kinematics problem. I will refer to the IK problem as $f^{-1}$

In order to get the joint space equivalent of the Cartesian pose A (position and orientation, denoted as $X_A$) you can use the inverse kinematics: $$Q_A = f^{-1}(X_A) $$ similarly $$Q_B = f^{-1}(X_B) $$

(Please note that in most cases the IK has more then one solutions, I am just considering one, pre-selected solution here.)

Now if you give the motors the reference position $Q_A$ or $Q_B$ "eventually" you will end up in pose $A$ and in pose $B$. In this case there are no guaranties that the motors will have the same travel time and there are no guarantees about the "shape" of the path the robot will take.

You can define a straight line between $A$ and $B$ and interpolate between these points on this straight line. In this case there are solutions for an equivalent interpolation technique for the orientation also. For each of the Cartesian interpolated points you can calculate the joint space equivalent and use them as reference positions for the robot. In this case you will have a straight line path in joint space, but the path will very likely have an oscillating velocity profile as the distance to each point will eventually (in the closed loop position controller) will define the implicit velocity profile.

This can be done using IK only. If you prefer to use the $J^{-1}$ you can subtract the current pose of the robot from your target pose.

$$ D = X_B - X_{Current}$$ If we consider D a velocity vector, we can interpret it as the velocity needed to reach point B in unit time. If we are talking about millimetre and seconds, then it is mm/s. $$\omega = J^{-1} \times D $$ If you give this as a constant reference velocity to the robot it will travel in the direction of B and then surpass B and continue. If you cyclically re-evaluate $D$ using the current position it will slow down the robot until eventually almost reaching point $B$. You can add a multiplication factor to this equation and if it is cyclically evaluated with the updated robot positions you will have the effect of a proportional velocity controller. But the velocity profile obtained this way will still be an implicit profile, which slows down towards the end of the path (only) linearly and tries to start with a high velocity.

In order to have a more controlled velocity profile and a straight line you can design an explicit velocity profile. This is called trajectory planning and, most commonly uses an S shaped curve. More about this here.

If you define a velocity profile in Cartesian space (or Task space if we consider pose not just position) at each point in time you can evaluate the velocity profile function at one given point. This is your $v$ vector. You will need the Cartesian position or the joint space position, depending on how you derive your $J^{-1}$ it is usually $(J(Q))^{-1}$ or $J^{-1}(X)$. (One could argue that $(J(Q))^{-1}$ is more helpful as it can use the current joint space coordinates to convert the velocity vector.). To summarize: you define a path in Task space, you plan a velocity profile and you convert this velocity profile, based on the current time step and current robot pose to obtain a new velocity vector in joint space which is given to the velocity controller as a reference value.

Answer 2

If I understand correctly, your end effector is at pose A, you want to move it to pose B, and you have Jacobian pseudo-inverse control set up so that you can specify a pose velocity $v$ and get a corresponding joint velocity $\omega$ that will produce that pose velocity.

There are several ways to produce the $v$ vector. All of them start by defining a time period $T$ for the motion. Your options then include

1. Take a simple difference of the poses:

   a. Take the difference between the position components of the start and end poses, divide by $T$, and use this as the position component of your velocity, $$ v_{\text{pos}} = \frac{B_{\text{pos}}-A_{\text{pos}}}{T}.$$ b. Take a difference between the orientation parameters of $A$ and $B$, and divide by the time period to get the rotational component of your velocity, $$ v_{\text{rot}} = \frac{B_{\text{rot}}-A_{\text{rot}}}{T}.$$ The orientation path you get from following this velocity will depend on your orientation parameterization. Some parameterizations (e.g., quaternions) will give you smooth interpolations, but others (e.g., Euler angles) may end up looking weird and awkward.

2. You can construct your Jacobian so that it gives the body velocity of the end effector (its velocity with respect to the world, in coordinates instantaneously aligned with the end-effector frame). If you have a world-frame Jacobian, you can get the body frame Jacobian by using the inverse of the end-effector's rotation matrix to rotate from world to local coordinates, $$ J_{b} = \begin{bmatrix} R_{E}^{-1} & \\ & R_{E}^{-1} \end{bmatrix} J_{w}. $$ Once you have done this, you can find the local transformation from $A$ to $B$ (the pose that B would have if the origin were at $A$) by multiplying $B$ by the inverse of $A$, $$ \Delta^{b} = A^{-1}B. $$ Taking the matrix log of $\Delta^{b}$, $$ \delta^{b} = \log_{m}(\Delta^{b}) $$ gives you a matrix whose components are $$ \delta^{b} = \begin{bmatrix} 0 & -v^{b}_{rz} & v^{b}_{ry} & v^{b}_{x} \\ v^{b}_{rz} & 0 & -v^{b}_{rx} & v^{b}_{y} \\ -v^{b}_{ry} & v^{b}_{rx} & 0 & v^{b}_{z} \\ 0 & 0 & 0 & 0 \end{bmatrix}. $$ If you have the joints follow the trajectory defined by $$ \omega = J_{b}^{-1} \frac{v^{b}}{T} $$ the end effector will move between the start and end configurations at a constant body velocity.

3. You can construct your Jacobian so that it gives the spatial velocity of the end effector (its velocity with respect to the world, measured as the velocity of the frame attached to the end effector, but currently overlapping the origin). If you have a world-frame Jacobian, you can get the spatial Jacobian by using the end-effector's position as, $$ J_{w} = \begin{bmatrix} % \begin{pmatrix} 1 &&\\ &1&\\ && 1 \end{pmatrix} % & % \begin{pmatrix} \phantom{-}0 & -z & \phantom{-}y\\ \phantom{-}z& \phantom{-}0 & -x \\ -y& \phantom{-}x & \phantom{-}0 \end{pmatrix} \\ \begin{pmatrix} 0&& \\ &0&\\ &&0 \end{pmatrix} & \begin{pmatrix} 1 & & \\ & 1 & \\ && 1 \end{pmatrix} \end{bmatrix} J^{s}, $$ with the terms in the upper-right block encoding the cross-product that relates the translational velocities of attached frames.

   Once you have done this, you can find the spatial transformation from $A$ to $B$ (the global transformation that takes $A$ to $B$) by right-multiplying $B$ by the inverse of $A$, $$ \Delta^{s} = BA^{-1}. $$ Taking the matrix log of $\Delta^{s}$, $$ \delta^{s} = \log_{m}(\Delta^{s}) $$ gives you a matrix whose components are of the same form as those of $\delta^{b}$, $$ \delta^{s} = \begin{bmatrix} 0 & -v^{s}_{rz} & v^{s}_{ry} & v^{s}_{x} \\ v^{s}_{rz} & 0 & -v^{s}_{rx} & v^{s}_{y} \\ -v^{s}_{ry} & v^{s}_{rx} & 0 & v^{s}_{z} \\ 0 & 0 & 0 & 0 \end{bmatrix}. $$ If you have the joints follow the trajectory defined by $$ \omega = J_{s}^{-1} \frac{v^{s}}{T} $$ the end effector will move between the start and end configurations at a constant spatial velocity (i.e. the motion will be as if the end effector was extended to cover the whole space, and someone at the origin was propelling it at what they see as a constant velocity).

An interesting property of physical motions is that both 2 and 3 produce the same trajectory -- a helix around a fixed axis, with special cases of a planar arc when the rotation axis is perpendicular to the translation vector, and a straight line when there is no rotation.

For a smoother motion, you can use a time parameterization that changes the speed across the path so that it eases into and out of the motion.

Beyond the options described above, you start to get deep into motion planning, where you choose paths that minimize things like joint torques or payload acceleration/jerk, take into account the mass distribution of the payload, or include the masses of the links as well as the payload.