def __init__(self, file, robotConfiguration): self.file = 'file' self.config = robotConfiguration self.jointCalc = JointCalculation(file) #TODO take into account that the elbowyaw can change from sagital plane to transversal self.rElbowYaw = self.jointCalc.calculateRightSagital("rForeArm", "rShldr", "rHand") self.framesQty = len(self.rElbowYaw) self.frameVectors = {} self.frameVectors["R_Shoulder_Yaw"] = self.jointCalc.calculateTransversal("rCollar","chest", "rShldr") self.frameVectors["R_Shoulder_Pitch"] = 360 - self.jointCalc.calculateRightSagital("rShldr", "rForeArm", "abdomen" ) #swaped self.frameVectors["R_Hip_Yaw"] = self.jointCalc.calculateTransversal("rShin", "rThigh", "rFoot") self.frameVectors["R_Hip_Roll"] = self.jointCalc.calculateFrontal("rThigh", "abdomen", "rShin") self.frameVectors["R_Hip_Pitch"] = self.jointCalc.calculateLeftSagital("rThigh", "abdomen", "rShin") self.frameVectors["R_Knee"] = self.jointCalc.calculateLeftSagital("rShin", "rThigh","rFoot") self.frameVectors["R_Ankle_Pitch"] = self.jointCalc.calculateLeftSagital("rFoot", "rShin", "End Site12") self.frameVectors["R_Ankle_Roll"] = self.jointCalc.calculateFrontal("rFoot", "rShin", "End Site12") self.frameVectors["R_Elbow_Yaw"] = self.rElbowYaw self.frameVectors["L_Shoulder_Yaw"] = self.jointCalc.calculateFrontal("lCollar","chest", "lShldr") self.frameVectors["L_Shoulder_Pitch"] = 360 - self.jointCalc.calculateLeftSagital("lShldr", "lForeArm", "abdomen") #swaped self.frameVectors["L_Hip_Yaw"] = self.jointCalc.calculateTransversal("lShin", "lThigh", "lFoot") self.frameVectors["L_Hip_Roll"] = self.jointCalc.calculateFrontal("lThigh", "abdomen", "lShin") self.frameVectors["L_Hip_Pitch"] = self.jointCalc.calculateLeftSagital("lThigh", "abdomen", "lShin") self.frameVectors["L_Knee"] = self.jointCalc.calculateLeftSagital("lShin", "lThigh","lFoot") self.frameVectors["L_Ankle_Pitch"] = self.jointCalc.calculateLeftSagital("lFoot", "lShin", "End Site13") self.frameVectors["L_Ankle_Roll"] = self.jointCalc.calculateFrontal("lFoot", "lShin", "End Site13") self.frameVectors["L_Elbow_Yaw"] = self.jointCalc.calculateLeftSagital("lForeArm", "lShldr", "lHand")
def __init__(self, file, robotConfiguration): self.file = 'file' self.config = robotConfiguration self.jointCalc = JointCalculation(file) #TODO take into account that the elbowyaw can change from sagital plane to transversal self.rElbowYaw = self.jointCalc.calculateSagital("rForeArm", "rShldr", "rHand") #validado self.framesQty = len(self.rElbowYaw) self.frameVectors = {} self.frameVectors["R_Shoulder_Yaw"] = self.jointCalc.calculateTransversal("rCollar","chest", "rShldr") #validado, por ahora no usar o tener cuidado self.frameVectors["R_Shoulder_Pitch"] = self.jointCalc.calculateSagital("rShldr", "head", "rForeArm" ) #validado, revisar signo self.frameVectors["R_Hip_Yaw"] = self.jointCalc.calculateTransversal("rShin", "rThigh", "rFoot") #validado, por ahora no usar self.frameVectors["R_Hip_Roll"] = self.jointCalc.calculateFrontal("rThigh", "abdomen", "rShin") #validado self.frameVectors["R_Hip_Pitch"] = self.jointCalc.calculateSagital("rThigh", "abdomen", "rShin") #validado self.frameVectors["R_Knee"] = self.jointCalc.calculateSagital("rShin", "rThigh","rFoot") #validado self.frameVectors["R_Ankle_Pitch"] = self.jointCalc.calculateSagital("rFoot", "rShin", "End Site12") #validado self.frameVectors["R_Ankle_Roll"] = self.jointCalc.calculateFrontal("rFoot", "rShin", "End Site12") #self.nullFrameVector(self.framesQty) self.frameVectors["R_Elbow_Yaw"] = self.rElbowYaw #validado self.frameVectors["L_Shoulder_Yaw"] = self.jointCalc.calculateFrontal("lCollar","chest", "lShldr") #validado self.frameVectors["L_Shoulder_Pitch"] = self.jointCalc.calculateSagital("lShldr", "head", "lForeArm") #validado self.frameVectors["L_Hip_Yaw"] = self.jointCalc.calculateTransversal("lShin", "lThigh", "lFoot") #validado self.frameVectors["L_Hip_Roll"] = self.jointCalc.calculateFrontal("lThigh", "abdomen", "lShin") #validado self.frameVectors["L_Hip_Pitch"] = self.jointCalc.calculateSagital("lThigh", "abdomen", "lShin") #validado self.frameVectors["L_Knee"] = self.jointCalc.calculateSagital("lShin", "lThigh","lFoot") #validado self.frameVectors["L_Ankle_Pitch"] = self.jointCalc.calculateSagital("lFoot", "lShin", "End Site13") #validado self.frameVectors["L_Ankle_Roll"] = self.jointCalc.calculateFrontal("lFoot", "lShin", "End Site13") #self.nullFrameVector(self.framesQty) self.frameVectors["L_Elbow_Yaw"] = self.jointCalc.calculateSagital("lForeArm", "lShldr", "lHand") #validado
class MocapLucyMapping: def __init__(self, file, robotConfiguration): self.file = 'file' self.config = robotConfiguration self.jointCalc = JointCalculation(file) #TODO take into account that the elbowyaw can change from sagital plane to transversal self.rElbowYaw = self.jointCalc.calculateRightSagital("rForeArm", "rShldr", "rHand") self.framesQty = len(self.rElbowYaw) self.frameVectors = {} self.frameVectors["R_Shoulder_Yaw"] = self.jointCalc.calculateTransversal("rCollar","chest", "rShldr") self.frameVectors["R_Shoulder_Pitch"] = 360 - self.jointCalc.calculateRightSagital("rShldr", "rForeArm", "abdomen" ) #swaped self.frameVectors["R_Hip_Yaw"] = self.jointCalc.calculateTransversal("rShin", "rThigh", "rFoot") self.frameVectors["R_Hip_Roll"] = self.jointCalc.calculateFrontal("rThigh", "abdomen", "rShin") self.frameVectors["R_Hip_Pitch"] = self.jointCalc.calculateLeftSagital("rThigh", "abdomen", "rShin") self.frameVectors["R_Knee"] = self.jointCalc.calculateLeftSagital("rShin", "rThigh","rFoot") self.frameVectors["R_Ankle_Pitch"] = self.jointCalc.calculateLeftSagital("rFoot", "rShin", "End Site12") self.frameVectors["R_Ankle_Roll"] = self.jointCalc.calculateFrontal("rFoot", "rShin", "End Site12") self.frameVectors["R_Elbow_Yaw"] = self.rElbowYaw self.frameVectors["L_Shoulder_Yaw"] = self.jointCalc.calculateFrontal("lCollar","chest", "lShldr") self.frameVectors["L_Shoulder_Pitch"] = 360 - self.jointCalc.calculateLeftSagital("lShldr", "lForeArm", "abdomen") #swaped self.frameVectors["L_Hip_Yaw"] = self.jointCalc.calculateTransversal("lShin", "lThigh", "lFoot") self.frameVectors["L_Hip_Roll"] = self.jointCalc.calculateFrontal("lThigh", "abdomen", "lShin") self.frameVectors["L_Hip_Pitch"] = self.jointCalc.calculateLeftSagital("lThigh", "abdomen", "lShin") self.frameVectors["L_Knee"] = self.jointCalc.calculateLeftSagital("lShin", "lThigh","lFoot") self.frameVectors["L_Ankle_Pitch"] = self.jointCalc.calculateLeftSagital("lFoot", "lShin", "End Site13") self.frameVectors["L_Ankle_Roll"] = self.jointCalc.calculateFrontal("lFoot", "lShin", "End Site13") self.frameVectors["L_Elbow_Yaw"] = self.jointCalc.calculateLeftSagital("lForeArm", "lShldr", "lHand") def generateFile(self,file): root = ET.Element("root") lucy = ET.SubElement(root, "Lucy") for frameIt in xrange(self.framesQty): frame = ET.SubElement(lucy, "frame") frame.set("number" , str(frameIt)) for joint in self.config.getJointsName(): xmlJoint = ET.SubElement(frame, joint) joint_id = self.config.loadJointId(joint) pos = self.frameVectors[joint][frameIt] xmlJointAngle = xmlJoint.set("angle" , str(pos)) tree = ET.ElementTree(root) tree.write(file) def nullFrameVector(self, length): frameVector = {} for i in xrange(length): frameVector[i]=0 return frameVector