# Jacobian-based trajectory following

I would like to control my 7 DOF robot arm to move along a Cartesian trajectory in the world frame. I can do this just fine for translation, but I am struggling on how to implement something similar for rotation. So far, all my attempts seem to go unstable.

The trajectory is described as a translational and rotational velocity, plus a distance and/or timeout stopping criteria. Basically, I want the end-effector to move a short distance relative to its current location. Because of numerical errors, controller errors, compliance, etc, the arm won't be exactly where you wanted it from the previous iteration. So I don't simply do $J^{-1}v_e$. Instead, I store the pose of the end-effector at the start, then at every iteration I compute where the end-effector should be at the current time, take the difference between that and the current location, then feed that into the Jacobian.

I'll first describe my translation implementation. Here is some pseudo OpenRave Python:

# velocity_transform specified in m/s as relative motion
def move(velocity_transform):
t_start = time.time()
pose_start = effector.GetTransform()
while True:
t_now = time.time()
t_elapsed = t_now - t_start
pose_current = effector.GetTransform()
translation_target = pose_start[:3,3] + velocity_transform[:3,3] * t_elapsed
v_trans = translation_target - pose_current[:3,3]
vels = J_plus.dot(v_trans) # some math simplified here


The rotation is a little trickier. To determine the desired rotation at the current time, i use Spherical Linear Interpolation (SLERP). OpenRave provides a quatSlerp() function which I use. (It requires conversion into quaternions, but it seems to work). Then I calculate the relative rotation between the current pose and the target rotation. Finally, I convert to Euler angles which is what I must pass into my AngularVelocityJacobian. Here is the pseudo code for it. These lines are inside the while loop:

rot_t1 = np.dot(pose_start[:3,:3], velocity_transform[:3,:3]) # desired rotation of end-effector 1 second from start
quat_start = quatFromRotationMatrix(pose_start) # start pose as quaternion
quat_t1 = quatFromRotationMatrix(rot_t1) # rot_t1 as quaternion

# use SLERP to compute proper rotation at this time
quat_target = quatSlerp(quat_start, quat_t1, t_elapsed) # world_to_target
rot_target = rotationMatrixFromQuat(quat_target) # world_to_target
v_rot = np.dot(np.linalg.inv(pose_current[:3,:3]), rot_target) # current_to_target
v_euler = eulerFromRotationMatrix(v_rot) # get rotation about world axes


Then v_euler is fed into the Jacobian along with v_trans. I am pretty sure my Jacobian code is fine. Because i have given it (constant) rotational velocities ok.

Note, I am not asking you to debug my code. I only posted code because I figured it would be more clear than converting this all to math. I am more interested in why this might go unstable. Specifically, is the math wrong? And if this is completely off base, please let me know. I'm sure people must go about this somehow.

So far, I have been giving it a slow linear velocity (0.01 m/s), and zero target rotational velocity. The arm is in a good spot in the workspace and can easily achieve the desired motion. The code runs at 200Hz, which should be sufficiently fast enough.

I can hard-code the angular velocity fed into the Jacobian instead of using the computed v_euler and there is no instability. So there is something wrong in my math. This works for both zero and non-zero target angular velocities. Interestingly, when i feed it an angular velocity of 0.01 rad/sec, the end-effector rotates at a rate of 90 deg/sec.

Update: If I put the end-effector at a different place in the workspace so that its axes are aligned with the world axes, then everything seems works fine. If the end-effector is 45 degrees off from the world axes, then some motions seem to work, while others don't move exactly as they should, although i don't think i've seen it go unstable. At 90 degrees or more off from world, then it goes unstable.

• Does the arm go unstable for simple commanded rotations? Feb 19 '15 at 15:39
• Is the time step sufficiently short? Feb 20 '15 at 13:47
• Good questions, I expanded my question with a response.
– Ben
Feb 20 '15 at 17:17
• Thanks a lot for your post and the answers. I am able to compute the error in the rotation matrices between the desired and the current which are 3x3. I am not able to figure out how to derive the axis angle(r*sin(theta)). Please correct me if i am wrong, i think that the axis and angle are obatined by conversion into the quaternion form. Could you please explain ? Thanks in advance
– user13543
Apr 27 '16 at 7:57
• Welcome to robotics Vikram Sridhar. On Stack exchange answers need to answer the question. If you have a related question, it should be asked as a new question (ideally referencing this one). Note that we prefer practical, answerable questions based on actual problems that you face. Please take a look at How to Ask and tour for more information on how stack exchange works. Apr 27 '16 at 9:31

The math required to deal with the orientation error (in terms of error with respect to both desired position and desired velocity) is well described in the book of Sciavicco-Siciliano. See section 3.7.3 starting from page 137.

I'm personally employing the axis-angle notation, thus I coded the formula (3.89) given at page 139 with correct results.

In short, given the goal of tracking a 6D trajectory in the Cartesian space both in position and velocity, the inputs are $\left(p_d,\dot{p}_d\right)$ for the translational part and $\left(\theta_d,\omega_d\right)$ for the orientation part (axis-angle). The inputs are meant to be time varying (i.e. $p_d \equiv p_d\left(t\right)$). Then, the dynamics of the error is analyzed (see e.g. section 3.7.1) to come up with a convergence rule whose $\mathbin{K}_P$ and $\mathbin{K}_O$ are the positive definite matrices gains acting on the error itself. These matrices set just the convergence speed.

I didn't go into your code, but this formula, as different versions of it, makes use of the pseudo-inverse of the Jacobian to allow producing step-by-step the joint velocities required to drive the manipulator towards the target position and orientation of the end-effector with approximately the specified Cartesian speed.

### Update

Following up our discussion below in the comments, to give you a very basic point in the code where to start from, take a glace at these lines. They are part of a method computing the Cartesian error, which is in turn an important piece of our operational space controller we use with the iCub.

Imagine now that ctrlPose==IKINCTRL_POSE_FULL; all the lines 102 => 118 will be then executed. The first block computes the translational error, while the second block implements the formula (3.84) above that deals with the orientation error in axis-angle notation, where the matrix Des represents the desired orientation transformed in DCM format (Direction-Cosine-Matrix, i.e. the standard rotation matrix) from the axis-angle and the matrix H accounts for the current pose.

Once the $6 \times 1$ error vector $e$ gets correctly populated - being the last three components the orientation error in axis-angle convention - you can then run the control law:

$\dot{q}=J_G^+ \cdot e,$

where $J_G$ is the $6 \times n$ Geometric Jacobian and $\dot{q}$ the $n \times 1$ vector of joint velocities. In this basic implementation we don't handle the target orientation velocities $\omega_d$, which are zero, neither the target translational velocities $\dot{p}_d$.

Moreover, we know that $e=\left[e_P, e_O\right]^T$, but we can also impose $e=\left[K_P \cdot e_P, K_O \cdot e_O\right]^T$ to play with convergence speed.

The values of $K_P$ and $K_O$ can be even made greater than $I$ to start with but then bear in mind that the pseudo-inverse algorithm is intrinsically unstable due to e.g. singularities in the Jacobian and high values of gain matrices could enhance this problem.

To mitigate such a downside, one can implement a synergy between the Jacobian pseudo-inverse (very fast but prone to instabilities) and the Jacobian transposed (much slower but intrinsically stable), ending up with the classical Damped-Least-Squares (DLS) technique, aka Levenberg-Marquardt algorithm.

In the end, also DLS can suffer from reliability, accuracy and speed issues, especially when you want to control at the same time the translation and the orientation for a very redundant manipulator equipped with many degrees of freedom. That's why I finally came up with the use of a nonlinear optimizer capable of solving the inverse kinematics on the fly. Once the desired pose is known also in the configuration space the design of the controller is extremely facilitated.

• This looks like what I want. I have the book and am working through it, but could you elaborate on your answer? I am confused as to why $\omega_d$ appears in equation 3.89. Because getting angular velocities in the world frame is the whole problem. And what is $K_O$ and $K_P$? Also, this doesn't solve the first half of the problem (what i am using quatSLERP for) which computes the incremental target rotation. This seems to be covered in section 4.3.3, (page 187). But I can't quite figure that out either.
– Ben
Feb 20 '15 at 21:48
• I've extended the answer. Hope this will better clarify. Feb 21 '15 at 8:41
• Probably, your doubt could be solved by looking at equation (3.84), which represents the rotation matrix error between the current orientation and the desired one. From the matrix you can then get the corresponding axis-angle. The axis part (3 components) will remain unchanged over time, while the angle (1 component) will vary, e.g. linearly with time. Feb 22 '15 at 0:36
• OK, I think it is becoming more clear. Two quick questions though. I am a little confused regarding your change of notation. In the book $\vartheta$ is angle and $\boldsymbol{r}$ is axis, whereas you wrote $\theta$ for angle and $\omega$ for axis. The book specifies $\boldsymbol\omega$ as angular velocity about each axis. Which I always thought was Euler angles with a non-rotating frame, but perhaps I am wrong. What do you use for $\boldsymbol\omega$? Is it $\vartheta\boldsymbol{r}$? Also, is something like $0.9I$ sufficient for the $K$ matrices?
– Ben
Feb 23 '15 at 16:36
• In my answer I somehow abused of notation: with $\theta_d$ I meant the axis-angle $\left(a_x,a_y,a_z,\theta\right)$ with $\|a\|=1$ corresponding to the target rotation matrix $R_d$, whereas $\omega_d$ is the target angular velocity of the frame $R\left(t\right)$ described at page 107 to identify its derivative (here appears the famous skew matrix). $0.9I$ is ok. However I'll expand a bit further directly into the answer. Feb 23 '15 at 19:35

Ugo's answer refers to "Sciavicco-Siciliano" which is a good book I'll quote as well.

Chapter 3.6 introduces the so-called analytical Jacobian which is not the same as the so called geometrical Jacobian as it shows up in: $\omega=J_{geom} \cdot \dot{q},$ but has to be obtained from $J_{geom}$ with the help of the transformation matrix $T_A$:

$J_{analytical}=T_A \cdot J_{geom}$

$\dot{\phi}=J_{analytical} \cdot \dot{q}$

I allow myself one quotation from this chapter:

From a physical viewpoint, the meaning of $\omega_e$ is more intuitive than that of $\dot{\phi}_e$. The three components of ωe represent the components of angular velocity with respect to the base frame. Instead, the three elements of $\dot{\phi}_e$ represent nonorthogonal components of angular velocity defined with respect to the axes of a frame that varies as the end-effector orientation varies. On the other hand, while the integral of $\dot{\phi}_e$ over time gives $\phi_e$, the integral of $\omega_e$ does not admit a clear physical interpretation...

So the good news is that we can work with $\phi_e$ -- orientation of the end effector expressed in Euler angles, the bad news is that the transformation matrix $T_A$ introduces singularities which makes the authors of 2 abandon the concept of analytical Jacobian.

But here comes OpenRave:

manip=robot.GetManipulators()
# we calculate the rotational Jacobian in quaternion space
quatJ = manip.CalculateRotationJacobian()
quatManip0 = manip.GetTransformPose()[0:4]
# order of multiplication may be wrong in the next line
quatManipTarget = qmult(quatManip0, deltaQuat) #
wSol=np.linalg.lstsq(quatJ, quatManipTarget)


So by using the analytical Jacobian in quaternion space you avoid the singularities of the transformation matrix $T_A$. Your Jacobian can still have numeric instability, but this is something different.

UPDATE: I thought again about the singularities of $T_A$ and possible numeric instabilities of the analytical Jacobian in quaternion space. Indeed my previous claim about the singularities in $\mathbb{R}^3$ being not the same as numeric instabilities in quaternion space is too "strong".

So I looked into the quatSlerp function and it seems like you are passing t_elapsed incorrectly. It looked like that argument is supposed to be the the interpolation value, where if t = 0, the rotation returned is the identity, and if t = 1 the rotation returned is the full rotation between first two quaternion arguments passed.

If this code is inside a while loop, and t_elapsed is getting larger than 1, the arm will go unstable.

This doesn't explain why the unstable behavior would be affected by position.

Hopefully this isn't what you meant by debugging your code.

quatSlerp

• I think quatSlerp can be used in this way. You are right that I am effectively extrapolating instead of interpolating if t_elapsed is greater than 1. (I did some quick tests and it appears that quatSlerp allows this with the proper behavior.) So velocity_transform is angular velocity in rad/sec, and quat_t1 is therefore the desired rotation 1 second after start. After getting some comments, I realized that it might be better if I pass in the goal pose and time to reach it instead of velocity. This would then only interpolate, but I don't think it changes anything much.
– Ben
Feb 22 '15 at 1:22