Hi,
I’ve not had time to work more with the Archer code. There is still some “cleaning” and tuning to do but the code work pretty good though. So I’ve decided to share the code. The code is based on Xan’s V2.0 code and Kurt’s ARC-32 code version.
The two parts I did spent some time to write and modify is the IK and the gait part.
This is the gait code:
[code]GaitSelect
;Gait selector
IF (GaitType = 0) THEN ;9 step Biped gait, Note: Half Walking cycle!
'GaitLegNr(cRightLeg) = 1
'GaitLegNr(cLeftLeg) = 8
'NrLiftedPos = 1
HalfLiftHeigth = 1
LiftedMiddlePos = 5
TLDivFactor = 13
StepsInGait = 9
NomGaitSpeed = 60
ENDIF
return
;--------------------------------------------------------------------
;[GAIT Sequence]
GaitSeq
;Check IF the Gait is in motion
GaitInMotion = ((ABS(TravelLengthX)>cTravelDeadZone) | (ABS(TravelLengthZ)>cTravelDeadZone) | (ABS(TravelRotationY)>cTravelDeadZone) )
;Don’t cycle the gait while not walking:
IF NOT Walking Then
GaitStep = 1 'Keep it to the first step until leg start walking again
;Decide what leg to set active before start walking after standing still:
IF TravelRotationY > cTravelDeadZone THEN ;The robot are commanded to walk and turn left
ActiveLeg = cRightLeg ; This means that the left leg are going to be lifted first
ELSEIF TravelRotationY < -cTravelDeadZone
ActiveLeg = cLeftLeg ; This means that the right leg are going to be lifted first
ENDIF
;Same method as for the TravelRotationY, but making sure that sidewalking has higher priority then TravelRotation by placing the code after:
IF TravelLengthX > cTravelDeadZone THEN ;The robot are commanded to sidewalk to the left
ActiveLeg = cRightLeg ; This means that the left leg are going to be lifted first
ELSEIF TravelLengthX < -cTravelDeadZone
ActiveLeg = cLeftLeg
ENDIF
ENDIF
;Calculate Gait sequence
LastLeg = 0
for LegIndex = 0 to 1 ; for all legs
if LegIndex = 1 then ; last leg
LastLeg = 1
endif
GOSUB BipedGAIT [LegIndex]
next ; next leg
return
;--------------------------------------------------------------------
;[BipedGAIT], Half walk Cycle !
; Note: The first and the last GaitStep are used for shifting COG (SC), both leg (feet) are also on ground and walking while SC
BipedGAIT [GaitCurrentLegNr]
;Clear values under the cTravelDeadZone
IF (GaitInMotion=0) THEN
TravelLengthX=0
TravelLengthZ=0
TravelRotationY=0
ENDIF
;Prevent that the feet crashes into eachother while doing sidewalking:
;The robot are commanded to sidewalk to the right and the opposite leg (now the left leg) are the active leg:
IF (TravelLengthX > cMaxOppositeTravel) AND (ActiveLeg = cLeftLeg) THEN
TravelLengthX = cMaxOppositeTravel
ELSEIF (TravelLengthX < -cMaxOppositeTravel) AND (ActiveLeg = cRightLeg)
TravelLengthX = -cMaxOppositeTravel
ENDIF
IF ActiveLeg = GaitCurrentLegNr THEN 'Is this leg the active walking leg?
IF ((GaitStep = 1) or (GaitStep = 2)) THEN 'SC towards the active leg + Walk
'Shift COG routine here:
IF ActiveLeg = cRightLeg THEN
'Shift COG over to the right leg:
CogShifterAngle1 = -cSliderHalfMaxMinDeg * GaitStep
SCBodyPosX = -CogShifterAngle1/cSCBPX 'just for testing…
ELSE
'Shift COG over to the left leg:
CogShifterAngle1 = cSliderHalfMaxMinDeg * GaitStep
SCBodyPosX = -CogShifterAngle1/cSCBPX 'just for testing…
ENDIF
'Call walk sub
GOSUB WalkInDirection [GaitCurrentLegNr]
ELSEIF ((GaitStep = StepsInGait) or (GaitStep = (StepsInGait-1))) 'SC towards the passive leg + Walk
'Shift COG routine here:
IF ActiveLeg = cRightLeg THEN
'Shift COG over to the left leg:
CogShifterAngle1 = -cSliderHalfMaxMinDeg * (StepsInGait - GaitStep + 1) ;GaitStep = (StepsInGait-1) => cSliderHalfMaxMinDeg*2
SCBodyPosX = -CogShifterAngle1/cSCBPX 'just for testing…
ELSE
'Shift COG over to the right leg:
CogShifterAngle1 = cSliderHalfMaxMinDeg * (StepsInGait - GaitStep + 1)
SCBodyPosX = -CogShifterAngle1/cSCBPX 'just for testing…
ENDIF
'Call walk sub
GOSUB WalkInDirection [GaitCurrentLegNr]
ELSE ’ Walking only:
'Call walk sub
GOSUB WalkInDirection [GaitCurrentLegNr]
ENDIF
ELSE 'The current leg is not the active, its moving into the lifting state:
CompLiftToe = FALSE ; Reset
IF ((GaitStep = 1) or (GaitStep = 2) or (GaitStep = StepsInGait) or (GaitStep = (StepsInGait-1))) THEN
'Call walk sub only
GOSUB WalkInDirection [GaitCurrentLegNr]
GearSlopON = False 'don’t compensate
ELSE 'Lifting state, lift leg and move it towards the walking direction:
;At this moment the gait is not very universal…
GearSlopON = True 'compensate for gear slop / gear backlash
IF (GaitStep = (LiftedMiddlePos-2)) & GaitInMotion THEN
GaitPosX(GaitCurrentLegNr) = -TravelLengthX/2
GaitPosY(GaitCurrentLegNr) = -LegLiftHeight/(HalfLiftHeigth+1)
GaitPosZ(GaitCurrentLegNr) = -TravelLengthZ/2
GaitRotY(GaitCurrentLegNr) = -TravelRotationY/2
ELSEIF (GaitStep = (LiftedMiddlePos-1)) & GaitInMotion
GaitPosX(GaitCurrentLegNr) = -TravelLengthX/4
GaitPosY(GaitCurrentLegNr) = -LegLiftHeight
GaitPosZ(GaitCurrentLegNr) = -TravelLengthZ/4
GaitRotY(GaitCurrentLegNr) = -TravelRotationY/4
;Leg middle up position:
;Gait in motion | Gait NOT in motion, return to home position:
ELSEIF ((GaitInMotion & (GaitStep = LiftedMiddlePos)) | (NOT GaitInMotion & GaitStep=LiftedMiddlePos & ((ABS(GaitPosX(GaitCurrentLegNr))>2) | (ABS(GaitPosZ(GaitCurrentLegNr))>2) | (ABS(GaitRotY(GaitCurrentLegNr))>2))))
GaitPosX(GaitCurrentLegNr) = 0
GaitPosY(GaitCurrentLegNr) = -LegLiftHeight
GaitPosZ(GaitCurrentLegNr) = 0
GaitRotY(GaitCurrentLegNr) = 0
ELSEIF (GaitStep = (LiftedMiddlePos+1)) & GaitInMotion
GaitPosX(GaitCurrentLegNr) = TravelLengthX/4
GaitPosY(GaitCurrentLegNr) = -LegLiftHeight
GaitPosZ(GaitCurrentLegNr) = TravelLengthZ/4
GaitRotY(GaitCurrentLegNr) = TravelRotationY/4
IF TravelLengthZ <-cTravelDeadZone THEN 'Only comp while walking forward
CompLiftToe = TRUE
ENDIF
;Leg front down position:
ELSEIF (GaitStep = (LiftedMiddlePos+2))& (GaitPosY(GaitCurrentLegNr)<0)
GaitPosX(GaitCurrentLegNr) = TravelLengthX/2
GaitPosY(GaitCurrentLegNr) = -LegLiftHeight/(HalfLiftHeigth+1)
GaitPosZ(GaitCurrentLegNr) = TravelLengthZ/2
GaitRotY(GaitCurrentLegNr) = TravelRotationY/2
IF TravelLengthZ <-cTravelDeadZone THEN
CompLiftToe = TRUE
ENDIF
ENDIF
ENDIF
ENDIF
IF NOT Walking THEN 'this must be improved and integrated to the gait cycle better
CogShifterAngle1 = 0
SCBodyPosX = 0
ENDIF
;Advance to the next step
IF LastLeg THEN ;The last leg in this step
GaitStep = GaitStep+1
IF GaitStep>StepsInGait THEN
GaitStep = 1
IF ActiveLeg = cRightLeg THEN 'At the end of a half walkcycle toogle active leg
ActiveLeg = cLeftLeg
ELSE
ActiveLeg = cRightLeg
ENDIF
ENDIF
ENDIF
#ifdef debugGait
if GaitCurrentLegNr = cLeftLeg then
hserout “Step#:”,dec GaitStep," WalkingState:", dec Walking," GaitInMotion:", dec GaitInMotion, " ActiveLeg (0=R):", dec ActiveLeg, " TLX:", sdec TravelLengthX,|
" GaitPosX:", sdec GaitPosX(GaitCurrentLegNr) , 13]
endif
#endif
return
;--------------------------------------------------------------------
;[WalkInDirection] a part of the gait routine
WalkInDirection [GaitCurrentLegNr]
GaitPosX(GaitCurrentLegNr) = GaitPosX(GaitCurrentLegNr) - (TravelLengthX/TLDivFactor)
GaitPosY(GaitCurrentLegNr) = 0
GaitPosZ(GaitCurrentLegNr) = GaitPosZ(GaitCurrentLegNr) - (TravelLengthZ/TLDivFactor)
GaitRotY(GaitCurrentLegNr) = GaitRotY(GaitCurrentLegNr) - (TravelRotationY/TLDivFactor)
return
[/code]
For adding the 6.DOF (HipYaw rotation) I’m doing a local rotation that compensate for the HipYaw servo. In the main loop I’m also calling the local Yaw rotation (GOSUB RotateLeg()):
[code];Do IK for all Right legs
LegIndex = cRightLeg
GOSUB BodyIK -LegPosX(LegIndex)+BodyPosX+SCBodyPosX+GaitPosX(LegIndex) - TotalTransX, |
LegPosZ(LegIndex)+BodyPosZ+GaitPosZ(LegIndex) - TotalTransZ, |
LegPosY(LegIndex)+BodyPosY+GaitPosY(LegIndex) - TotalTransY, |
GaitRotY(LegIndex), LegIndex]
GOSUB RotateLeg -LegPosX(LegIndex)+BodyPosX+SCBodyPosX+GaitPosX(LegIndex) - TotalTransX, |
LegPosZ(LegIndex)+BodyPosZ+GaitPosZ(LegIndex) - TotalTransZ, GaitRotY(LegIndex)]
GOSUB LegIK [LegPosX(LegIndex)-BodyPosX-SCBodyPosX+LegRotPosX+BodyIKPosX-(GaitPosX(LegIndex) - TotalTransX), |
LegPosY(LegIndex)+BodyPosY-BodyIKPosY+GaitPosY(LegIndex) - TotalTransY, |
LegPosZ(LegIndex)+BodyPosZ-LegRotPosZ-BodyIKPosZ+GaitPosZ(LegIndex) - TotalTransZ, LegIndex]
;Do IK for all Left legs
LegIndex = cLeftLeg
GOSUB BodyIK [LegPosX(LegIndex)-BodyPosX-SCBodyPosX+GaitPosX(LegIndex) - TotalTransX, |
LegPosZ(LegIndex)+BodyPosZ+GaitPosZ(LegIndex) - TotalTransZ, |
LegPosY(LegIndex)+BodyPosY+GaitPosY(LegIndex) - TotalTransY, |
GaitRotY(LegIndex), LegIndex]
GOSUB RotateLeg [LegPosX(LegIndex)+BodyPosX+SCBodyPosX+GaitPosX(LegIndex) - TotalTransX, |
LegPosZ(LegIndex)+BodyPosZ+GaitPosZ(LegIndex) - TotalTransZ, GaitRotY(LegIndex)]
GOSUB LegIK [LegPosX(LegIndex)+BodyPosX+SCBodyPosX-LegRotPosX-BodyIKPosX+GaitPosX(LegIndex) - TotalTransX, |
LegPosY(LegIndex)+BodyPosY-BodyIKPosY+GaitPosY(LegIndex) - TotalTransY, |
LegPosZ(LegIndex)+BodyPosZ-LegRotPosZ-BodyIKPosZ+GaitPosZ(LegIndex) - TotalTransZ, LegIndex]
[/code]
This is the local rotation and the modified IK part:
[code];[Local Leg Rotation] YAW rotate only, compensate for hip rotation
RotateLeg [PosX, PosZ, RotationY]
;Calculating totals from center of the body to the feet
CPR_X = PosX
CPR_Z = PosZ
;Successive global rotation matrix:
;Math shorts for rotation: Alfa (A) = Xrotate, Beta (B) = Zrotate, Gamma (G) = Yrotate
;Sinus Alfa = sinA, cosinus Alfa = cosA. and so on…
;First calculate sinus and cosinus for Y rotation:
GOSUB GetSinCos -BodyRotY1-(RotationY*c1DEC)]
SinA4 = Sin4
CosA4 = Cos4
;Calcualtion of rotation matrix:
;Y rotation only (Yaw)
;LegRotPosX = (CPR_X- (CPR_XCosA - CPR_ZSinA))
;LegRotPosZ = (CPR_Z- (CPR_XSinA + CPR_ZCosA))
LegRotPosX = (CPR_Xc2DEC - ( CPR_Xc2DECCosA4/c4DEC - CPR_Zc2DECSinA4/c4DEC))/c2DEC
LegRotPosZ = (CPR_Zc2DEC - ( CPR_Xc2DECSinA4/c4DEC + CPR_Zc2DECCosA4/c4DEC))/c2DEC
return
;--------------------------------------------------------------------
;[LEG INVERSE KINEMATICS] Calculates the angles of the coxa, Femur and tibia for the given position of the feet
;IKFeetPosX - Input position of the Feet X
;IKFeetPosY - Input position of the Feet Y
;IKFeetPosZ - Input Position of the Feet Z
;IKSolution - Output true IF the solution is possible
;IKSolutionWarning - Output true IF the solution is NEARLY possible
;IKSolutionError - Output true IF the solution is NOT possible
;FemurAngle1 - Output Angle of Femur in degrees
;TibiaAngle1 - Output Angle of Tibia in degrees
;CoxaAngle1 - Output Angle of Coxa in degrees
LegIKLegNr var nib
cGearSlop con 50 'test value
cToeComp con 150
LegIK [IKFeetPosX, IKFeetPosY, IKFeetPosZ, LegIKLegNr]
;Calculate IKCoxaAngle and IKFeetPosXZ
GOSUB GetATan2 [IKFeetPosX, IKFeetPosY]
IF GearSlopON AND (LegIKLegNr = ActiveLeg) THEN
HipRollAngle1(LegIKLegNr) = -((ATan4*180) / 3141) +900 + cGearSlop
ELSE
HipRollAngle1(LegIKLegNr) = -((ATan4*180) / 3141) +900
ENDIF
IKFeetLocalPosY = (XYHyp2/c2DEC)-cHipVertLength
HipYawAngle1(LegIKLegNr) = BodyRotY1+(GaitRotY(LegIKLegNr)*c1DEC) ;
;Length between the Coxa and tars (foot)
;IKFeetPosXZ = XYhyp2/c2DEC
;Using GetAtan2 for solving IKA1 and IKSW
;IKA14 - Angle between SW line and the ground in radians
'GOSUB GetATan2 [IKFeetPosY, IKFeetPosXZ-cCoxaLength], IKA14
GOSUB GetArcTan2 [IKFeetPosZ, IKFeetLocalPosY], IKA14 'from felix float
'GOSUB GetATan2 [IKFeetPosZ, IKFeetLocalPosY], IKA14
;IKSW2 - Length between Femur axis and tars
'IKSW2 = XYhyp2
IKSW2 = SQR((((IKFeetPosZ)*(IKFeetPosZ))+(IKFeetLocalPosY*IKFeetLocalPosY))*c4DEC)
;IKA2 - Angle of the line S>W with respect to the Femur in radians
Temp1 = (((cFemurLength*cFemurLength) - (cTibiaLength*cTibiaLength))*c4DEC + (IKSW2*IKSW2))
Temp2 = ((2*cFemurlength)*c2DEC * IKSW2)
GOSUB GetArcCos [Temp1 / (Temp2/c4DEC) ], IKA24
;IKFemurAngle
FemurAngle1(LegIKLegNr) = -(IKA14 + IKA24) * 180 / 3141 + 900 -cFemurOffset
;IKTibiaAngle
Temp1 = (((cFemurLength*cFemurLength) + (cTibiaLength*cTibiaLength))*c4DEC - (IKSW2*IKSW2))
Temp2 = (2*cFemurlength*cTibiaLength)
GOSUB GetArcCos [Temp1 / Temp2]
TibiaAngle1(LegIKLegNr) = -(900-AngleRad4*180/3141)
;AnklePitchAngle
IF (CompLiftToe AND NOT(LegIKLegNr = ActiveLeg)) THEN
AnklePitchAngle1(LegIKLegNr) = FemurAngle1(LegIKLegNr) - TibiaAngle1(LegIKLegNr) -cAnklePitchOffset + cFemurOffset - BodyRotX1 - cToeComp/(1+SingelLegModeOn) ;Do half ToeComp in single leg mode
'high cBattLED' visual debugging
ELSE
AnklePitchAngle1(LegIKLegNr) = FemurAngle1(LegIKLegNr) - TibiaAngle1(LegIKLegNr) -cAnklePitchOffset + cFemurOffset - BodyRotX1
'low cBattLED
ENDIF
;AnkleRollAngle
IF GearSlopON AND (LegIKLegNr = ActiveLeg) THEN
IF LegIKLegNr = cRightLeg then
AnkleRollAngle1(LegIKLegNr) = -HipRollAngle1(LegIKLegNr)+ 2*cGearSlop - BodyRotZ1
ELSE
AnkleRollAngle1(LegIKLegNr) = -HipRollAngle1(LegIKLegNr)+ 2*cGearSlop + BodyRotZ1
ENDIF
ELSE
IF LegIKLegNr = cRightLeg then
AnkleRollAngle1(LegIKLegNr) = -HipRollAngle1(LegIKLegNr) - BodyRotZ1
ELSE
AnkleRollAngle1(LegIKLegNr) = -HipRollAngle1(LegIKLegNr) + BodyRotZ1
ENDIF
ENDIF
;Set the Solution quality
IF(IKSW2 < (cFemurLength+cTibiaLength-30)*c2DEC) THEN
IKSolution = 1
ELSE
IF(IKSW2 < (cFemurLength+cTibiaLength)*c2DEC) THEN
IKSolutionWarning = 1
ELSE
IKSolutionError = 1
ENDIF
ENDIF
return[/code]
I’m sorry for the lack of comments and explanations. Its just that my brain is occupied with a lot of family stuff (we are waiting our 3. son in the beginning of December) and my current secret project involving 31 servos (12x5990 + 19x5645). But I do hope some of you find this stuff useful.
New 6DOF Biped.rar (49 KB)