def targetPitchSliderCallback(val):
# Pitch is rotation around Y
applyYawPitchRoll(
robot.legTargets[robot.selectedLeg],
0.0, #targetYawSlider.get(),
float(val),
targetRollSlider.get())
robot.runLegIK(robot.selectedLeg)
def reorientateRearLegTargets(self, legTargetsPrior=None):
# Helper transforms
BinW_T = identityTF() # Base (without orientation) in World
WinB_T = identityTF() # World in Base (without orientation)
# Set position vectors
for r in range(0, 3):
BinW_T[r, 3] = self.spine.tfSpineBaseInRobotBase[r, 3]
WinB_T[r, 3] = -self.spine.tfSpineBaseInRobotBase[r, 3]
# Rotate about spine base, in world coords
BinW_T_Rotated = deepcopy(BinW_T)
if (legTargetsPrior):
# Using "prior" targets - Currently used only in Gaits->loadTargetsStep()
# Apply yaw amount to targets which are starting from "prior" position
applyYawPitchRoll(BinW_T_Rotated, self.spine.angles[0], 0, 0)
legTargetsPrior[2] = BinW_T_Rotated * WinB_T * legTargetsPrior[2]
legTargetsPrior[3] = BinW_T_Rotated * WinB_T * legTargetsPrior[3]
# Update targets
self.legTargets = deepcopy(legTargetsPrior)
else:
# Using normal targets - Used in all other cases
# Apply yaw amount equal to difference of front spine joint angle
# from previous call (otherwise rear legs will end up spinning)
a = self.spine.angles[
0] - self.spine.prevAnglesForRearLegReorientation[0]
if abs(a) > 0.01: # Avoid drift
applyYawPitchRoll(BinW_T_Rotated, a, 0, 0)
self.legTargets[
2] = BinW_T_Rotated * WinB_T * self.legTargets[2]
self.legTargets[
3] = BinW_T_Rotated * WinB_T * self.legTargets[3]
# Update previous
self.spine.prevAnglesForRearLegReorientation = deepcopy(
self.spine.angles)
def loadTargetsStep(self, t, spineDeflection=0, adjustRP=False):
posAdjust = [-40, 0, 20] # X, Y, Z
m = 5 # Leg X, Y, Z, Roll, Pitch
n = 4 # Num. of legs
tempTargets = deepcopy(self.robot.legTargetsHome)
for i in range(0, n):
for j in range(0, 3):
tempTargets[i][j,
3] += self.gaitData[t, j + i * m] + posAdjust[j]
roll = self.gaitData[t, 3 + i * m]
pitch = self.gaitData[t, 4 + i * m]
applyYawPitchRoll(tempTargets[i], 0.0, pitch, roll)
for j in range(0, 3):
self.robot.baseTarget[j, 3] = self.robot.baseTargetHome[
j, 3] + self.gaitData[t, j + m * n]
roll = self.gaitData[t, 23]
pitch = self.gaitData[t, 24]
yaw = self.gaitData[t, 25]
applyYawPitchRoll(self.robot.baseTarget, yaw, pitch, roll)
self.robot.spine.angles[0] = self.gaitData[t, 26] + spineDeflection
self.robot.spine.angles[2] = self.gaitData[t, 27]
if adjustRP:
# Roll/pitch adjustment
roll = (self.robot.spineAngleOffsets[0] +
self.robot.spine.angles[0]) / 2.0
pitch = (self.robot.spineAngleOffsets[2] -
self.robot.spine.angles[2]) / 2.0
applyYawPitchRoll(self.robot.baseTarget, yaw, pitch, roll)
if Params.rearLegsAdjustment:
self.robot.moveBase(tempTargets)
else:
self.robot.legTargets = deepcopy(tempTargets)
self.robot.moveBase()
self.savePose(t)
def __init__(self):
self.spine = initSpine()
self.legs = initLegs()
# Link lengths "a-1" (last value is the foot offset)
self.a = [0, 29.05, 76.919, 72.96, 45.032, 33.596]
# Offsets for natural "home" position
self.spineAngleOffsets = [0, 0, -45]
self.legAngleOffsets = [0, -34, 67.5, -33.5, 0]
# Servo adjustments - To be used when sending commands to servos
# Spine - Direction
# Joint 2 needs its direction inverted
self.spineServoDirections = [1, 1, -1]
# Spine - Offset
# Joint 2: 0=-45+45
self.spineServoOffsets = [0, 0, 45]
# Legs - Direction
# Left side: Joint 2 needs its direction inverted
self.leftLegServoDirections = [1, -1, 1, 1, 1]
# Right side: All joints except for 1 and 5 are mirrors of left side
self.rightLegServoDirections = [1, 1, -1, -1, 1]
# Legs - Offsets
# Joint 2: -45=-34-11, Joint 3: 90=67.5+22.5, Joint 4: -45=-33.5-11.5
#self.legServoOffsets = [0, -11, 22.5, -11.5, 0]
self.legServoOffsets = [0, -11, 22.5, -11.5, 0]
# Targeting
self.baseTarget = None
self.baseTargetHome = None
self.baseTargetSpeed = None
n = 4
self.legTargets = [None] * n
self.legTargetsHome = [None] * n
self.legTargetSpeeds = [None] * n
self.selectedLeg = 0
# Dummy targets, because of moveBase()->runLegIK()
for i in range(0, len(self.legs)):
self.legTargets[i] = identityTF()
# Base in world
self.tfSpineBaseInWorld = identityTF()
# Base target in world
self.baseTargetHome = deepcopy(self.tfSpineBaseInWorld)
self.baseTarget = deepcopy(self.baseTargetHome)
self.baseTargetSpeed = [0, 0, 0]
self.spine.angles = deepcopy(self.spineAngleOffsets)
self.spine.prevAnglesForRearLegReorientation = deepcopy(
self.spine.angles)
self.moveBase()
for i, leg in enumerate(self.legs):
leg.angles = deepcopy(self.legAngleOffsets)
self.runLegFK(i)
# Leg targets: Foot in world
for i, leg in enumerate(self.legs):
self.legTargetsHome[i] = deepcopy(leg.joints[5].tfJointInWorld)
applyYawPitchRoll(self.legTargetsHome[i], 0, 0, 0)
self.legTargetSpeeds[i] = [0, 0, 0]
self.legTargets = deepcopy(self.legTargetsHome)
def baseYawSliderCallback(val):
roll = baseRollSlider.get()
pitch = basePitchSlider.get()
yaw = float(val)
applyYawPitchRoll(robot.baseTarget, yaw, pitch, roll)
robot.moveBase()