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
