Quadruped robot is not moving properly

user13196 · May 26, 2022, 9:56am

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:

The on-the-spot rotation is slow, possibly due to the feet sliding and legs not ‘anchoring’ to the ground.
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.
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.

hanlonm · March 7, 2023, 3:43pm

I am having the same issue you are describing. Any inisght would be greatly appreciated!

rthaker · March 10, 2023, 9:29pm

Hi!
Can you please confirm that you have reviewed this document and still having the issues: Quadruped — Omniverse Robotics documentation

Topic		Replies	Views
Curobo MotionGenStatus.INVALID_START_STATE_JOINT_LIMITS Isaac Sim isaac-sim-v4-0-0	3	41	December 14, 2024
Changed Robot orientation while spawning from USD in Isaac Sim Isaac Sim python , custom-robot	5	459	November 9, 2023
Improper movement of joints during UR arm articulation on a mobile base Isaac Gym python , custom-robot	2	712	October 25, 2023
Controlling a Four-Legged Robot Isaac Sim	6	435	December 5, 2024
Retrieving replicator value Isaac Sim	13	715	April 17, 2024
ROS Published Joint States Incorrect when Using Non-Fixed Base Isaac Sim ros , nvbugs	7	786	April 24, 2023
How to speed up the robot? Isaac Sim python	15	1421	July 13, 2022
Illegal BroadPhaseUpdateData Isaac Sim urdf-importer	26	128	August 22, 2024
How to move the vehicle to its destination by using Python? Isaac Sim python , physx , isaacsim	17	2979	May 5, 2022
Moveit2 on Isaac Sim with custom robot( turtlebot lime) Isaac Sim isaacsim , custom-robot , ros-2-humble , isaac_manipulator , isaac-sim-v4-1-0	7	83	December 12, 2024

Quadruped robot is not moving properly

Related topics