Merge pull request #1043 from gabrielebndn/topic/explog

[python] Fix explog for input arrays

bindings/python/pinocchio/deprecated.py

@@ -129,40 +129,22 @@ buildGeomFromUrdf.__doc__ = (
   pin.buildGeomFromUrdf.__doc__
 ) 
 
-def potentialEnergy(model,data,q,update_kinematics=None):
-  if update_kinematics is None:
+@deprecated("This function is now deprecated and will be removed in future releases of Pinocchio. "
+            "Please change for the new function computePotentialEnergy.")
+def potentialEnergy(model,data,q,update_kinematics=True):
+  if update_kinematics:
     return pin.computePotentialEnergy(model,data,q)
   else:
-    if update_kinematics:
-      message = ("This function signature has been renamed and will be removed in future releases of Pinocchio. "
-                 "Please change for the new signature computePotentialEnergy(model,data,q).")
-      _warnings.warn(message, category=DeprecatedWarning, stacklevel=2)
-      return pin.computePotentialEnergy(model,data,q)
-    else:
-      message = ("This function signature has been renamed and will be removed in future releases of Pinocchio. "
-                 "Please change for the new signature computePotentialEnergy(model,data).")
-      _warnings.warn(message, category=DeprecatedWarning, stacklevel=2)
-      return pin.computePotentialEnergy(model,data)
+    return pin.computePotentialEnergy(model,data)
 
-potentialEnergy.__doc__ = (
-  pin.computePotentialEnergy.__doc__
-)
+potentialEnergy.__doc__ += '\n' + pin.computePotentialEnergy.__doc__
 
-def kineticEnergy(model,data,q,v,update_kinematics=None):
-  if update_kinematics is None:
+@deprecated("This function is now deprecated and will be removed in future releases of Pinocchio. "
+            "Please change for the new function computeKineticEnergy.")
+def kineticEnergy(model,data,q,v,update_kinematics=True):
+  if update_kinematics:
     return pin.computeKineticEnergy(model,data,q,v)
   else:
-    if update_kinematics:
-      message = ("This function signature has been renamed and will be removed in future releases of Pinocchio. "
-                 "Please change for the new signature computeKineticEnergy(model,data,q,v).")
-      _warnings.warn(message, category=DeprecatedWarning, stacklevel=2)
-      return pin.computeKineticEnergy(model,data,q,v)
-    else:
-      message = ("This function signature has been renamed and will be removed in future releases of Pinocchio. "
-                 "Please change for the new signature computeKineticEnergy(model,data).")
-      _warnings.warn(message, category=DeprecatedWarning, stacklevel=2)
-      return pin.computeKineticEnergy(model,data)
+    return pin.computeKineticEnergy(model,data)
 
-kineticEnergy.__doc__ = (
-  pin.computeKineticEnergy.__doc__
-)
+kineticEnergy.__doc__ += '\n' + pin.computeKineticEnergy.__doc__

bindings/python/pinocchio/explog.py

@@ -15,9 +15,9 @@ def exp(x):
     if np.isscalar(x):
         return math.exp(x)
     if isinstance(x, np.ndarray):
-        if x.shape == (6, 1):
+        if x.shape == (6, 1) or x.shape == (6,):
             return pin.exp6(pin.Motion(x))
-        if x.shape == (3, 1):
+        if x.shape == (3, 1) or x.shape == (3,):
             return pin.exp3(x)
         raise ValueError('Error only 3 and 6 vectors are allowed.')
     raise ValueError('Error exp is only defined for real, vector3, vector6 and pin.Motion objects.')

bindings/python/pinocchio/utils.py

@@ -42,6 +42,8 @@ def isapprox(a, b, epsilon=1e-6):
     if "np" in b.__class__.__dict__:
         b = b.np
     if isinstance(a, (np.ndarray, list)) and isinstance(b, (np.ndarray, list)):
+        a = np.squeeze(np.array(a))
+        b = np.squeeze(np.array(b))
         return np.allclose(a, b, epsilon)
     return abs(a - b) < epsilon
 

unittest/python/CMakeLists.txt

@@ -28,6 +28,7 @@ IF(HPP_FCL_FOUND)
   SET(${PROJECT_NAME}_PYTHON_TESTS
     ${${PROJECT_NAME}_PYTHON_TESTS}
     bindings_geometry_object
+    bindings_fcl_transform
     )
 ENDIF(HPP_FCL_FOUND)
 

unittest/python/bindings_fcl_transform.py

@@ -6,18 +6,16 @@ import numpy as np
 class TestFCLTransformConversion(unittest.TestCase):
 
     def test_from_SE3(self):
-
         M = pin.SE3.Random()
-        #fcl_transform = pin.hppfcl.Transform3f(M)
-        #
-        #self.assertEqual(M.rotation,fcl_transform.getRotation())
-        #self.assertEqual(M.translation,fcl_transform.getTranslation())
-    
-    def test_to_SE3(self):
+        fcl_transform = pin.hppfcl.Transform3f(M)
         
+        self.assertTrue((M.rotation==fcl_transform.getRotation()).all())
+        self.assertTrue((M.translation==fcl_transform.getTranslation()).all())
+
+    def test_to_SE3(self):
         fcl_transform = pin.hppfcl.Transform3f()
         M = pin.SE3(fcl_transform)
-        self.assertEqual(M.isIdentity())
+        self.assertTrue(M.isIdentity())
 
 if __name__ == '__main__':
     if pin.WITH_HPP_FCL_BINDINGS:

unittest/python/explog.py

@@ -62,16 +62,29 @@ class TestExpLog(TestCase):
         self.assertApprox(log(42), math.log(42))
         self.assertApprox(exp(log(42)), 42)
         self.assertApprox(log(exp(42)), 42)
+
         m = rand(3)
-        self.assertTrue(np.linalg.norm(m) < np.pi) # necessary for next test
+        self.assertLess(np.linalg.norm(m), np.pi) # necessary for next test
+        self.assertApprox(log(exp(m)), m)
+
+        m = np.random.rand(3)
+        self.assertLess(np.linalg.norm(m), np.pi) # necessary for next test
         self.assertApprox(log(exp(m)), m)
+
         m = pin.SE3.Random()
         self.assertApprox(exp(log(m)), m)
+
         m = rand(6)
-        self.assertTrue(np.linalg.norm(m) < np.pi) # necessary for next test (actually, only angular part)
+        self.assertLess(np.linalg.norm(m), np.pi) # necessary for next test (actually, only angular part)
         self.assertApprox(log(exp(m)), m)
+
+        m = np.random.rand(6)
+        self.assertLess(np.linalg.norm(m), np.pi) # necessary for next test (actually, only angular part)
+        self.assertApprox(log(exp(m)), m)
+
         m = eye(4)
         self.assertApprox(exp(log(m)).homogeneous, m)
+
         with self.assertRaises(ValueError):
             exp(eye(4))
         with self.assertRaises(ValueError):