Jacobian-based trajectory following

Question

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.

Answer 1

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.

Answer 2

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} $

or even more informative:

$ \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()[0]
# 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".

Answer 3

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