Inertia.From{Ellipsoid,Cylinder,Box}

src/python/inertia.hpp

@@ -98,6 +98,18 @@ namespace se3
         .staticmethod("Zero")
         .def("Random",&Inertia_fx::Random)
         .staticmethod("Random")
+        .def("FromEllipsoid", &Inertia_fx::FromEllipsoid,
+            bp::default_call_policies(), (bp::arg("mass"),
+              bp::arg("length_x"), bp::arg("length_y"), bp::arg("length_z")))
+        .staticmethod("FromEllipsoid")
+        .def("FromCylinder", &Inertia_fx::FromCylinder,
+            bp::default_call_policies(), (bp::arg("mass"),
+              bp::arg("radius"), bp::arg("length")))
+        .staticmethod("FromCylinder")
+        .def("FromBox", &Inertia_fx::FromBox,
+            bp::default_call_policies(), (bp::arg("mass"),
+              bp::arg("length_x"), bp::arg("length_y"), bp::arg("length_z")))
+        .staticmethod("FromBox")
         ;
 	  }
       

src/spatial/inertia.hpp

@@ -186,6 +186,33 @@ namespace se3
                         Vector3::Random(),
                         Symmetric3::RandomPositive());
     }
+
+    static InertiaTpl FromEllipsoid(
+        const Scalar_t m, const Scalar_t x, const Scalar_t y, const Scalar_t z)
+    {
+      Scalar_t a = m * (y*y + z*z) / 5;
+      Scalar_t b = m * (x*x + z*z) / 5;
+      Scalar_t c = m * (y*y + x*x) / 5;
+      return InertiaTpl(m, Vector3::Zero(), Symmetric3(a, 0, b, 0, 0, c));
+    }
+
+    static InertiaTpl FromCylinder(
+        const Scalar_t m, const Scalar_t r, const Scalar_t l)
+    {
+      Scalar_t a = m * (r*r / 4 + l*l / 12);
+      Scalar_t c = m * (r*r / 2);
+      return InertiaTpl(m, Vector3::Zero(), Symmetric3(a, 0, a, 0, 0, c));
+    }
+
+    static InertiaTpl FromBox(
+        const Scalar_t m, const Scalar_t x, const Scalar_t y, const Scalar_t z)
+    {
+      Scalar_t a = m * (y*y + z*z) / 12;
+      Scalar_t b = m * (x*x + z*z) / 12;
+      Scalar_t c = m * (y*y + x*x) / 12;
+      return InertiaTpl(m, Vector3::Zero(), Symmetric3(a, 0, b, 0, 0, c));
+    }
+
     
     void setRandom()
     {

unittest/tspatial.cpp

@@ -213,6 +213,7 @@ BOOST_AUTO_TEST_CASE ( test_Inertia )
   using namespace se3;
   typedef Inertia::Matrix6 Matrix6;
   typedef Inertia::Matrix3 Matrix3;
+  typedef SE3::Vector3 Vector3;
 
   Inertia aI = Inertia::Random();
   Matrix6 matI = aI;
@@ -270,6 +271,24 @@ BOOST_AUTO_TEST_CASE ( test_Inertia )
   // Test constructor (Matrix6)
   Inertia I1_bis(I1.matrix());
   BOOST_CHECK(I1.matrix().isApprox(I1_bis.matrix(), 1e-12));
+
+  // Test Inertia from ellipsoid
+  I1 = Inertia::FromEllipsoid(2., 3., 4., 5.);
+  BOOST_CHECK_EQUAL(I1.mass(), 2.);
+  BOOST_CHECK_EQUAL(I1.lever(), Vector3::Zero());
+  BOOST_CHECK_EQUAL(I1.inertia(), Symmetric3(16.4, 0., 13.6, 0., 0., 10.));
+
+  // Test Inertia from Cylinder
+  I1 = Inertia::FromCylinder(2., 4., 6.);
+  BOOST_CHECK_EQUAL(I1.mass(), 2.);
+  BOOST_CHECK_EQUAL(I1.lever(), Vector3::Zero());
+  BOOST_CHECK_EQUAL(I1.inertia(), Symmetric3(14., 0., 14., 0., 0., 16.));
+
+  // Test Inertia from Box
+  I1 = Inertia::FromBox(2., 6., 12., 18.);
+  BOOST_CHECK_EQUAL(I1.mass(), 2.);
+  BOOST_CHECK_EQUAL(I1.lever(), Vector3::Zero());
+  BOOST_CHECK_EQUAL(I1.inertia(), Symmetric3(78., 0., 60., 0., 0., 30.));
 }
 
 BOOST_AUTO_TEST_CASE ( test_ActOnSet )