From 1a6a62329542ce9dd8d69931933025f02b3db19d Mon Sep 17 00:00:00 2001 From: ashvath100 Date: Tue, 25 Aug 2020 21:16:55 +0900 Subject: [PATCH] AP_Scripting: quadruped example script --- libraries/AP_Scripting/examples/quadruped.lua | 299 ++++++++++++++++++ 1 file changed, 299 insertions(+) create mode 100644 libraries/AP_Scripting/examples/quadruped.lua diff --git a/libraries/AP_Scripting/examples/quadruped.lua b/libraries/AP_Scripting/examples/quadruped.lua new file mode 100644 index 0000000000..904522db2c --- /dev/null +++ b/libraries/AP_Scripting/examples/quadruped.lua @@ -0,0 +1,299 @@ +-- 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()