Manipulability ellipsoid of arm and mobile robot is drawn with python

There is a concept of manipulability ellipsoid in the robot arm. It is an index to evaluate in which direction it is easy to move the hand position at a certain angle.

This time, after finding this with a 2-axis robot arm, we will also find it with a mobile robot that uses a Mecanum wheel.

2-axis robot arm

Consider a 2-axis robot arm as shown below. It's a simple one that is often used in mechanics.

Quoted from https://jp.mathworks.com/discovery/inverse-kinematics.html

Forward kinematics

The forward kinematics of this robot arm is expressed by the following formula. Well, there is no need to explain so far.

\left\{\begin{array}{l}
x=L_{1} \cos \left(\theta_{1}\right)+L_{2} \cos \left(\theta_{1}+\theta_{2}\right) \\
y=L_{1} \sin \left(\theta_{1}\right)+L_{2} \sin \left(\theta_{1}+\theta_{2}\right)
\end{array}\right.

Manipulability ellipsoid

From here, we will explain the manipulability ellipsoid. Both sides of the above forward kinematics equation are time-differentiated. If you write it, the following relational expression between tip velocity and joint angular velocity can be derived.

\frac{dx}{dt}=
-L_{1}\dot{\theta_1} \sin \left(\theta_{1}\right)
-L_{2}\dot{\theta_1} \sin \left(\theta_{1}+\theta_{2}\right)
-L_{2}\dot{\theta_2} \sin \left(\theta_{1}+\theta_{2}\right) \\
\frac{dy}{dt}=
L_{1}{\theta_1} \cos \left(\theta_{1}\right)
+L_{2}{\theta_1} \cos \left(\theta_{1}+\theta_{2}\right)
+L_{2}{\theta_2} \cos \left(\theta_{1}+\theta_{2}\right)

Furthermore, when this expression is displayed in a matrix, it can be summarized as follows.

\left[
\begin{array}{ll}
\dot{x}\\
\dot{y}
\end{array}
\right]

=\left[\begin{array}{ll}
-L_{1} \sin \left(\theta_{1}\right)-L_{2} \sin \left(\theta_{1}+\theta_{2}\right) & -L_{2} \sin \left(\theta_{1}+\theta_{2}\right) \\
L_{1} \cos \left(\theta_{1}\right)+L_{2} \cos \left(\theta_{1}+\theta_{2}\right) & L_{2} \cos \left(\theta_{1}+\theta_{2}\right)
\end{array}\right]

\left[
\begin{array}{ll}
\dot{\theta_{1}}\\
\dot{\theta_{2}}
\end{array}
\right]

Furthermore, each vector and matrix are collectively expressed as follows.

\dot{\boldsymbol{r}}=\boldsymbol{J}(\theta) \dot{\boldsymbol{\theta}}

However, r is the position vector of the tip and θ is the joint vector. J is also called the Jacobian matrix, which is a matrix represented by the following. You may have learned so far.

\begin{aligned}
\boldsymbol{J(\theta)} &=\left[\begin{array}{cc}
\frac{\partial x}{\partial \theta_{1}} & \frac{\partial x}{\partial \theta_{2}} \\
\frac{\partial y}{\partial \theta_{1}} & \frac{\partial y}{\partial \theta_{2}}
\end{array}\right] \\
&=\left[\begin{array}{ll}
-L_{1} \sin \left(\theta_{1}\right)-L_{2} \sin \left(\theta_{1}+\theta_{2}\right) & -L_{2} \sin \left(\theta_{1}+\theta_{2}\right) \\
L_{1} \cos \left(\theta_{1}\right)+L_{2} \cos \left(\theta_{1}+\theta_{2}\right) & L_{2} \cos \left(\theta_{1}+\theta_{2}\right)
\end{array}\right]
\end{aligned}

Using this Jacobian matrix J, create the following square matrix A.

\boldsymbol{A}=\boldsymbol{J(\theta)} \boldsymbol{J(\theta)}^{T}

The square root of the determinant of this matrix A is called the operability w.

w=\sqrt{\operatorname{det} \boldsymbol{A}}

In addition, the manipulability ellipsoid can be obtained from the eigenvalue λ and the eigenvector v of this matrix A. The eigenvector v represents the axial direction of the operability ellipsoid, and the square root of the eigenvalue λ represents the size of the ellipse with respect to the corresponding axial direction.

The code you actually want

I calculated it with python. Drawing is matplotlib

draw_ManipulabilityEllipsoid_RobotArm.py

import numpy as np
import matplotlib.pyplot as plt
import matplotlib.patches as pat

fig, ax = plt.subplots(figsize=(10, 8))

def draw_ellipsoid(ax, a, b, x=0, y=0, deg=0):
    ellips = pat.Ellipse(xy = (x, y), width=a, height=b, alpha = 0.6, angle=deg, color="red", label="")
    ax.add_patch(ellips)

def calculate_Ellipsoid_from_Jacobian(J):
    #Calculate the square matrix A
    A = J * J.T
    #Eigenvalue of A,Calculate eigenvectors
    values, vectors = np.linalg.eig(A)
    #print(values, vectors)

    #eigenvalue^(1/2)Get
    Lambda_1 = np.sqrt(values[0])
    Lambda_2 = np.sqrt(values[1])
    #Get the size of the ellipse from the eigenvalues
    a = Lambda_2    #Horizontal axis length
    b = Lambda_1    #Vertical axis length
    
    #Get eigenvectors
    Vector_1 = vectors[:,0]
    Vector_2 = vectors[:,1]
    #Get the slope of the ellipse
    #Since A is a symmetric matrix, the eigenvectors are orthogonal
    #So you can use either of the eigenvectors for the slope of the ellipse.
    rad = -np.arctan2(Vector_1[0],Vector_1[1])
    rad = np.arctan2(Vector_2[1],Vector_2[0])

    return a, b, rad

def get_robotarm_pose(L1, L2, theta1, theta2):
    #Forward kinematics
    X1 = L1*np.cos(theta1)
    Y1 = L1*np.sin(theta1)
    X2 = L1*np.cos(theta1)+L2*np.cos(theta1+theta2)
    Y2 = L1*np.sin(theta1)+L2*np.sin(theta1+theta2)
    return X1, Y1, X2, Y2

def get_Jacobian_robotarm_kinematics(L1, L2, theta1, theta2):
    # Jacobian Matrix
    J = np.matrix([[-L1*np.sin(theta1)-L2*np.sin(theta1+theta2), -L2*np.sin(theta1+theta2)], 
                    [L1*np.cos(theta1)+L2*np.cos(theta1+theta2), L2*np.cos(theta1+theta2)] ])

    return J

def draw_ellipsoid_from_robotarm(L1, L2, theta1, theta2):

    J = get_Jacobian_robotarm_kinematics(L1, L2, theta1, theta2)
    x1,y1,x2,y2 = get_robotarm_pose(L1, L2, theta1, theta2)
    a, b, rad = calculate_Ellipsoid_from_Jacobian( J )

    #Drawing an ellipse
    draw_ellipsoid(ax, a*0.5, b*0.5, x2, y2, np.rad2deg(rad))

    #Drawing of robot arm
    ax.plot([0, x1], [0, y1], 'k', linewidth = 10.0, zorder=1)
    ax.plot([x1, x2], [y1, y2], 'k', linewidth = 10.0, zorder=1)
    c0 = pat.Circle(xy=(0, 0), radius=.1, ec='w', fill=True, zorder=2)
    c1 = pat.Circle(xy=(x1, y1), radius=.1, ec='w', fill=True, zorder=2)
    ax.add_patch(c0)
    ax.add_patch(c1)

    #Adjustment of figure
    ax.set_xlabel('x')
    ax.set_ylabel('y')
    ax.set_aspect('equal')
    ax.grid()
    plt.xlim([-2,2])
    plt.ylim([-0.5,2])

if __name__ == "__main__":
    L1 = 1.0
    L2 = 1.0
    theta1 = np.deg2rad(30)
    theta2 = np.deg2rad(40)
    draw_ellipsoid_from_robotarm(L1, L2, theta1, theta2)
    plt.show()

result

The execution result is as follows. It would be fun to add a track bar and move it slimy.

Mecanum wheel mobile robot

Next, consider a manipulability ellipsoid for a mobile robot. We could not find any literature that applies manipulability ellipsoids other than robot arms, but the idea is the same.

Forward kinematics

A robot that moves with a Mecanum wheel is defined as shown in the figure below.

--Mecanum wheel roller angle φ --Vertical width of the aircraft a --Width of the aircraft (tread width) b It was made.

L is below from the figure.

L = \sqrt { a ^ { 2 } + b ^ { 2 } } \cos \alpha \\
\alpha = \varphi - \tan ^ { - 1 } \frac { a } { b }

From the figure, if the peripheral speed of each wheel is v1, v2, v3, v4, the following relationship can be geometrically derived. (Imagine moving the robot directly by hand.)

\begin{aligned} v _ { 1 } & = - \dot { x } \sin \varphi + \dot { y } \cos \varphi + L \dot { \theta } \\ v _ { 2 } & = - \dot { x } \sin \varphi - \dot { y } \cos \varphi + L \dot { \theta } \\ v _ { 3 } & = \dot { x } \sin \varphi - \dot { y } \cos \varphi + L \dot { \theta } \\ v _ { 4 } & = \dot { x } \sin \varphi + \dot { y } \cos \varphi + L \dot { \theta } \end{aligned}

The matrix display is as follows.

 \begin{bmatrix} { v _ { 1 } } \\ { v _ { 2 } } \\ { v _ { 3 } } \\ { v _ { 4 } } \end{bmatrix} = \begin{bmatrix} { - \sin \varphi } & { \cos \varphi } & { L } \\ { - \sin \varphi } & { - \cos \varphi } & { L } \\ { \sin \varphi } & { - \cos \varphi } & { L } \\ { \sin \varphi } & { \cos \varphi } & { L } \end{bmatrix} \begin{bmatrix}{ \dot { x } } \\ { \dot { y } } \\ { \dot { \theta } } \end{bmatrix}

However, the velocity vector is the velocity seen from the robot frame {R}. When actually controlling, I would like to think about how to move from the viewpoint of the inertia frame {I}. The velocity vector in the robot frame is expressed by the following equation from the velocity vector in the inertial frame using the rotation matrix.

\begin{bmatrix}{ \dot { x } } \\ { \dot { y } } \\ { \dot { \theta } }\end{bmatrix}  =  \begin{bmatrix} { \cos \theta } & { \sin \theta } & { 0 } \\ { - \sin \theta } & { \cos \theta } & { 0 } \\ { 0 } & { 0 } & { 1 } \end{bmatrix}   \begin{bmatrix} { \dot { x } _ { I } } \\ { \dot { y } _ { I } } \\ { \dot { \theta } _ { I } } \end{bmatrix} 

Therefore, by substituting into the previous equation, the following equation is obtained.

 \begin{bmatrix} { v _ { 1 } } \\ { v _ { 2 } } \\ { v _ { 3 } } \\ { v _ { 4 } } \end{bmatrix} = \begin{bmatrix} { - \sin \varphi } & { \cos \varphi } & { L } \\ { - \sin \varphi } & { - \cos \varphi } & { L } \\ { \sin \varphi } & { - \cos \varphi } & { L } \\ { \sin \varphi } & { \cos \varphi } & { L } \end{bmatrix} \begin{bmatrix} { \cos \theta } & { \sin \theta } & { 0 } \\ { - \sin \theta } & { \cos \theta } & { 0 } \\ { 0 } & { 0 } & { 1 } \end{bmatrix}  \begin{bmatrix}{ \dot { x } _ { l } } \\ { \dot { y } _ { I } } \\ { \dot { \theta } _ { I } } \end{bmatrix}

You can see that it looks like the following formula.

\boldsymbol{\dot{ \theta }} = \boldsymbol{X(\theta)} \boldsymbol{\dot{ x }}

Here, the Jacobian matrix J (θ) appears when the formula is transformed into the following shape.

\boldsymbol{\dot{ x }} = \boldsymbol{J(\theta)} \boldsymbol{\dot{ \theta }}

Here, we find the manipulability ellipsoid of the Mecanum wheel robot. However, since the operability related to rotation is not taken into consideration, the formula used is changed as follows.

 \begin{bmatrix} { v _ { 1 } } \\ { v _ { 2 } } \\ { v _ { 3 } } \\ { v _ { 4 } } \end{bmatrix} = \begin{bmatrix} { - \sin \varphi } & { \cos \varphi }  \\ { - \sin \varphi } & { - \cos \varphi } \\ { \sin \varphi } & { - \cos \varphi }  \\ { \sin \varphi } & { \cos \varphi } \end{bmatrix} \begin{bmatrix} { \cos \theta } & { \sin \theta } \\ { - \sin \theta } & { \cos \theta }  \end{bmatrix}  \begin{bmatrix}{ \dot { x } _ { l } } \\ { \dot { y } _ { I } } \end{bmatrix}

code

You need the above draw_ManipulabilityEllipsoid_RobotArm.py.

draw_ManipulabilityEllipsoid_Mecanum.py

import numpy as np
import matplotlib.pyplot as plt
import matplotlib.patches as pat
import matplotlib.transforms as transforms
from draw_ManipulabilityEllipsoid_RobotArm import *

def get_Jacobian_MecanumRobot_kinematics(theta, fai):
    # Jacobian Matrix
    A = np.matrix([ [-np.sin(fai),np.cos(fai)], [-np.sin(fai),-np.cos(fai)], [np.sin(fai),-np.cos(fai)],[np.sin(fai),np.cos(fai)] ])
    R = np.matrix([ [np.cos(theta),np.sin(theta)], [-np.sin(theta),np.cos(theta)] ])
    J = np.linalg.inv(R) * np.linalg.pinv(A);
    return J

def draw_ellipsoid_from_MacanumRobot(theta, fai):

    J = get_Jacobian_MecanumRobot_kinematics(theta, fai)
    a, b, rad = calculate_Ellipsoid_from_Jacobian( J )

    #Drawing an ellipse
    draw_ellipsoid(ax, a*3, b*3, 0, 0, np.rad2deg(rad))

    #Robot drawing
    a = 1.0
    b = 1.0
    wheel_width = 0.3
    wheel_diameter = 0.6
    wheel_pos_list = [(b/2,a/2), (-b/2,a/2), (-b/2,-a/2), (b/2,-a/2)]
    ts = ax.transData
    tr = transforms.Affine2D().rotate_deg_around(0, 0, np.rad2deg(theta)) 
    t = tr + ts
    for pos in wheel_pos_list:
        wheel = pat.Rectangle(xy=(pos[0]-wheel_width/2,pos[1]-wheel_diameter/2),width=wheel_width,height=wheel_diameter,transform=t,color="k")
        ax.add_patch(wheel)
    robot = pat.Rectangle(xy=(-b/2+wheel_width/2,-a/2),width=b-wheel_width,height=a,transform=t,color="b")
    ax.add_patch(robot)

    #Adjustment of figure
    ax.set_xlabel('x')
    ax.set_ylabel('y')
    ax.set_aspect('equal')
    ax.grid()
    plt.xlim([-1.5,1.5])
    plt.ylim([-1.5,1.5])

if __name__ == "__main__":
    theta = np.deg2rad(20)
    fai = np.deg2rad(30)
    draw_ellipsoid_from_MacanumRobot(theta, fai)
    plt.show()

result

You need the above draw_ManipulabilityEllipsoid_RobotArm.py.

θ=20°, φ=30°

The execution result is as follows. Like that

θ=0°, φ=30°

Well, even if the robot rotates, it does not affect the shape of the ellipse. If you set theta = 0, it will be as follows.

θ=0°, φ=45°

Also, if fai = 45 °, the ease of movement in the x and y directions becomes equal, and the result is as follows.

θ=0°, φ=0°

Besides, if fai = 0 °, it will be as follows. If you look closely, there is a vertical line in the center of the robot. (The color of the main body has been lightened for clarity.)

It is easy to understand if you consider the following figure. You can immediately see that if fai = 0, you cannot move in the x direction.