You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
299 lines
11 KiB
299 lines
11 KiB
-- 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()
|
|
|