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
