add getPointFrom to util

Author: Rachel

src/prpy/util.py

@@ -1598,3 +1598,35 @@ def GetManipulatorIndex(robot, manip=None):
             manip_idx = manip.GetRobot().GetActiveManipulatorIndex()
 
     return (manip, manip_idx)
+
+def GetPointFrom(env, focus):
+    """
+    Given a kinbody, array or transform, returns the xyz
+    location. 
+    param env The environment where the item exists
+    param focus THe area to be referred to
+    """
+    #Pointing at an object
+    if isinstance(focus, (openravepy.KinBody, openravepy.KinBody.Link)):
+        with env:
+            focus_trans = focus.GetTransform()
+        coord = list(focus_trans[0:3, 3])
+
+    #Pointing at a point in space as numpy array
+    elif (isinstance(focus, numpy.ndarray) and (focus.ndim == 1)
+           and (len(focus) == 3)):
+        coord = list(focus)
+
+    #Pointing at point in space as 4x4 transform
+    elif isinstance(focus, numpy.ndarray) and (focus.shape == (4, 4)):
+        coord = list(focus[0:3, 3])
+
+    #Pointing at a point in space as list or tuple
+    elif (isinstance(focus, (tuple, list)) and len(focus) == 3):
+        coord = focus
+
+    else:
+        raise prpy.exceptions.PrPyException(
+                'Focus of the point is an unknown object')
+
+    return coord