1
$\begingroup$

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.

enter image description here

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()
| improve this question | | | | |
$\endgroup$
  • $\begingroup$ 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? $\endgroup$ – Franky Mar 25 at 6:24
  • $\begingroup$ @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. $\endgroup$ – Prathik Gurudatt Mar 25 at 17:35
  • $\begingroup$ 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]? $\endgroup$ – SteveO Mar 26 at 8:17
0
$\begingroup$

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.

| improve this answer | | | | |
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.