Quadruped robot is not moving properly

Hi all,

I am trying to program a quadruped robot (similar to the Mini Cheetah), and trying to get it to run forward, backward, and rotate it left and right on the spot. I am publishing the angles of the joints via ROS and into Isaac Sim via a joint state.

Although I am able to move the robot, I am experiencing a few problems with the gait of the robot:

  1. The on-the-spot rotation is slow, possibly due to the feet sliding and legs not ‘anchoring’ to the ground.
  2. Possibly for the same reason, its backward motion is slower than its forward motion, and not in a straight line, possibly for the same reason. It slightly lifts its hind legs up the air.
  3. The transition between different states (moving/not moving/rotation) is not smooth as the angles I am sending out to the robot are very different from each other between states.

Code

import rospy
from sensor_msgs.msg import JointState
import numpy as np
import time
import sys, select, termios, tty
from math import radians

rospy.init_node("mini_cheetah_bridge",anonymous=True)

pub = rospy.Publisher("/joint_command",JointState, queue_size=10)
joint_state = JointState()

joint_state.name = [
    "lf_upper_leg_joint",
    "lf_lower_leg_joint",
    "lh_upper_leg_joint",
    "lh_lower_leg_joint",
    "rf_upper_leg_joint",
    "rf_lower_leg_joint",
    "rh_upper_leg_joint",
    "rh_lower_leg_joint",
    "lf_hip_joint",
    "lh_hip_joint",
    "rf_hip_joint",
    "rh_hip_joint"
    ]

num_joints = len(joint_state.name)
crouch_angle = radians(45)  #initial angle of hip joint (in radians)

velocity = 10 #how fast the robot is moving

joint_state.position = np.array([crouch_angle,-2*crouch_angle,
    crouch_angle,-2*crouch_angle,
    crouch_angle,-2*crouch_angle,
    crouch_angle,-2*crouch_angle, 0, 0, 0, 0]) 

joint_state_default = joint_state.position

#print(joint_state.position)

#WALKING VARIABLES
normalized_leg_time = 0.8 #time that hip and leg change angle to lift leg (portion in walk cycle, 0 = beginning and 1 = end)
velocity = 2 #velocity feature not done yet - SHOULD BE KEPT AT 1
cycle_time = float(1/velocity)
print("Cycle Time:", cycle_time)

max_ul_angle = 60
min_ul_angle = 15
max_ll_angle = -90
min_ll_angle = -120

normalized_leg_turn_time = 0.5
max_hip_turn_angle = 5
min_hip_turn_angle = -5
max_ul_turn_angle = 60
min_ul_turn_angle = 45
max_ll_turn_angle = -90
min_ll_turn_angle = -120 #-135

rate = rospy.Rate(20)

joint_state.velocity = np.zeros([num_joints])
joint_state.effort = np.zeros([num_joints])
#print(joint_state.velocity)

def get_walking_pattern(x, dir, move_as_pace=False):

    hip_pattern1 = hip_pattern2 = 0
    u_leg_pattern1 = u_leg_pattern2 = crouch_angle
    l_leg_pattern1 = l_leg_pattern2 = -2*crouch_angle

    lift_leg_time = normalized_leg_time/velocity
    lift_leg_turn_time = normalized_leg_turn_time/velocity
    x_other = x + 0.5/velocity

    if dir == "forward":

        if x % cycle_time < lift_leg_time:
            u_leg_pattern1 = (radians(max_ul_angle - min_ul_angle)/lift_leg_time)*(x % cycle_time) + radians(min_ul_angle)
            l_leg_pattern1 = radians(-90) #((np.pi/6)/lift_leg_time)*(x % 1) + radians(-90)
        elif x % cycle_time < cycle_time - (cycle_time - lift_leg_time)/2:
            u_leg_pattern1 = (-radians(max_ul_angle - min_ul_angle)/(cycle_time - lift_leg_time))*(x % cycle_time) + radians(240)
            l_leg_pattern1 = (-radians(max_ll_angle - min_ll_angle)/((cycle_time - lift_leg_time)/2))*(x % cycle_time) + radians(150)
        else:
            u_leg_pattern1 = (-radians(max_ul_angle - min_ul_angle)/(cycle_time - lift_leg_time))*(x % cycle_time) + radians(240)
            l_leg_pattern1 = (radians(max_ll_angle - min_ll_angle)/((cycle_time - lift_leg_time)/2))*(x % cycle_time) + radians(-390)
        
        #print("U Leg pattern 1:",u_leg_pattern1)
        #print("\r")
        
        #print("L Leg pattern 1:",l_leg_pattern1)
        #print("\r")

        if x_other % cycle_time < lift_leg_time:
            u_leg_pattern2 = (radians(max_ul_angle - min_ul_angle)/lift_leg_time)*(x_other % cycle_time) + radians(min_ul_angle)
            l_leg_pattern2 = radians(-90) #((np.pi/6)/lift_leg_time)*(x_other % 1) + radians(-90)
        elif x_other % cycle_time < cycle_time - (cycle_time - lift_leg_time)/2:
            u_leg_pattern2 = (-radians(max_ul_angle - min_ul_angle)/(cycle_time - lift_leg_time))*(x_other % cycle_time) + radians(240)
            l_leg_pattern2 = (-radians(max_ll_angle - min_ll_angle)/((cycle_time - lift_leg_time)/2))*(x_other % cycle_time) + radians(150)
        else:
            u_leg_pattern2 = (-radians(max_ul_angle - min_ul_angle)/(cycle_time - lift_leg_time))*(x_other % cycle_time) + radians(240)
            l_leg_pattern2 = (radians(max_ll_angle - min_ll_angle)/((cycle_time - lift_leg_time)/2))*(x_other % cycle_time) + radians(-390)

        #print("U Leg pattern 2:",u_leg_pattern2)
        #print("\r")

        #print("L Leg pattern 2:",l_leg_pattern2)
        #print("\r")

    elif dir == "left":

        if x % cycle_time < lift_leg_turn_time:
            hip_pattern1 = (-radians(max_hip_turn_angle - min_hip_turn_angle)/lift_leg_turn_time)*(x % cycle_time) + radians(max_hip_turn_angle)
            hip_pattern2 = (radians(max_hip_turn_angle - min_hip_turn_angle)/lift_leg_turn_time)*(x % cycle_time) - radians(max_hip_turn_angle)
            u_leg_pattern1 = radians(min_ul_turn_angle)
            l_leg_pattern1 = radians(-90) #-radians(max_ll_turn_angle - min_ll_turn_angle)/(lift_leg_turn_time/2)*(x % cycle_time) + radians(max_ll_turn_angle)
        elif x % cycle_time < 1.5*lift_leg_turn_time:
            hip_pattern1 = (radians(max_hip_turn_angle - min_hip_turn_angle)/lift_leg_turn_time)*(x % cycle_time) + radians(-10)
            hip_pattern2 = (-radians(max_hip_turn_angle - min_hip_turn_angle)/lift_leg_turn_time)*(x % cycle_time) - radians(-10)
            u_leg_pattern1 = radians(max_ul_turn_angle - min_ul_turn_angle)/(lift_leg_turn_time/2)*(x % cycle_time) + radians(15)
            l_leg_pattern1 = -radians(max_ll_turn_angle - min_ll_turn_angle)/((cycle_time - lift_leg_turn_time)/2)*(x % cycle_time) + radians(-30)
        else:
            hip_pattern1 = (radians(max_hip_turn_angle - min_hip_turn_angle)/(cycle_time - lift_leg_turn_time))*(x % cycle_time) + radians(-10)
            hip_pattern2 = (-radians(max_hip_turn_angle - min_hip_turn_angle)/(cycle_time - lift_leg_turn_time))*(x % cycle_time) - radians(-10)
            u_leg_pattern1 = -radians(max_ul_turn_angle - min_ul_turn_angle)/(lift_leg_turn_time/2)*(x % cycle_time) + radians(105)
            l_leg_pattern1 = radians(max_ll_turn_angle - min_ll_turn_angle)/(lift_leg_turn_time/2)*(x % cycle_time) + radians(-210)

        if x_other % cycle_time < lift_leg_turn_time:
            u_leg_pattern2 = radians(min_ul_turn_angle)
            l_leg_pattern2 = radians(-90) #-radians(max_ll_turn_angle - min_ll_turn_angle)/(lift_leg_turn_time/2)*(x_other % cycle_time) + radians(max_ll_turn_angle)
        elif x_other % cycle_time < 1.5*lift_leg_turn_time:
            u_leg_pattern2 = radians(max_ul_turn_angle - min_ul_turn_angle)/(lift_leg_turn_time/2)*(x_other % cycle_time) + radians(15)
            l_leg_pattern2 = -radians(max_ll_turn_angle - min_ll_turn_angle)/((cycle_time - lift_leg_turn_time)/2)*(x_other % cycle_time) + radians(-30)
        else:
            u_leg_pattern2 = -radians(max_ul_turn_angle - min_ul_turn_angle)/(lift_leg_turn_time/2)*(x_other % cycle_time) + radians(105)
            l_leg_pattern2 = radians(max_ll_turn_angle - min_ll_turn_angle)/(lift_leg_turn_time/2)*(x_other % cycle_time) + radians(-210)

    elif dir == "right":

        if x % cycle_time < lift_leg_turn_time/2:
            hip_pattern1 = (-radians(max_hip_turn_angle - min_hip_turn_angle)/lift_leg_turn_time)*(x % cycle_time) + radians(max_hip_turn_angle)
            hip_pattern2 = (radians(max_hip_turn_angle - min_hip_turn_angle)/lift_leg_turn_time)*(x % cycle_time) - radians(max_hip_turn_angle)
            u_leg_pattern1 = radians(max_ul_turn_angle - min_ul_turn_angle)/(lift_leg_turn_time/2)*(x % cycle_time) + radians(min_ul_turn_angle)
            l_leg_pattern1 = -radians(max_ll_turn_angle - min_ll_turn_angle)/(lift_leg_turn_time/2)*(x % cycle_time) + radians(max_ll_turn_angle)
        elif x % cycle_time < lift_leg_turn_time:
            hip_pattern1 = (-radians(max_hip_turn_angle - min_hip_turn_angle)/lift_leg_turn_time)*(x % cycle_time) + radians(max_hip_turn_angle)
            hip_pattern2 = (radians(max_hip_turn_angle - min_hip_turn_angle)/lift_leg_turn_time)*(x % cycle_time) - radians(max_hip_turn_angle)
            u_leg_pattern1 = -radians(max_ul_turn_angle - min_ul_turn_angle)/(lift_leg_turn_time/2)*(x % cycle_time) + radians(75)
            l_leg_pattern1 = radians(max_ll_turn_angle - min_ll_turn_angle)/((cycle_time - lift_leg_turn_time)/2)*(x % cycle_time) + radians(-150)
        else:
            hip_pattern1 = (radians(max_hip_turn_angle - min_hip_turn_angle)/(cycle_time - lift_leg_turn_time))*(x % cycle_time) + radians(-10)
            hip_pattern2 = (-radians(max_hip_turn_angle - min_hip_turn_angle)/(cycle_time - lift_leg_turn_time))*(x % cycle_time) - radians(-10)
            u_leg_pattern1 = radians(min_ul_turn_angle)
            l_leg_pattern1 = radians(-90)

        if x_other % cycle_time < lift_leg_turn_time/2:
            l_leg_pattern2 = -radians(max_ll_turn_angle - min_ll_turn_angle)/(lift_leg_turn_time/2)*(x_other % cycle_time) + radians(max_ll_turn_angle)
            u_leg_pattern2 = radians(max_ul_turn_angle - min_ul_turn_angle)/(lift_leg_turn_time/2)*(x_other % cycle_time) + radians(min_ul_turn_angle)
        elif x_other % cycle_time < lift_leg_turn_time:
            l_leg_pattern2 = radians(max_ll_turn_angle - min_ll_turn_angle)/((cycle_time - lift_leg_turn_time)/2)*(x_other % cycle_time) + radians(-150)
            u_leg_pattern2 = -radians(max_ul_turn_angle - min_ul_turn_angle)/(lift_leg_turn_time/2)*(x_other % cycle_time) + radians(75)
        else:
            u_leg_pattern2 = radians(min_ul_turn_angle)
            l_leg_pattern2 = radians(-90)

    elif dir == "backward":

        if x % cycle_time < lift_leg_time:
            u_leg_pattern1 = (-radians(max_ul_angle - min_ul_angle)/lift_leg_time)*(x % cycle_time) + radians(max_ul_angle)
            l_leg_pattern1 = radians(-90) #((np.pi/6)/lift_leg_time)*(x % 1) + radians(-90)
        elif x % cycle_time < cycle_time - (cycle_time - lift_leg_time)/2:
            u_leg_pattern1 = (radians(max_ul_angle - min_ul_angle)/(cycle_time - lift_leg_time))*(x % cycle_time) + radians(-165)
            l_leg_pattern1 = (-radians(max_ll_angle - min_ll_angle)/((cycle_time - lift_leg_time)/2))*(x % cycle_time) + radians(150)
        else:
            u_leg_pattern1 = (radians(max_ul_angle - min_ul_angle)/(cycle_time - lift_leg_time))*(x % cycle_time) + radians(-165)
            l_leg_pattern1 = (radians(max_ll_angle - min_ll_angle)/((cycle_time - lift_leg_time)/2))*(x % cycle_time) + radians(-390)

        if x_other % cycle_time < lift_leg_time:
            u_leg_pattern2 = (-radians(max_ul_angle - min_ul_angle)/lift_leg_time)*(x_other % cycle_time) + radians(max_ul_angle)
            l_leg_pattern2 = radians(-90) #((np.pi/6)/lift_leg_time)*(x_other % 1) + radians(-90)
        elif x_other % cycle_time < cycle_time - (cycle_time - lift_leg_time)/2:
            u_leg_pattern2 = (radians(max_ul_angle - min_ul_angle)/(cycle_time - lift_leg_time))*(x_other % cycle_time) + radians(-165)
            l_leg_pattern2 = (-radians(max_ll_angle - min_ll_angle)/((cycle_time - lift_leg_time)/2))*(x_other % cycle_time) + radians(150)
        else:
            u_leg_pattern2 = (radians(max_ul_angle - min_ul_angle)/(cycle_time - lift_leg_time))*(x_other % cycle_time) + radians(-165)
            l_leg_pattern2 = (radians(max_ll_angle - min_ll_angle)/((cycle_time - lift_leg_time)/2))*(x_other % cycle_time) + radians(-390)

    if move_as_pace:
        return np.array([u_leg_pattern1, l_leg_pattern1, u_leg_pattern1, l_leg_pattern1, 
                        u_leg_pattern2, l_leg_pattern2, u_leg_pattern2, l_leg_pattern2, 
                        hip_pattern1, hip_pattern1, hip_pattern2, hip_pattern2]) #pace

    return np.array([u_leg_pattern1, l_leg_pattern1, u_leg_pattern2, l_leg_pattern2, 
                     u_leg_pattern2, l_leg_pattern2, u_leg_pattern1, l_leg_pattern1,
                     hip_pattern1, hip_pattern1, hip_pattern2, hip_pattern2]) #trot
    
def getKey(): #reads keyboard input
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, termios.tcgetattr(sys.stdin)) 
    return key

print("Use the WASD keys to control the robot.")

def control_cheetah():
    while not rospy.is_shutdown():
        key = getKey()
        key = key.lower()
        if key == 'q':
            print("Quitting program\r")
            break
        if key == 'w':
            #print("W")
            joint_state.position = get_walking_pattern(time.time(), "forward")
        elif key == 's':
            #print("S")
            joint_state.position = get_walking_pattern(time.time(), "backward")
        elif key == 'a':
            #print("A\r")
            joint_state.position = get_walking_pattern(time.time(), "left")
        elif key == 'd':
            #print("D\r")
            joint_state.position = get_walking_pattern(time.time(), "right")
        else:
            joint_state.position = joint_state_default
        #print(joint_state.position)
        pub.publish(joint_state)
        rate.sleep()

if __name__ == "__main__":
    control_cheetah()

(and on a minor note, when I run this code and exit the program , I can’t seem to type anything on the terminal and have to close it).

The reason why I am not implementing a full controller is because programming rotation and forward/backward movement is sufficient for my project, and I couldn’t find any controllers specifically for Isaac Sim (I am trying to see if there are controllers implemented for other programs that I can adapt to Isaac Sim, but with no success so far).

Would like to know what I could do now, thanks.