# Inverse Kinematics for 4 legged robots and body translation

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

"""
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:

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]

def update_ao_val(val):
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*" )
fig.canvas.draw()

def update_a1_val(val):
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*" )
fig.canvas.draw()

# 228 81
# 116 169
def update_a2_val(val):
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*" )
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()

• Are you sure you mean "Legged" robot instead of a "4-link" robot? At least from your figure, it does not look like a quadruped robot rather a simple 4-link arm. Can you elaborate on your first question, what frame you are talking about? Mar 25 '20 at 6:24
• @Franky I mean by 4 legged robot. The example is for one leg. The frame I'm talking about is the transformation from the center of the body to the legs. Currently, the leg is placed at the origin, but it has to be placed at the edge of the body, but this requires a transformation from the center to an offset. Mar 25 '20 at 17:35
• The fourth row of your DH table confuses me. Why are you scaling x by a3 and not having a row of [0 0 0 1]? Mar 26 '20 at 8:17

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.

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.