# Cartesian Velocity Control between Two 3D Poses

I am really struggling to understand how to do Cartesian velocity control when 3D rotations are involved. So, below is a very simple example which I am hoping somebody can help me with.

Let's say the base frame of my robot is labelled $$B$$, the end-effector frame is labelled $$E$$, and the goal frame is labelled $$G$$. So, the Cartesian pose of the end-effector relative to the base can be expressed as a matrix $$T_{BE}$$, and similarly, $$T_{BG}$$ for the goal. The Jacobian, which describes the rate of change of the end-effector about the base frame, is $$J$$.

I want to move the end-effector from $$E$$ to $$G$$, in time $$T$$. To do this, I can create a loop which continually calculates the required Cartesian velocity of the end-effector, about the base frame. This is a vector length 6, containing translational velocity about the base's x-y-z axes, and rotational velocity about the base's x-y-z axes. We call this velocity vector $$v$$. Then, I will move the end-effector at this velocity, using the inverse Jacobian to calculate the required velocities of the joints, $$q$$. This equation is $$q = J^{-1} v$$.

However, what I don't understand, is how to calculate $$v$$, which is the required Cartesian velocity of the end-effector about the base frame. I know how to do this with 1D rotation: I would just take the difference between the current angle and the goal angle, and divide by $$T$$. I also know how to do this in 3D if the motion only involves translation, and no rotation: again, I can find the difference in the current and goal position. But when 3D rotations are involved, I don't know how to do this.

I have tried converting matrices $$T_{BE}$$ and $$T_{BG}$$ into Euler representations (with X-Y-Z rotations about $$B$$), and then finding the difference in the three components of the Euler vectors, between $$T_{BE}$$ and $$T_{BG}$$. My intuition was that I could then divide these three components by $$T$$, and this would give me the rotational velocity components of vector $$v$$. However, I have implemented this, and I do not get the desired behaviour. I think that this is because the three rotational components are dependent on each other, and so I cannot simply treat each one independently like this.

So, can anybody help me please? How can I create the rotational components of vector $$v$$, given the current pose $$T_{BE}$$ and the target pose $$T_{BG}$$?

The below image summarises the problem: You essentially want to find the time derivative of a linear interpolation between two rotations. The easiest way to obtain this would probably to convert the rotation matrix between the two orientations to a axis-angle representation and the angular velocity would simply be the axis times the angle divided by $$T$$.
• Thanks, I think I understand this a bit better now. Just one more thing though. Let's say I calculated $v$ by taking the difference in the two Euler angles, and dividing by time $T$. Even if the rotation would not necessarily be smooth, would the end-effector arrive at $G$ after time $T$? Or would it actually move to a different pose? The reason I am confused here is that Euler rotations involve three sequential rotations, but the vector $v$ applies a rotation about all three axes instantaneously. So I don't know whether this instantaneous rotation would arrive at $G$. Jul 14, 2020 at 16:26
You could run the solution with some guess of $$v$$ then command a velocity normalized by how long your guess velocity took.