def GetAllPoses(): trTool = Frame() trTool.FromVctFrm3(pointerBody.measured_cp().Position()) trArm = Frame() trArm.FromVctFrm3(armBody.measured_cp().Position()) trBase = Frame() trBase.FromVctFrm3(referenceBody.measured_cp().Position()) return trTool, trArm, trBase
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