Inverse kinematic orientation problem

Question

I am trying IK for 5-DOF robot all revolute joint.

I am working IK with Jacobian inverse i.e
end effector velocity = J inverse * error vector.

In error vector I am feeding (x,y,z) positional error; this gives me correct positioning by first 3 joints only. later 2 joints are pitch and roll used for end effector orientation. How can I input my required orientation and calculate orientation error.

I have Jacobian matrix[6 X 5] considering all 5 joints; so J inverse will be [5 X 6] matrix. Error vector [6 X 1] consisting first 3 element of positional error, but for last 3 element what should I feed to get required orientation for my robot and how would I calculate the orientation error and in which form I should input my orientation of end effector.
please help me out.

Answer 1

Up till now, I am using the Euler angle representation but I have not been successful. After referring to the book "Robotics Modelling, Planning and control" by Bruno Siciliano form Springer publication. It is concluded that the Euler angle is only used for the manipulator having spherical wrist; the robot which doesn't have one need to use another representation i.e equivalent angle representation.

The orientation error vector or error in orientation can be calculated by the equivalent angle representation or angle-axis representation (both are same) of orientation.

If $R_d$ denotes desired orientation matrix and $R_e$ denotes rotation matrix computed form joint variables, then orientation error is calculated using

$$e_{O}=r \sin \vartheta$$

where $r$ and $v(nue)$ can be calculated from matrix multiplication as

$${R}(\vartheta, {r})={R}_{d} {R}_{e}^{T}({q})$$

This gives the $3 \times 1$ vector which is the orientation error about $X, Y, Z$ axis. For more information one can refer book I mention above.

using equation

$$\begin{array}{l}{\vartheta=\cos ^{-1}\left(\frac{r_{11}+r_{22}+r_{33}-1}{2}\right)} \\ {r=\frac{1}{2 \sin \vartheta}\left[\begin{array}{l}{r_{32}-r_{23}} \\ {r_{13}-r_{31}} \\ {r_{21}-r_{12}}\end{array}\right]}\end{array}$$

Answer 2

Referring to the equation $\dot{x} = J(q)\dot{q}$, what the last three elements of $\dot{x}$ should be depends on what kind of Jacobian you are using.

Geometric Jacobian: Suppose your Jacobian is computed as

$$J = \begin{bmatrix}J_1 & J_2 & \cdots J_n\end{bmatrix},$$

where

$$J_i = \begin{cases} \begin{bmatrix}z_{i - 1}\\0_{3\times1}\end{bmatrix} & \text{the $i^\text{th}$ joint is revolute}\\ \begin{bmatrix}z_{i - 1} \times (o_n - o_{i - 1})\\z_{i - 1}\end{bmatrix} & \text{the $i^\text{th}$ joint is prismatic (linear)} \end{cases},$$ $z_i$ is the axis of the $i^\text{th}$ joint and $o_i$ is the origin of the $i^\text{th}$ frame. This Jacobian is called the geometric Jacobian. In this case, the last three elements of $\dot{x}$ should be the angular velocity of the end-effector described w.r.t. the base frame.

Analytical Jacobian: This kind of Jacobian is computed based on representation of the orientation of the end-effector frame. This Jacobian ($J_a$) is related to the geometric Jacobian by $J_a = GJ$, for some matrix $G$. If the simulator is using this kind of matrix, the last three elements of $\dot{x}$ will depend of which representation of orientation is used to calculate the Jacobian.

The easiest way to see what the three elements of $\dot{x}$ should be may be just to try out different orientation representations and see the result.

---

The terminology here is quoted from Chapter 5 of Robots Dynamics and Control (Spong et al.).