zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Scripting: quadruped example script

zr-v5.1
ashvath100 5 years ago committed by Randy Mackay
parent
9ea390e24b
commit
1a6a623295
1 changed files with 299 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 299
      libraries/AP_Scripting/examples/quadruped.lua

299
libraries/AP_Scripting/examples/quadruped.lua
Unescape View File

@ -0,0 +1,299 @@ @@ -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()
Write Preview
Loading…
Cancel
Save