Merge pull request #253 from personalrobotics/feature/rogue_util

Adding Util function for RoGuE

Author: mkoval

src/prpy/util.py

@@ -1598,3 +1598,39 @@ def GetManipulatorIndex(robot, manip=None):
             manip_idx = manip.GetRobot().GetActiveManipulatorIndex()
 
     return (manip, manip_idx)
+
+def GetPointFrom(focus):
+    """
+    Given a kinbody, array or transform, returns the xyz
+    location. 
+    param focus The area to be referred to
+    """
+    #Pointing at a kinbody
+    if isinstance(focus, openravepy.KinBody):
+        with focus.GetEnv():
+            focus_trans = focus.GetTransform()
+        coord = list(focus_trans[0:3, 3])
+
+    #Pointing at a kinbody link
+    elif isinstance(focus, openravepy.KinBody.Link):
+        with focus.GetParent().GetEnv():
+            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 ValueError('Focus of the point is an unknown object')
+
+    return coord

tests/test_util.py

@@ -595,6 +595,51 @@ def test_SampleTimeGenerator_(self):
                                                 decimal=7, err_msg=error, \
                                                 verbose=True)
 
+class Test_GetPointFrom(unittest.TestCase):
+    def setUp(self):
+        self.env = openravepy.Environment()
+        
+    def tearDown(self):
+        self.env.Destroy()
+
+    def test_GetPointFrom_Kinbody(self):
+        # Check that each input type returns the correct xyz coord
+
+        # Kinbody
+        self.env.Load('data/mug1.dae')
+        mug = self.env.GetKinBody('mug')
+        mug.SetTransform(numpy.eye(4)) 
+        coord_kinbody = prpy.util.GetPointFrom(mug)
+        expected_kinbody_coords = [0, 0, 0]
+        numpy.testing.assert_array_almost_equal(coord_kinbody, expected_kinbody_coords)
+
+    def test_GetPointFrom_Space(self):
+        # Check that each input type returns the correct xyz coord
+        expected_coord = [1, 3, 4]
+
+        # Point in space
+        space_coord = numpy.array([1, 3, 4])
+        space_result = prpy.util.GetPointFrom(space_coord)
+        numpy.testing.assert_array_almost_equal(space_result, expected_coord)
+
+    def test_GetPointFrom_Transform(self):
+        # Check that each input type returns the correct xyz coord
+        expected_coord = [1, 3, 4]
+ 
+        # 4x4 Transform
+        trans_coord = numpy.eye(4)
+        trans_coord[0:3, 3] = expected_coord
+        trans_result = prpy.util.GetPointFrom(trans_coord)
+        numpy.testing.assert_array_almost_equal(trans_result, expected_coord)
+
+    def test_GetPointFrom_List(self):
+        # Check that each input type returns the correct xyz coord
+        expected_coord = [1, 3, 4]
+
+        # List
+        list_coord = [1, 3, 4]
+        list_result = prpy.util.GetPointFrom(list_coord)
+        numpy.testing.assert_array_almost_equal(list_result, expected_coord)
 
 if __name__ == '__main__':
     unittest.main()