Inverse Kinematics for 4 legged robots and body translation

Question

The prompt here is to implement the IK on a real robot with constraints. I started off with writing the DH-Table.

$\begin{bmatrix}n & a_{i-1} & \alpha_{i-1} & d_i & \theta_i \\1 & 0 & 0 & 0 & \theta_1 \\ 2 & a_1 & \frac{pi}{2} & 0 & \theta_2 \\ 3 & a_3 & 0 & 0 & \theta_3 \\ 4 & a_3 & 0 & 0 & 0 \end{bmatrix}$

I've deduced this DH table into its corresponding matrices and solved them for $\theta_1$, $\theta_2$ and $\theta_3$. ( The math isn't shown here. )

I arrive at these 3 equations.

$$ \theta_1 = tan^{-1}\frac{P_y}{P_x}$$ $$ \theta_2 = ....$$ $$ \theta_3 = ...$$

Now the question is to place the legs on the edge of the robot's body.

1.) Is there material out there that explains how the frame can be translated from the center of the body to the legs?

2.) Is there any material out there that explains Body IK and how it can be implemented for Roll Pitch and Yaw? and how to develop gaits for a robot like this in terms of code?

any information, tips, and suggestions are much welcome. I've also placed a python script where the user can control the position of the leg using IK.

import math
import numpy as np
from mpl_toolkits import mplot3d
import matplotlib.pyplot as plt
from matplotlib.widgets import Slider, RadioButtons

LINK_1 = 60 + 60
LINK_2 = 110
LINK_3 = 120


"""
Place simulation on leg and check. Makes sure the image is inverted on the real leg.
"""

def constrain(val, min_val, max_val):
    return min( max_val, max(min_val, val) )


class Joint:
    def __init__(self, link_1, link_2, link_3):
        self.link_1 = link_1
        self.link_2 = link_2
        self.link_3 = link_3

        self.theta_1 = -1
        self.theta_2 = -1
        self.theta_3 = -1


def FK( joint ):
    T0_1 = np.array([ 
        [math.cos(joint.theta_1), -math.sin(joint.theta_1), 0, 0], 
        [math.sin(joint.theta_1),  math.cos(joint.theta_1), 0, 0],
        [0, 0, 1, 0],  
        [0, 0, 0, 1 ]
        ])
    T1_2 = np.array([
        [math.cos(joint.theta_2), -math.sin(joint.theta_2), 0, joint.link_1],  
        [0, 0, -1, 0], 
        [math.sin(joint.theta_2),  math.cos(joint.theta_2), 0, 0], 
        [0, 0, 0, 1]
        ])
    T2_3 = np.array([
        [math.cos(joint.theta_3), -math.sin(joint.theta_3), 0, joint.link_2],
        [math.sin(joint.theta_3),  math.cos(joint.theta_3), 0, 0],
        [0, 0, 1, 0],  
        [0, 0, 0, 1]
        ])
    T3_4 = np.array([
        [1, 0, 0, joint.link_3],
        [0, 1, 0, 0], 
        [0, 0, 1, 0], 
        [0, 0, 0, 1]
        ])

    POINT_SET_1 = np.dot( T0_1, T1_2 )
    POINT_SET_2 = np.dot( POINT_SET_1, T2_3 )
    POINT_SET_3 = np.dot( POINT_SET_2, T3_4 )
    return [POINT_SET_1, POINT_SET_2, POINT_SET_3]



def leg2( x, y, z ):
    #Final calculation resulting in smooth motion.

    theta_1 = math.atan2(y, x)
    A = z
    B = math.cos(theta_1) * x + y * math.sin(theta_1) - LINK_1
    C = ((math.pow(A, 2) + math.pow(B,2) - math.pow(LINK_3, 2) - math.pow(LINK_2,2)) / (2 * LINK_3 * LINK_2))
    theta_3 = math.atan2( math.sqrt( 1 - math.pow( C,2 )), C)
    D = math.cos(theta_3) * LINK_3 + LINK_2
    E = math.sin(theta_3) * LINK_3

    numerator = (A * D - B * E) / (math.pow(E,2) + math.pow(D,2) )
    denominator = 1 - math.pow(numerator,2)

    theta_2 = math.atan2(numerator, math.sqrt(denominator))
    return [ math.degrees(theta_1), math.degrees(theta_2), math.degrees(theta_3)]




def filterEndPoints( point_set ):
    return (
        [
          [point_set[0][0][3], point_set[0][1][3], point_set[0][2][3]],
          [point_set[1][0][3], point_set[1][1][3], point_set[1][2][3]],
          [point_set[2][0][3], point_set[2][1][3], point_set[2][2][3]]
        ]
    )


fig  = plt.figure()
ax = plt.axes(projection='3d')
ax.set_xlim3d(-300, 300)
ax.set_ylim3d(-300, 300)
ax.set_zlim3d(-300, 300)
ax.set_xlabel("X-axis")
ax.set_ylabel("Y-axis")
ax.set_zlabel("Z-axis")
axe = plt.axes([0.25, 0.85, 0.001, 0.001])

axxval = plt.axes([0.35, 0.9, 0.45, 0.03])
axyval = plt.axes([0.35, 0.93, 0.45, 0.03])
axzval = plt.axes([0.35, 0.96, 0.45, 0.03])
a0_val = Slider(axxval, "X", -300, 300, valinit=180)
a1_val = Slider(axyval, "Y", -300, 300, valinit=180)
a2_val = Slider(axzval, "Z", -300, 300, valinit=180)


x_value = 0
y_value = 0
z_value = 0

joint =  Joint(60+60, 110, 120)
joint_values = leg2( 100, 100, 100 )
joint.theta_1 = joint_values[0]
joint.theta_2 = joint_values[1]
joint.theta_3 = joint_values[2]
link_1 = filterEndPoints(FK( joint ))
originPoints = [0, 0, 0]





def returnConstrainedAngles( joint_values ):
    theta_1 = -constrain(joint_values[0], 0.0, 180.0)
    theta_2 = -constrain(joint_values[1], 0.0, 180.0)
    theta_3 = -constrain(joint_values[2], 0.0, 180.0)
    # theta_1 = -joint_values[0]
    # theta_2 = -joint_values[1]
    # theta_3 = -joint_values[2]
    return [math.radians(theta_1), math.radians(theta_2), math.radians(theta_3)]


def update_ao_val(val):  
    global link_1
    global x_value, y_value, z_value
    x_value = int(val)

    try:  
        #joint_values = leg(x_value, y_value, z_value)
        joint_values = leg2(x_value, y_value, z_value)
    except Exception as e:
        return


    joint.theta_1 = returnConstrainedAngles( joint_values )[0]
    joint.theta_2 = returnConstrainedAngles( joint_values )[1]
    joint.theta_3 = returnConstrainedAngles( joint_values )[2]
    serial_print = " T1 : "+ str( abs( round( math.degrees( joint.theta_1 ) ) ) )  + " T2: " + str( abs( round( math.degrees( joint.theta_2 ) ) )) + " T3 : "+ str( abs( round( math.degrees( joint.theta_3 ) ) ))
    print( serial_print )

    link_1 = filterEndPoints(FK( joint ))
    print( str(link_1[0][2]) + " : " + str(link_1[1][2]) + " : " + str(link_1[2][2]) )
    fig.canvas.draw_idle()
    ax.clear()
    ax.set_xlabel("X-axis")
    ax.set_ylabel("Y-axis")
    ax.set_zlabel("Z-axis")
    ax.set_xlim3d(-300, 300)
    ax.set_ylim3d(-300, 300)
    ax.set_zlim3d(-300, 300)
    ax.plot3D( [ -50, 50, 50, -50, -50 ], [ -50, -50, 50, 50, -50 ], [ 0, 0, 0, 0, 0 ], "-r*" )
    ax.plot3D( [ originPoints[0], link_1[0][0], link_1[1][0], link_1[2][0] ], 
               [ originPoints[1], link_1[0][1], link_1[1][1], link_1[2][1] ], 
               [ originPoints[2], link_1[0][2], link_1[1][2], link_1[2][2] ], "-go" )
    fig.canvas.draw()


def update_a1_val(val):  
    global link_1
    global x_value, y_value, z_value
    y_value = int(val)
    try:  
        #joint_values = leg(x_value, y_value, z_value)
        joint_values = leg2(x_value, y_value, z_value)
    except NoneType as e:
        return
    joint.theta_1 = returnConstrainedAngles( joint_values )[0]
    joint.theta_2 = returnConstrainedAngles( joint_values )[1]
    joint.theta_3 = returnConstrainedAngles( joint_values )[2]
    serial_print = " T1 : "+ str( abs( round( math.degrees( joint.theta_1 ) ) ) )  + " T2: " + str( abs( round( math.degrees( joint.theta_2 ) ) )) + " T3 : "+ str( abs( round( math.degrees( joint.theta_3 ) ) ))
    print( serial_print )
    link_1 = filterEndPoints(FK( joint ))
    print( str(link_1[0][2]) + " : " + str(link_1[1][2]) + " : " + str(link_1[2][2]) )
    ax.clear()
    ax.set_xlabel("X-axis")
    ax.set_ylabel("Y-axis")
    ax.set_zlabel("Z-axis")
    ax.set_xlim3d(-300, 300)
    ax.set_ylim3d(-300, 300)
    ax.set_zlim3d(-300, 300)
    ax.plot3D( [ -50, 50, 50, -50, -50 ], [ -50, -50, 50, 50, -50 ], [0, 0, 0, 0, 0], "-r*" )
    ax.plot3D( [ originPoints[0], link_1[0][0], link_1[1][0], link_1[2][0] ], 
               [ originPoints[1], link_1[0][1], link_1[1][1], link_1[2][1] ], 
               [ originPoints[2], link_1[0][2], link_1[1][2], link_1[2][2] ], "-go" )
    fig.canvas.draw()


# 228 81
# 116 169
def update_a2_val(val):  
    global link_1
    global x_value, y_value, z_value, radius_value
    z_value = int(val)
    try:  

        joint_values = leg2(x_value, y_value, z_value)
    except NoneType as e:
        return
    joint.theta_1 = returnConstrainedAngles( joint_values )[0]
    joint.theta_2 = returnConstrainedAngles( joint_values )[1]
    joint.theta_3 = returnConstrainedAngles( joint_values )[2]
    serial_print = "T1: "+ str( abs( round( math.degrees( joint.theta_1 ) ) ) )  + " T2: " + str( abs( round( math.degrees( joint.theta_2 ) ) )) + " T3 : "+ str( abs( round( math.degrees( joint.theta_3 ) ) ))
    print( serial_print )
    link_1 = filterEndPoints(FK( joint ))
    print( str(link_1[0][2]) + " : " + str(link_1[1][2]) + " : " + str(link_1[2][2]) )
    ax.clear()
    ax.set_xlabel("X-axis")
    ax.set_ylabel("Y-axis")
    ax.set_zlabel("Z-axis")
    ax.set_xlim3d(-300, 300)
    ax.set_ylim3d(-300, 300)
    ax.set_zlim3d(-300, 300)
    ax.plot3D( [ -50, 50, 50, -50, -50 ], [ -50, -50, 50, 50, -50 ], [0, 0, 0, 0, 0], "-r*" )
    ax.plot3D( [ originPoints[0], link_1[0][0], link_1[1][0], link_1[2][0] ], 
               [ originPoints[1], link_1[0][1], link_1[1][1], link_1[2][1] ], 
               [ originPoints[2], link_1[0][2], link_1[1][2], link_1[2][2] ], "-go" )
    fig.canvas.draw()



def generateTrajectorySemicircle( radius ):
    theta = np.linspace(0, -np.pi, 100)
    r     = np.sqrt( radius )
    x1    = r   * np.cos( theta )
    x2    = 75  + r * np.sin( theta )
    x3    = 170 - np.linspace(0, 0, 100)
    return [ x1, x2, x3 ]

def plotGraph( graphInstance, currentPoint, trajectoryPoints ): 
  graphInstance.clear()
  graphInstance.plot3D( trajectoryPoints[2], trajectoryPoints[0], trajectoryPoints[1], "*", markerSize="4")

  #Setting limits
  graphInstance.set_xlim3d(-200, 200)
  graphInstance.set_ylim3d(-200, 200)
  graphInstance.set_zlim3d(-200, 200)

  graphInstance.set_xlabel("X-axis")
  graphInstance.set_ylabel("Y-axis")
  graphInstance.set_zlabel("Z-axis")
  graphInstance.plot3D( [ -50, 50, 50, -50, -50 ], [ -50, -50, 50, 50, -50 ], [0, 0, 0, 0, 0], "-r*" )
  graphInstance.plot3D( 
      [ 100, currentPoint[0][0], currentPoint[1][0], currentPoint[2][0] ], 
      [ 0, currentPoint[0][1], currentPoint[1][1], currentPoint[2][1] ], 
      [ 0, currentPoint[0][2], currentPoint[1][2], currentPoint[2][2] ], "-go")

  plt.pause(0.00001)


points = generateTrajectorySemicircle( 1000 )



a0_val.on_changed(update_ao_val)
a1_val.on_changed(update_a1_val)
a2_val.on_changed(update_a2_val)
plt.show()

Answer 1

1.) Is there material out there that explains how the frame can be translated from the center of the body to the legs?

When you say I've deduced this DH table into its corresponding matrices - presumably these matrices you mention are homogenous transformation matrices that capture the four movements implied by the DH parameters ($a, \alpha, d, \theta$). You can also use these to describe the (fixed) relationship between the centre of the body and the base frame of the leg. For example, if your body frame has the x axis pointing forwards, and the front left leg is located 2 units forward and 1.5 units left, with the base frame of that leg rotated (around z axis) so that the leg is pointed 'outwards' by 90 degrees, then you'll have $$ {}^{body}T_{frontleftleg} = \begin{bmatrix} \mathbf{R} & t \\ \mathbf{0} & 1\\ \end{bmatrix} = \begin{bmatrix} 0 & -1 & 0 & 2 \\ 1 & 0 & 0 & 1.5\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ \end{bmatrix} $$

2.) Is there any material out there that explains Body IK and how it can be implemented for Roll Pitch and Yaw?

Once you've got the leg IK worked out, there isn't much more to do. Consider the robot being stationary with all feet on the ground and the body in some known position. You can work out where a foot is relative to the body by combining the fixed transformation between body and frame at the base of the leg with the transformation from the base of the leg to the foot which is a function of the joint angles (which you control and so should be known). $${}^{body}T_{foot} = {}^{body}T_{leg} * {}^{leg}T_{foot}(\theta_1,\theta_2,\theta_3)$$ Now if you want to move the body, create a transformation matrix representing that movement ${}^{body}T_{bodynew}$ and use it to find out the required position (relative to the current body) that the leg base frame would need to have $${}^{body}T_{legnew} = {}^{body}T_{bodynew} * {}^{body}T_{leg}$$ in order to produce this body position.

Then find out where the foot is relative to this desired leg base position $${}^{legnew}T_{foot} = ({}^{body}T_{legnew})^{-1} * {}^{body}T_{foot}$$

And you can just use your leg IK equations to find the joint angles that give this transformation. Do that for all legs and the body will move as desired.

Answer 2

For frame transformations, you need a homogenous transformation matrix (which consists of the rotation matrix and position vector between your reference frame and the target frame). I would suggest "Foundations of Robotics: Analysis and Control" by Tsuneo Yoshikawa if you can access it, or just google how to find a transformation matrix.