def __init__(self, geneticMaterial):
DTIndividualGeneticMaterial.__init__(self)
lp = LoadPoses(geneticMaterial)
robotConfig = LoadRobotConfiguration()
poseSize = lp.getFrameQty()
self.geneticMatrix = [[lp.getPose(i).getValue(j) for j in robotConfig.getJointsName()] for i in
xrange(poseSize)]
def __init__(self, geneticMaterial):
DTIndividualGeneticMaterial.__init__(self)
lp = LoadPoses(geneticMaterial)
cycleSize = lp.getFrameQty()
CYCLE_REPETITION = self.cyclesQty
print "cicleSize: ", cycleSize
self.geneticMatrix = [[lp.getPose(i).getValue(j) for j in self.robotConfig.getJointsName()] for i in
xrange(cycleSize)] * CYCLE_REPETITION
#for debugging info:
#self.calculateGapByInterpolation(REFERENCE_WINDOW_RADIUS, INTERPOLATION_WINDOW, SPLINE_SMOOTHING_FACTOR, cycleSize, CYCLE_REPETITION, True)
self.calculateGapByCubicInterpolation(REFERENCE_WINDOW_RADIUS, INTERPOLATION_WINDOW, SPLINE_SMOOTHING_FACTOR, cycleSize, CYCLE_REPETITION)
poseFix["R_Ankle_Pitch"] = 9
poseFix["R_Elbow_Yaw"] = 0
poseFix["R_Ankle_Roll"] = 0
poseFix["L_Shoulder_Yaw"] = 0
poseFix["L_Shoulder_Pitch"] = -70
poseFix["L_Hip_Yaw"] = 0
poseFix["L_Hip_Roll"] = 0
poseFix["L_Hip_Pitch"] = -50
poseFix["L_Knee"] = 0
poseFix["L_Ankle_Pitch"] = 9
poseFix["L_Elbow_Yaw"] = 0
poseFix["L_Ankle_Roll"] = 0
avoid_joints = ["R_Hip_Yaw", "R_Shoulder_Yaw", "L_Hip_Yaw", "L_Shoulder_Yaw", "L_Hip_Roll", "R_Hip_Roll", "L_Ankle_Roll", "R_Ankle_Roll"]
frameQty=lp.getFrameQty()
while True:
for index in range(frameQty):
pose=lp.getFramePose(index)
for joint in pose.keys():
if joint not in avoid_joints:
#print joint, pose[joint], pose[joint] + poseFix[joint]
#angleExecute.setDegreeValue(pose[joint]+ poseFix[joint])
angleExecute.setDegreeValue(pose[joint]) # TODO REMOVE this is only for moonwalking
poseExecute[joint] = angleExecute.toVrep()
#print joint, poseExecute[joint], angleExecute.getValue()
#print poseExecute['L_Ankle_Pitch'], pose['L_Ankle_Pitch']
lucy.executeFrame(poseExecute)
#print index
lucy.stopLucy()
print 'Program ended'