def GetReference(): f = Frame() pose = referenceBody.measured_cp() if pose.GetValid(): # if visible f.FromVctFrm3(pose.Position()) f.IsValid = True #else return just the identity return f
def GetPose(): f = Frame() pose = pointerBody.GetPositionCartesian() if pose.GetValid(): # if visible f.FromVctFrm3(pose.Position()) f.IsValid = True #else return just the identity return f
def GetPoseInRef(): r = GetReference() f = Frame() pointerInRef = Frame() pose = pointerBody.measured_cp() if pose.GetValid(): # if visible f.FromVctFrm3(pose.Position()) pointerInRef = r.Inverse() * f pointerInRef.IsValid = r.IsValid #will return global pointer pose. if (not r.IsValid): print 'Reference not valid' elif (not r.IsValid): print 'Reference and pointer not valid' else: print 'Pointer not valid' #else return just the identity return pointerInRef