def initLegJoints(startingJoint):
tmpTF = identityTF()
joints = [0, 0, 0, 0, 0, 0]
for j in range(0, 5):
joints[j] = Joint(startingJoint + j, tmpTF, tmpTF)
joints[5] = Joint("F", tmpTF, tmpTF) # Foot
return joints
def initSpineJoints(startingJoint):
tmpTF = identityTF()
joints = [0, 0, 0]
joints[0] = Joint(startingJoint, tmpTF, tmpTF)
joints[1] = Joint("Dummy", tmpTF, tmpTF)
joints[2] = Joint(startingJoint + 1, tmpTF, tmpTF)
return joints
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 initSpine():
tfSpineBaseInRobotBase = identityTF()
# TODO: Get this translation accurate e.g. at location of IMU
tfSpineBaseInRobotBase *= np.matrix([[1, 0, 0, -50], [0, 1, 0, 0],
[0, 0, 1, 0], [0, 0, 0, 1]])
# -45 around Y (to get from world to robot spine)
s = math.sin(-math.pi / 4)
c = math.cos(-math.pi / 4)
tfSpineBaseInRobotBase *= np.matrix([[c, 0, s, 0], [0, 1, 0, 0],
[-s, 0, c, 0], [0, 0, 0, 1]])
spineAngles = [0, 0, 0]
spine = Spine("B", initSpineJoints(21), spineAngles,
tfSpineBaseInRobotBase)
return spine
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)