Ardupilot2/libraries/AP_Scripting/examples/quadruped.lua

-- quadruped robot script 

local L = 80  -- length of frame
local W = 150  -- width of frame 

local L_COXA = 30 --distance from coxa servo to femur servo
local L_FEMUR = 85    --distance from femur servo to tibia servo
local L_TIBIA = 125   --distance from tibia servo to foot

--body position and rotation parameters
local bodyRotX = 0
local bodyRotY = 0         
local bodyRotZ = 0    
local bodyPosX = 0     
local bodyPosY = 0            
local bodyPosZ = 0

-- starting positions of the legs
local endpoints1 = {math.cos(45/180*math.pi)*(L_COXA + L_FEMUR), math.sin(45/180*math.pi)*(L_COXA + L_FEMUR),      L_TIBIA }
local endpoints2 = {math.cos(45/180*math.pi)*(L_COXA + L_FEMUR), math.sin(-45/180*math.pi)*(L_COXA + L_FEMUR),      L_TIBIA }
local endpoints3 = {-math.cos(45/180*math.pi)*(L_COXA + L_FEMUR), math.sin(-45/180*math.pi)*(L_COXA + L_FEMUR),      L_TIBIA }
local endpoints4 = {-math.cos(45/180*math.pi)*(L_COXA + L_FEMUR), math.sin(45/180*math.pi)*(L_COXA + L_FEMUR),      L_TIBIA }

--select a gait pattern(default gait = 0)
local GaitType = 0
--lift height while walking 
local LegLiftHeight = 50  
--gait step in exectution 
local GaitStep = 0   
--initial position of the leg
local GaitLegNr = {0,0,0,0}  
local TLDivFactor = 0       
local NrLiftedPos = 0    
local LiftDivFactor = 0    
local HalfLiftHeigth = 0     
local FrontDownPos = 0			
local TravelRequest = false       
--Number of steps in gait     
local StepsInGait = 0 
local GaitStep = 0         
local GaitPosX = {0,0,0,0}         
local GaitPosY = {0,0,0,0}            
local GaitPosZ = {0,0,0,0}             
local GaitRotZ = {0,0,0,0}      
local LegIndex = 0   
local Walking = false
local X_speed = 0
local Yaw_speed = 0
local Y_speed = 0
local DeadZone = 5
local last_angle = {0,0,0,0,0,0,0,0,0,0,0,0}
local current = {0,0,0,0,0,0,0,0,0,0,0,0}
local start_time = 0
local curr_target = 0
local throttle = 3
local yaw = 4
local roll = 1
local pitch = 2
local gait = 5
local mode = 6
local max_step_factor = 40
local max_rotation_factor = 10
local max_yaw_factor = 10
local pwm = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500}
-- turn off rudder based arming/disarming
param:set_and_save('ARMING_RUDDER',0)
function Gaitselect()
    --Alternating gait
    if (GaitType == 0) then
        GaitLegNr = {1,4,1,4}
        NrLiftedPos = 2
        FrontDownPos = 1	
        LiftDivFactor = 2
        HalfLiftHeigth = 1
        TLDivFactor = 4   
        StepsInGait = 6    
    elseif (GaitType == 1) then
    -- wave gait with 28 steps
        GaitLegNr = {8,15,1,22}
        NrLiftedPos = 3
        FrontDownPos = 2	
        LiftDivFactor = 2
        HalfLiftHeigth = 3
        TLDivFactor = 24 
        StepsInGait = 28
    end 
end

-- Calculate Gait sequence
function Sequence_Gen()
    TravelRequest =(math.abs(X_speed) > DeadZone) or (math.abs(Y_speed) > DeadZone) or (math.abs(Yaw_speed) > DeadZone) 
    
    if TravelRequest then
        for LegIndex=1,4,1 
        do 
            Gaitgen(LegIndex)
        end

        GaitStep = GaitStep + 1
        if (GaitStep>StepsInGait) then
            GaitStep = 1
        end 
    else
        GaitPosX = {0,0,0,0}         
        GaitPosY = {0,0,0,0}            
        GaitPosZ = {0,0,0,0}             
        GaitRotZ = {0,0,0,0}  
    end
end

-- In order for the bot to move forward it needs to move its legs in a
-- specific order and this is repeated over and over to attain linear motion . when a
-- specific leg number is passed the Gaitgen() produces the set off values for the
-- given leg at that step , for each cycle of the gait each leg will move to a set
-- distance which is decided by the X_speed,Yaw_speed,Y_speed
function Gaitgen(moving_leg)
    local LegStep = GaitStep - GaitLegNr[moving_leg]

    if ((TravelRequest and (NrLiftedPos and 1) and 
    LegStep==0) or 
    (not TravelRequest and LegStep==0 and ((GaitPosX[moving_leg]>2) or 
    (GaitPosZ[moving_leg]>2) or (GaitRotZ[moving_leg] >2)))) 
    then
        GaitPosX[moving_leg] = 0
        GaitPosZ[moving_leg] = -LegLiftHeight
        GaitPosY[moving_leg] = 0
        GaitRotZ[moving_leg] = 0

    elseif (((NrLiftedPos==2 and LegStep==0) or (NrLiftedPos>=3 and 
    (LegStep==-1 or LegStep==(StepsInGait-1))))
    and TravelRequest)
    then
        GaitPosX[moving_leg] = -X_speed/LiftDivFactor
        GaitPosZ[moving_leg] = -3*LegLiftHeight/(3+HalfLiftHeigth)  
        GaitPosY[moving_leg] = -Y_speed/LiftDivFactor
        GaitRotZ[moving_leg] = -Yaw_speed/LiftDivFactor

    elseif ((NrLiftedPos>=2) and (LegStep==1 or LegStep==-(StepsInGait-1)) and TravelRequest)
    then
        GaitPosX[moving_leg] = X_speed/LiftDivFactor
        GaitPosZ[moving_leg] = -3*LegLiftHeight/(3+HalfLiftHeigth)  
        GaitPosY[moving_leg] = Y_speed/LiftDivFactor
        GaitRotZ[moving_leg] = Yaw_speed/LiftDivFactor

    elseif (((NrLiftedPos==5 and (LegStep==-2 ))) and TravelRequest)
    then
        GaitPosX[moving_leg] = -X_speed/2
        GaitPosZ[moving_leg] = -LegLiftHeight/2 
        GaitPosY[moving_leg] = -Y_speed/2
        GaitRotZ[moving_leg] = -Yaw_speed/2

    elseif ((NrLiftedPos==5) and (LegStep==2 or LegStep==-(StepsInGait-2)) and TravelRequest)
    then
        GaitPosX[moving_leg] = X_speed/2
        GaitPosZ[moving_leg] = -LegLiftHeight/2 
        GaitPosY[moving_leg] = Y_speed/2
        GaitRotZ[moving_leg] = Yaw_speed/2

    elseif ((LegStep==FrontDownPos or LegStep==-(StepsInGait-FrontDownPos)) and GaitPosY[moving_leg]<0)
    then
        GaitPosX[moving_leg] = X_speed/2
        GaitPosZ[moving_leg] = Y_speed/2
        GaitPosY[moving_leg] = Yaw_speed/2   
        GaitRotZ[moving_leg] = 0

    else 
        GaitPosX[moving_leg] = GaitPosX[moving_leg] - (X_speed/TLDivFactor)
        GaitPosZ[moving_leg] = 0
        GaitPosY[moving_leg] = GaitPosZ[moving_leg] - (Y_speed/TLDivFactor)
        GaitRotZ[moving_leg] = GaitRotZ[moving_leg] - (Yaw_speed/TLDivFactor)
    end
end

-- This function determines where each leg should be.
-- To calculate each legs pose this takes pose of the body
-- as input bodyRotX, bodyRotY, bodyRotZ - rotation of and body ,bodyPosX,
-- bodyPosY - offset of the center of body
function Body_FK(X , Y , Z,   Xdist, Ydist,Zrot)
    local totaldist = { X + Xdist + bodyPosX, Y + Ydist + bodyPosY }
    local distBodyCenterFeet = math.sqrt(totaldist[1]^2 + totaldist[2]^2)
    local AngleBodyCenter = math.atan(totaldist[2], totaldist[1])
    local rolly = math.tan(bodyRotY * math.pi/180) * totaldist[1]
    local pitchy = math.tan(bodyRotX * math.pi/180) * totaldist[2]

    local ansx = math.cos(AngleBodyCenter + ((bodyRotZ+Zrot)  * math.pi/180)) * distBodyCenterFeet - totaldist[1] + bodyPosX
    local ansy = math.sin(AngleBodyCenter + ((bodyRotZ+Zrot) * math.pi/180)) * distBodyCenterFeet - totaldist[2] + bodyPosY
    local ansz = rolly+pitchy + bodyPosZ
    local ans = {ansx, ansy ,ansz}
    return ans 
end 

-- Calculates the angles of servos of each joint using the output of the
-- Body_FK() function which gives the origin of each leg on the body frame
function Leg_IK(X , Y , Z)
    local coxa = math.atan(X,Y)* 180/math.pi
    local trueX = math.sqrt(X^2+ Y^2 ) - L_COXA
    local im = math.sqrt(trueX^2 + Z^2)

    local q1 = -math.atan(Z,trueX)
    local d1 = L_FEMUR^2 - L_TIBIA^2 + im^2
    local d2 = 2*L_FEMUR*im
    local q2 = math.acos(d1/d2)
    local femur = (q1+q2) * 180/math.pi

    local d1 = L_FEMUR^2 - im^2 + L_TIBIA^2
    local d2 = 2*L_TIBIA*L_FEMUR
    local tibia = (math.acos(d1/d2)-1.57) * 180/math.pi
    local ang = { coxa, -femur ,-tibia}
    return ang 
end

-- checks if the servo has moved to its expected postion 
function servo_estimate(start_time,current,last_angle)
    local target = 0
    for j = 1, 12 do
        curr_target = math.abs(current[j] - last_angle[j])
        if curr_target > target then
            target = curr_target
        end
    end    
    local target_time = target * (0.24/60) * 1000 
    local now = millis()

    if (target_time + start_time) <= now then
        return true
    else
        return false
    end   
end

-- main_IK produces the IK solution for each
-- leg joint servos by taking into consideration the initial_pos, gait offset and the
-- bodyIK values.
function main_IK()
    local ans1 = Body_FK(endpoints1[1]+GaitPosX[1], endpoints1[2]+GaitPosY[1], endpoints1[3]+GaitPosZ[1], L/2, W/2,GaitRotZ[1])
    local angles1 = Leg_IK(endpoints1[1]+ans1[1]+GaitPosX[1],endpoints1[2]+ans1[2]+GaitPosY[1], endpoints1[3]+ans1[3]+GaitPosZ[1])
    angles1 = {-45 + angles1[1],angles1[2],angles1[3]}

    local ans2 = Body_FK(endpoints2[1]+GaitPosX[2], endpoints2[2]+GaitPosY[2], endpoints2[3]+GaitPosZ[2], L/2, -W/2,GaitRotZ[2])
    local angles2 = Leg_IK(endpoints2[1]+ans2[1]+GaitPosX[2],endpoints2[2]+ans2[2]+GaitPosY[2], endpoints2[3]+ans2[3]+GaitPosZ[2])
    angles2 = {-135 + angles2[1],angles2[2],angles2[3]}

    local ans3 = Body_FK(endpoints3[1]+GaitPosX[3], endpoints3[2]+GaitPosY[3], endpoints3[3]+GaitPosZ[3], -L/2, -W/2,GaitRotZ[3])
    local angles3 = Leg_IK(endpoints3[1]+ans3[1]+GaitPosX[3],endpoints3[2]+ans3[2]+GaitPosY[3], endpoints3[3]+ans3[3]+GaitPosZ[3])
    angles3 = {135 + angles3[1],angles3[2],angles3[3]}

    local ans4 = Body_FK(endpoints4[1]+GaitPosX[4], endpoints4[2]+GaitPosY[4], endpoints4[3]+GaitPosZ[4], -L/2, W/2,GaitRotZ[4])
    local angles4 = Leg_IK(endpoints4[1]+ans4[1]+GaitPosX[4],endpoints4[2]+ans4[2]+GaitPosY[4], endpoints4[3]+ans4[3]+GaitPosZ[4])
    angles4 = {45 + angles4[1],angles4[2],angles4[3]}
    Gaitselect()
    current = {angles1[1],angles1[2],angles1[3],angles2[1],angles2[2],angles2[3],angles3[1],angles3[2],angles3[3],angles4[1],angles4[2],angles4[3]}
  
    if servo_estimate(start_time,current,last_angle) then
        start_time = millis()
        Sequence_Gen()
        last_angle = current
    end

    return angles1,angles4,angles3,angles2
end

local rest_angles = { 45, -90, 40,
                -45, -90, 40,
                45, -90, 40,
                -45, -90, 40}
local servo_direction = { 1,1,1,
                        -1,-1,-1,
                        -1,-1,1,
                        1,1,-1} 

function update()
    X_speed = vehicle:get_control_output(throttle) * max_step_factor
    Yaw_speed = vehicle:get_control_output(yaw) * max_yaw_factor

    bodyRotX = -(vehicle:get_control_output(roll) * 10)
    bodyRotY = -(vehicle:get_control_output(pitch) * 10)

    if arming:is_armed() then
        FR_angles ,  BL_angles, BR_angles, FL_angles = main_IK()

        angles = { FR_angles[1],FR_angles[2],FR_angles[3] , FL_angles[1],FL_angles[2],FL_angles[3],BR_angles[1],BR_angles[2],BR_angles[3], BL_angles[1],BL_angles[2],BL_angles[3]}
    else
        angles = rest_angles
    end

    for j = 1, 12 do
        pwm[j] = math.floor(((angles[j] * servo_direction[j] * 1000)/90) + 1500)
    end

    for i = 1, 12 do
        SRV_Channels:set_output_pwm_chan_timeout(i-1, pwm[i], 1000)
    end

    return update,10

end

gcs:send_text(0, "quadruped simulation")
return update()