Go1 Static Pose Example

example_static_pose.py

#!/usr/bin/python

import sys
import time
import math
import numpy as np

sys.path.append('../lib/python/amd64')
import robot_interface as sdk

def jointLinearInterpolation(initPos, targetPos, rate):

    rate = np.fmin(np.fmax(rate, 0.0), 1.0)
    p = initPos*(1-rate) + targetPos*rate
    return p


if __name__ == '__main__':
    
    #####
    # Initialize constants
    #####

    d = {'FR_0':0, 'FR_1':1, 'FR_2':2,
         'FL_0':3, 'FL_1':4, 'FL_2':5, 
         'RR_0':6, 'RR_1':7, 'RR_2':8, 
         'RL_0':9, 'RL_1':10, 'RL_2':11 }
    PosStopF  = math.pow(10,9)
    VelStopF  = 16000.0
    HIGHLEVEL = 0xee
    LOWLEVEL  = 0xff
    dt = 0.002
    qInit = [0] * 12
    qDes = [0] * 12
    sin_count = 0
    rate_count = 0
    Kp = [0] * 12
    Kd = [0] * 12

    udp = sdk.UDP(LOWLEVEL, 8080, "192.168.123.10", 8007)
    safe = sdk.Safety(sdk.LeggedType.Go1)
    
    cmd = sdk.LowCmd()
    state = sdk.LowState()
    udp.InitCmdData(cmd)
    
    #####
    # Control Loop
    #####
    
    # define the initial static pose
    static_pose_q = [0.0] * 12
    # extend the front right foot
    static_pose_q[1] = 1.2
    static_pose_q[2] = -2.0
    # shift the front left foot sideways and forward (adjust this to obtain a stable stance)
    static_pose_q[3] = -0.5
    static_pose_q[4] = 0.3

    # run the control loop
    Tpi = 0
    motiontime = 0
    while True:
        time.sleep(0.002)
        motiontime += 1
        
        udp.Recv()
        udp.GetRecv(state)
        
        if( motiontime >= 0):

            # first, get the initial position
            if( motiontime >= 0 and motiontime < 10):
                qInit[0] = state.motorState[d['FR_0']].q
                qInit[1] = state.motorState[d['FR_1']].q
                qInit[2] = state.motorState[d['FR_2']].q
                qInit[3] = state.motorState[d['FL_0']].q
                qInit[4] = state.motorState[d['FL_1']].q
                qInit[5] = state.motorState[d['FL_2']].q
                qInit[6] = state.motorState[d['RR_0']].q
                qInit[7] = state.motorState[d['RR_1']].q
                qInit[8] = state.motorState[d['RR_2']].q
                qInit[9] = state.motorState[d['RL_0']].q
                qInit[10] = state.motorState[d['RL_1']].q
                qInit[11] = state.motorState[d['RL_2']].q
            
            # second, move to the inital static pose with Kp Kd
            if( motiontime >= 10 and motiontime < 400):
                rate_count += 1
                rate = rate_count/200.0                       # needs count to 200
                Kp = [20] * 12
                Kd = [1] * 12
                
                for joint_id in range(12):
                    qDes[joint_id] = jointLinearInterpolation(qInit[joint_id], static_pose_q[joint_id], rate)
            
            # last, do sine wave with raised foot
            freq_Hz = 1
            freq_rad = freq_Hz * 2 * math.pi
            t = dt * sin_count
            if( motiontime >= 400):
                sin_count += 1
                sin_joint1 = 0.6 * math.sin(t*freq_rad)
                sin_joint2 = -0.9 * math.sin(t*freq_rad)
                qDes[0] = static_pose_q[0]
                qDes[1] = static_pose_q[1] + sin_joint1
                qDes[2] = static_pose_q[2] + sin_joint2
            
            for k, v in d.items():

                cmd.motorCmd[d[k]].q = qDes[v]
                cmd.motorCmd[d[k]].dq = 0
                cmd.motorCmd[d[k]].Kp = Kp[v]
                cmd.motorCmd[d[k]].Kd = Kd[v]
                cmd.motorCmd[d[k]].tau = 0.0

        # set and send the command to the robot

        if(motiontime > 10):
            safe.PowerProtect(cmd, state, 3)

        udp.SetSend(cmd)
        udp.Send()