Merge pull request #1288 from jcarpent/topic/devel

Cover Python bindings inside the unit tests

bindings/python/algorithm/expose-aba-derivatives.cpp

@@ -4,28 +4,41 @@
 
 #include "pinocchio/bindings/python/algorithm/algorithms.hpp"
 #include "pinocchio/algorithm/aba-derivatives.hpp"
+#include "pinocchio/bindings/python/utils/eigen.hpp"
+
+#include <eigenpy/eigen-to-python.hpp>
 
 namespace pinocchio
 {
   namespace python
   {
-    void computeABADerivativesDefault(const Model & model, Data & data,
-                                      const Eigen::VectorXd & q,
-                                      const Eigen::VectorXd & v,
-                                      const Eigen::VectorXd & tau)
+  
+    namespace bp = boost::python;
+    bp::tuple computeABADerivativesDefault(const Model & model, Data & data,
+                                           const Eigen::VectorXd & q,
+                                           const Eigen::VectorXd & v,
+                                           const Eigen::VectorXd & tau)
     {
       computeABADerivatives(model,data,q,v,tau);
+      make_symmetric(data.Minv);
+      return bp::make_tuple(make_ref(data.ddq_dq),
+                            make_ref(data.ddq_dv),
+                            make_ref(data.Minv));
     }
   
     typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceAlignedVector;
   
-    void computeABADerivatives_fext(const Model & model, Data & data,
-                                    const Eigen::VectorXd & q,
-                                    const Eigen::VectorXd & v,
-                                    const Eigen::VectorXd & tau,
-                                    const ForceAlignedVector & fext)
+    bp::tuple computeABADerivatives_fext(const Model & model, Data & data,
+                                         const Eigen::VectorXd & q,
+                                         const Eigen::VectorXd & v,
+                                         const Eigen::VectorXd & tau,
+                                         const ForceAlignedVector & fext)
     {
       computeABADerivatives(model,data,q,v,tau,fext);
+      make_symmetric(data.Minv);
+      return bp::make_tuple(make_ref(data.ddq_dq),
+                            make_ref(data.ddq_dv),
+                            make_ref(data.Minv));
     }
 
     void exposeABADerivatives()
@@ -52,8 +65,7 @@ namespace pinocchio
               "Computes the ABA derivatives with external contact foces,\n"
               "store the result in data.ddq_dq, data.ddq_dv and data.Minv\n"
               "which correspond to the partial derivatives of the acceleration output with respect to the joint configuration,\n"
-              "velocity and torque vectors.\n"
-              "The forces are of type StdVec_Force.");
+              "velocity and torque vectors.\n");
     }
   } // namespace python
 } // namespace pinocchio

bindings/python/algorithm/expose-aba.cpp

 //
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2015-2020 CNRS INRIA
 //
 
 #include "pinocchio/bindings/python/algorithm/algorithms.hpp"
 #include "pinocchio/algorithm/aba.hpp"
+#include "pinocchio/bindings/python/utils/eigen.hpp"
 
 namespace pinocchio
 {
@@ -14,8 +15,7 @@ namespace pinocchio
     computeMinverse_proxy(const Model & model, Data & data, const Eigen::VectorXd & q)
     {
       computeMinverse(model,data,q);
-      data.Minv.triangularView<Eigen::StrictlyLower>() =
-      data.Minv.transpose().triangularView<Eigen::StrictlyLower>();
+      make_symmetric(data.Minv);
       return data.Minv;
     }
     

bindings/python/algorithm/expose-centroidal-derivatives.cpp

@@ -56,6 +56,7 @@ namespace pinocchio
                        "a: acceleration vector (size model.nv)"),
               "Computes the analytical derivatives of the centroidal dynamics\n"
               "with respect to the joint configuration vector, velocity and acceleration.");
+      
       bp::def("getCentroidalDynamicsDerivatives",
               getCentroidalDynamicsDerivatives_proxy,
               bp::args("Model","Data"),

bindings/python/algorithm/expose-cholesky.cpp

@@ -11,13 +11,6 @@ namespace pinocchio
   namespace python
   {
     
-    Data::RowMatrixXs computeMinv(const Model & model, const Data & data)
-    {
-      Data::RowMatrixXs res(model.nv,model.nv);
-      pinocchio::cholesky::computeMinv(model,data,res);
-      return res;
-    }
-    
     void exposeCholesky()
     {
       using namespace Eigen;
@@ -46,12 +39,6 @@ namespace pinocchio
                 "Returns the inverse of the inverse of the joint space inertia matrix using the results of the Cholesky decomposition\n"
                 "performed by cholesky.decompose. The result is stored in data.Minv.",
                 bp::return_value_policy<bp::return_by_value>());
-        
-        bp::def("computeMinv",
-                &computeMinv,
-                bp::args("Model","Data"),
-                "Returns the inverse of the inverse of the joint space inertia matrix using the results of the Cholesky decomposition\n"
-                "performed by cholesky.decompose.");
       }
       
     }

bindings/python/algorithm/expose-geometry.cpp

@@ -50,8 +50,7 @@ namespace pinocchio
       bp::def("computeDistance",&computeDistance,
               bp::args("geometry_model", "geometry_data", "pair_index"),
               "Compute the distance between the two geometry objects of a given collision pair for a GeometryModel and associated GeometryData.",
-              bp::return_value_policy<bp::return_by_value>()
-//              bp::with_custodian_and_ward_postcall<0,1>()
+              bp::with_custodian_and_ward_postcall<0,2,bp::return_value_policy<bp::reference_existing_object> >()
               );
 
       bp::def("computeDistances",

bindings/python/algorithm/expose-rnea-derivatives.cpp

@@ -4,12 +4,14 @@
 
 #include "pinocchio/bindings/python/algorithm/algorithms.hpp"
 #include "pinocchio/algorithm/rnea-derivatives.hpp"
+#include "pinocchio/bindings/python/utils/eigen.hpp"
 
 namespace pinocchio
 {
   namespace python
   {
     
+    namespace bp = boost::python;
     typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceAlignedVector;
     
     Data::MatrixXs computeGeneralizedGravityDerivatives(const Model & model, Data & data,
@@ -31,27 +33,29 @@ namespace pinocchio
       return res;
     }
     
-    void computeRNEADerivatives(const Model & model, Data & data,
-                                const Eigen::VectorXd & q,
-                                const Eigen::VectorXd & v,
-                                const Eigen::VectorXd & a)
+    bp::tuple computeRNEADerivatives(const Model & model, Data & data,
+                                     const Eigen::VectorXd & q,
+                                     const Eigen::VectorXd & v,
+                                     const Eigen::VectorXd & a)
     {
       pinocchio::computeRNEADerivatives(model,data,q,v,a);
-      // Symmetrize M
-      data.M.triangularView<Eigen::StrictlyLower>()
-      = data.M.transpose().triangularView<Eigen::StrictlyLower>();
+      make_symmetric(data.M);
+      return bp::make_tuple(make_ref(data.dtau_dq),
+                            make_ref(data.dtau_dv),
+                            make_ref(data.M));
     }
     
-    void computeRNEADerivatives_fext(const Model & model, Data & data,
-                                     const Eigen::VectorXd & q,
-                                     const Eigen::VectorXd & v,
-                                     const Eigen::VectorXd & a,
-                                     const ForceAlignedVector & fext)
+    bp::tuple computeRNEADerivatives_fext(const Model & model, Data & data,
+                                          const Eigen::VectorXd & q,
+                                          const Eigen::VectorXd & v,
+                                          const Eigen::VectorXd & a,
+                                          const ForceAlignedVector & fext)
     {
       pinocchio::computeRNEADerivatives(model,data,q,v,a,fext);
-      // Symmetrize M
-      data.M.triangularView<Eigen::StrictlyLower>()
-      = data.M.transpose().triangularView<Eigen::StrictlyLower>();
+      make_symmetric(data.M);
+      return bp::make_tuple(make_ref(data.dtau_dq),
+                            make_ref(data.dtau_dv),
+                            make_ref(data.M));
     }
     
     void exposeRNEADerivatives()

bindings/python/multibody/geometry-object.hpp

@@ -7,6 +7,7 @@
 
 #include <boost/python.hpp>
 #include <eigenpy/memory.hpp>
+#include <eigenpy/eigen-to-python.hpp>
 
 #include "pinocchio/multibody/geometry.hpp"
 
@@ -45,11 +46,8 @@ namespace pinocchio
               bp::args("self","otherGeometryObject"),
               "Copy constructor"
               ))
-        .add_property("meshScale",
-                      bp::make_getter(&GeometryObject::meshScale,
-                                      bp::return_value_policy<bp::return_by_value>()),
-                      bp::make_setter(&GeometryObject::meshScale),
-                      "Scaling parameter of the mesh.")
+        .def_readwrite("meshScale",&GeometryObject::meshScale,
+                       "Scaling parameter of the mesh.")
         .add_property("meshColor",
                       bp::make_getter(&GeometryObject::meshColor,
                                       bp::return_value_policy<bp::return_by_value>()),

bindings/python/multibody/joint/joints-models.hpp

@@ -10,6 +10,8 @@
 #include "pinocchio/multibody/joint/joint-collection.hpp"
 #include "pinocchio/multibody/joint/joint-composite.hpp"
 
+#include <eigenpy/eigen-to-python.hpp>
+
 namespace pinocchio
 {
   namespace python
@@ -30,11 +32,10 @@ namespace pinocchio
     {
       return cl
                .def(bp::init<double, double, double> (bp::args("x", "y", "z"), "Init JointModelRevoluteUnaligned from the components x, y, z of the axis"))
-               .def(bp::init<Eigen::Vector3d> (bp::args("axis"), "Init JointModelRevoluteUnaligned from an axis with x-y-z components"))
-               .add_property("axis",
-                             make_getter(&JointModelRevoluteUnaligned::axis, bp::return_value_policy<bp::return_by_value>()),
-                             make_setter(&JointModelRevoluteUnaligned::axis, bp::return_value_policy<bp::return_by_value>()),
-                             "Rotation axis of the JointModelRevoluteUnaligned.")
+               .def(bp::init<Eigen::Vector3d> (bp::args("axis"),
+                                               "Init JointModelRevoluteUnaligned from an axis with x-y-z components"))
+               .def_readwrite("axis",&JointModelRevoluteUnaligned::axis,
+                              "Rotation axis of the JointModelRevoluteUnaligned.")
                ;
     }
 
@@ -43,12 +44,12 @@ namespace pinocchio
     inline bp::class_<JointModelPrismaticUnaligned>& expose_joint_model<JointModelPrismaticUnaligned> (bp::class_<JointModelPrismaticUnaligned> & cl)
     {
       return cl
-               .def(bp::init<double, double, double> (bp::args("x", "y", "z"), "Init JointModelPrismaticUnaligned from the components x, y, z of the axis"))
-               .def(bp::init<Eigen::Vector3d> (bp::args("axis"), "Init JointModelPrismaticUnaligned from an axis with x-y-z components"))
-               .add_property("axis",
-                             make_getter(&JointModelPrismaticUnaligned::axis, bp::return_value_policy<bp::return_by_value>()),
-                             make_setter(&JointModelPrismaticUnaligned::axis, bp::return_value_policy<bp::return_by_value>()),
-                             "Translation axis of the JointModelPrismaticUnaligned.")
+               .def(bp::init<double, double, double> (bp::args("x", "y", "z"),
+                                                      "Init JointModelPrismaticUnaligned from the components x, y, z of the axis"))
+               .def(bp::init<Eigen::Vector3d> (bp::args("axis"),
+                                               "Init JointModelPrismaticUnaligned from an axis with x-y-z components"))
+               .def_readwrite("axis",&JointModelPrismaticUnaligned::axis,
+                              "Translation axis of the JointModelPrismaticUnaligned.")
                ;
     }
 

bindings/python/utils/eigen.hpp

+//
+// Copyright (c) 2020 INRIA
+//
+
+#ifndef __pinocchio_python_utils_eigen_hpp__
+#define __pinocchio_python_utils_eigen_hpp__
+
+#include "pinocchio/bindings/python/fwd.hpp"
+#include <eigenpy/eigen-to-python.hpp>
+
+namespace pinocchio
+{
+  namespace python
+  {
+  
+    template<typename Matrix>
+    Eigen::Ref<Matrix> make_ref(Eigen::PlainObjectBase<Matrix> & mat)
+    {
+      typedef Eigen::Ref<Matrix> ReturnType;
+      return ReturnType(mat);
+    }
+  
+    template<typename Matrix>
+    void make_symmetric(Eigen::MatrixBase<Matrix> & mat)
+    {
+      mat.template triangularView<Eigen::StrictlyLower>() =
+      mat.transpose().template triangularView<Eigen::StrictlyLower>();
+    }
+  
+    template<typename Matrix>
+    typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix) make_copy(const Eigen::MatrixBase<Matrix> & mat)
+    {
+      typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix) ReturnType;
+      return ReturnType(mat);
+    }
+  
+  } // namespace python
+} // namespace pinocchio
+
+#endif // ifndef __pinocchio_python_utils_eigen_hpp__

unittest/python/CMakeLists.txt

@@ -4,25 +4,38 @@
 
 SET(${PROJECT_NAME}_PYTHON_TESTS
   bindings
+
+  # Multibody
+  bindings_joint_composite
   bindings_model
   bindings_data
-  bindings_com
-  bindings_com_velocity_derivatives
-  bindings_regressor
-  bindings_dynamics
+  bindings_geometry_model
+  bindings_liegroups
+
+  # Spatial
   bindings_force
   bindings_frame
-  bindings_kinematics
   bindings_inertia
   bindings_spatial
-  bindings_joint_composite
   bindings_motion
   bindings_SE3
-  bindings_geometry_model
-  bindings_rnea
-  bindings_liegroups
   explog
   rpy
+
+  # Algo
+  bindings_com
+  bindings_com_velocity_derivatives
+  bindings_regressor
+  bindings_dynamics
+  bindings_kinematics
+  bindings_rnea
+  bindings_aba
+  
+  # Algo derivatives
+  bindings_forward_dynamics_derivatives
+  bindings_inverse_dynamics_derivatives
+  bindings_centroidal_dynamics_derivatives
+
   utils
   serialization
   version

unittest/python/bindings_aba.py

+import unittest
+from test_case import PinocchioTestCase as TestCase
+
+import pinocchio as pin
+import numpy as np
+
+class TestABA(TestCase):
+
+    def setUp(self):
+        self.model = pin.buildSampleModelHumanoidRandom()
+        self.data = self.model.createData()
+
+        qmax = np.full((self.model.nq,1),np.pi)
+        self.q = pin.randomConfiguration(self.model,-qmax,qmax)
+        self.v = np.random.rand(self.model.nv)
+        self.ddq = np.random.rand(self.model.nv)
+
+        self.fext = []
+        for _ in range(self.model.njoints):
+          self.fext.append(pin.Force.Random())
+
+    def test_aba(self):
+        model = self.model
+        ddq = pin.aba(self.model,self.data,self.q,self.v,self.ddq)
+
+        null_fext = pin.StdVec_Force()
+        for _ in range(model.njoints):
+          null_fext.append(pin.Force.Zero())
+
+        ddq_null_fext = pin.aba(self.model,self.data,self.q,self.v,self.ddq,null_fext)
+        self.assertApprox(ddq_null_fext,ddq)
+
+        null_fext_list = []
+        for f in null_fext:
+            null_fext_list.append(f)
+
+        print('size:',len(null_fext_list))
+        ddq_null_fext_list = pin.aba(self.model,self.data,self.q,self.v,self.ddq,null_fext_list)
+        self.assertApprox(ddq_null_fext_list,ddq)
+
+    def test_computeMinverse(self):
+        model = self.model
+        Minv = pin.computeMinverse(model,self.data,self.q)
+
+        data2 = model.createData()
+        M = pin.crba(model,data2,self.q)
+
+        self.assertApprox(np.linalg.inv(M),Minv)
+
+if __name__ == '__main__':
+    unittest.main()

unittest/python/bindings_centroidal_dynamics_derivatives.py

+import unittest
+from test_case import PinocchioTestCase as TestCase
+
+import pinocchio as pin
+import numpy as np
+
+class TestDeriavtives(TestCase):
+
+    def setUp(self):
+        self.model = pin.buildSampleModelHumanoidRandom()
+        self.data = self.model.createData()
+
+        qmax = np.full((self.model.nq,1),np.pi)
+        self.q = pin.randomConfiguration(self.model,-qmax,qmax)
+        self.v = np.random.rand((self.model.nv))
+        self.a = np.random.rand((self.model.nv))
+
+    def test_centroidal_derivatives(self):
+
+        res = pin.computeCentroidalDynamicsDerivatives(self.model,self.data,self.q,self.v,self.a)
+
+        self.assertTrue(len(res) == 4)
+
+        data2 = self.model.createData()
+        pin.computeCentroidalMomentumTimeVariation(self.model,data2,self.q,self.v,self.a)
+
+        self.assertApprox(self.data.hg,data2.hg)
+        self.assertApprox(self.data.dhg,data2.dhg)
+
+        data3 = self.model.createData()
+        pin.computeRNEADerivatives(self.model,data3,self.q,self.v,self.a)
+        res2 = pin.getCentroidalDynamicsDerivatives(self.model,data3)
+
+        for k in range(4):
+            self.assertApprox(res[k],res2[k])
+
+if __name__ == '__main__':
+    unittest.main()

unittest/python/bindings_forward_dynamics_derivatives.py

+import unittest
+from test_case import PinocchioTestCase as TestCase
+
+import pinocchio as pin
+import numpy as np
+
+class TestDeriavtives(TestCase):
+
+    def setUp(self):
+        self.model = pin.buildSampleModelHumanoidRandom()
+        self.data = self.model.createData()
+
+        qmax = np.full((self.model.nq,1),np.pi)
+        self.q = pin.randomConfiguration(self.model,-qmax,qmax)
+        self.v = np.random.rand((self.model.nv))
+        self.tau = np.random.rand((self.model.nv))
+
+        self.fext = []
+        for _ in range(self.model.njoints):
+          self.fext.append(pin.Force.Random())
+
+    def test_aba_derivatives(self):
+        res = pin.computeABADerivatives(self.model,self.data,self.q,self.v,self.tau)
+
+        self.assertTrue(len(res) == 3)
+
+        data2 = self.model.createData()
+        pin.aba(self.model,data2,self.q,self.v,self.tau)
+
+        self.assertApprox(self.data.ddq,data2.ddq)
+
+        # With external forces
+        res = pin.computeABADerivatives(self.model,self.data,self.q,self.v,self.tau,self.fext)
+
+        self.assertTrue(len(res) == 3)
+
+        pin.aba(self.model,data2,self.q,self.v,self.tau,self.fext)
+
+        self.assertApprox(self.data.ddq,data2.ddq)
+
+if __name__ == '__main__':
+    unittest.main()

unittest/python/bindings_inverse_dynamics_derivatives.py

+import unittest
+from test_case import PinocchioTestCase as TestCase
+
+import pinocchio as pin
+import numpy as np
+
+class TestDeriavtives(TestCase):
+
+    def setUp(self):
+        self.model = pin.buildSampleModelHumanoidRandom()
+        self.data = self.model.createData()
+
+        qmax = np.full((self.model.nq,1),np.pi)
+        self.q = pin.randomConfiguration(self.model,-qmax,qmax)
+        self.v = np.random.rand((self.model.nv))
+        self.a = np.random.rand((self.model.nv))
+
+        self.fext = []
+        for _ in range(self.model.njoints):
+          self.fext.append(pin.Force.Random())
+
+    def test_rnea_derivatives(self):
+
+        res = pin.computeRNEADerivatives(self.model,self.data,self.q,self.v,self.a)
+
+        self.assertTrue(len(res) == 3)
+
+        data2 = self.model.createData()
+        pin.rnea(self.model,data2,self.q,self.v,self.a)
+
+        self.assertApprox(self.data.ddq,data2.ddq)
+
+        # With external forces
+        res = pin.computeRNEADerivatives(self.model,self.data,self.q,self.v,self.a,self.fext)
+
+        self.assertTrue(len(res) == 3)
+
+        pin.rnea(self.model,data2,self.q,self.v,self.a,self.fext)
+
+        self.assertApprox(self.data.ddq,data2.ddq)
+
+    def test_generalized_gravity_derivatives(self):
+
+        res = pin.computeGeneralizedGravityDerivatives(self.model,self.data,self.q)
+
+        data2 = self.model.createData()
+        ref,_,_ = pin.computeRNEADerivatives(self.model,data2,self.q,self.v*0,self.a*0)
+
+        self.assertApprox(res,ref)
+
+    def test_static_torque_derivatives(self):
+
+        res = pin.computeStaticTorqueDerivatives(self.model,self.data,self.q,self.fext)
+
+        data2 = self.model.createData()
+        ref,_,_ = pin.computeRNEADerivatives(self.model,data2,self.q,self.v*0,self.a*0,self.fext)
+
+        self.assertApprox(res,ref)
+
+if __name__ == '__main__':
+    unittest.main()

unittest/python/bindings_liegroups.py

@@ -8,8 +8,10 @@ class TestLiegroupBindings(unittest.TestCase):
         R3 = pin.liegroups.R3()
         SO3 = pin.liegroups.SO3()
         R3xSO3 = R3 * SO3
+        R10 = pin.liegroups.Rn(10)
 
         self.assertEqual(R3.name, "R^3")
+        self.assertEqual(R10.name, "R^10")
         self.assertEqual(SO3.name, "SO(3)")
         self.assertEqual(R3xSO3.name, R3.name + " x " + SO3.name)
 

unittest/python/bindings_rnea.py

@@ -2,8 +2,6 @@ import unittest
 from test_case import PinocchioTestCase as TestCase
 
 import pinocchio as pin
-
-from pinocchio.utils import rand, zero
 import numpy as np
 
 class TestRNEA(TestCase):
@@ -17,6 +15,10 @@ class TestRNEA(TestCase):
         self.v = np.random.rand(self.model.nv)
         self.a = np.random.rand(self.model.nv)
 
+        self.fext = []
+        for _ in range(self.model.njoints):
+          self.fext.append(pin.Force.Random())
+
     def test_rnea(self):
         model = self.model
         tau = pin.rnea(self.model,self.data,self.q,self.v,self.a)
@@ -36,5 +38,45 @@ class TestRNEA(TestCase):
         tau_null_fext_list = pin.rnea(self.model,self.data,self.q,self.v,self.a,null_fext_list)
         self.assertApprox(tau_null_fext_list,tau)
 
+    def test_nle(self):
+        model = self.model
+
+        tau = pin.nonLinearEffects(model,self.data,self.q,self.v)
+
+        data2 = model.createData()
+        tau_ref = pin.rnea(model,data2,self.q,self.v,self.a*0)
+
+        self.assertApprox(tau,tau_ref)
+
+    def test_generalized_gravity(self):
+        model = self.model
+
+        tau = pin.computeGeneralizedGravity(model,self.data,self.q)
+
+        data2 = model.createData()
+        tau_ref = pin.rnea(model,data2,self.q,self.v*0,self.a*0)
+
+        self.assertApprox(tau,tau_ref)
+
+    def test_static_torque(self):
+        model = self.model
+
+        tau = pin.computeStaticTorque(model,self.data,self.q,self.fext)
+
+        data2 = model.createData()
+        tau_ref = pin.rnea(model,data2,self.q,self.v*0,self.a*0,self.fext)
+
+        self.assertApprox(tau,tau_ref)
+
+    def test_coriolis_matrix(self):
+        model = self.model
+
+        C = pin.computeCoriolisMatrix(model,self.data,self.q,self.v)
+
+        data2 = model.createData()
+        tau_coriolis_ref = pin.rnea(model,data2,self.q,self.v,self.a*0) - pin.rnea(model,data2,self.q,self.v*0,self.a*0)
+
+        self.assertApprox(tau_coriolis_ref,C.dot(self.v))
+
 if __name__ == '__main__':
     unittest.main()

unittest/python/bindings_spatial.py

@@ -4,7 +4,7 @@ import numpy as np
 from numpy.random import rand
 
 import pinocchio as pin
-from pinocchio import skew, unSkew
+from pinocchio import skew, unSkew, skewSquare
 
 from test_case import PinocchioTestCase
 
@@ -27,5 +27,15 @@ class TestSpatial(PinocchioTestCase):
         x, y, z = tuple(v3.tolist())
         self.assertApprox(skew(v3), np.array([[0, -z, y], [z, 0, -x], [-y, x, 0]]))
 
+    def skewSquare(self):
+        v3 = rand(3)
+
+        Mss = skewSquare(v3)
+
+        Ms = skew(v3)
+        Mss_ref = Ms.dot(Ms)
+
+        self.assertApprox(Mss,Mss_ref)
+
 if __name__ == '__main__':
     unittest.main()

unittest/python/rpy.py

@@ -26,5 +26,19 @@ class TestRPY(TestCase):
         self.assertApprox(matrixToRpy(rpyToMatrix(rpy)), rpy)
         self.assertApprox(rpyToMatrix(rpy), rpyToMatrix(float(rpy[0]), float(rpy[1]), float(rpy[2])))
 
+        try:
+            rotate('toto',10.)
+        except ValueError:
+          self.assertTrue(True)
+        else:
+          self.assertTrue(False)
+
+        try:
+            rotate('w',10.)
+        except ValueError:
+          self.assertTrue(True)
+        else:
+          self.assertTrue(False)
+
 if __name__ == '__main__':
     unittest.main()