diff --git a/CMakeLists.txt b/CMakeLists.txt
index 274b8217be96a1ba7241d9f488db6f79a0441b72..8037c059614759727385a9ecc56a71807a4487e5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -138,6 +138,7 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS
   algorithm/center-of-mass.hpp
   algorithm/non-linear-effects.hpp
   algorithm/joint-limits.hpp
+  algorithm/energy.hpp
   )
 
 SET(${PROJECT_NAME}_SIMULATION_HEADERS
diff --git a/src/algorithm/energy.hpp b/src/algorithm/energy.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..d1fc6de3aebc7009cc74b786eb4524f5ac14bcdb
--- /dev/null
+++ b/src/algorithm/energy.hpp
@@ -0,0 +1,56 @@
+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_energy_hpp__
+#define __se3_energy_hpp__
+
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
+
+namespace se3 {
+  inline double
+  kineticEnergy(const Model & model,
+                Data & data,
+                const Eigen::VectorXd & q,
+                const Eigen::VectorXd & v,
+                const bool update_kinematics = true);
+}
+
+/* --- Details -------------------------------------------------------------------- */
+namespace se3
+{
+   
+  inline double
+  kineticEnergy(const Model & model,
+                Data & data,
+                const Eigen::VectorXd & q,
+                const Eigen::VectorXd & v,
+                const bool update_kinematics)
+  {
+    data.kinetic_energy = 0.;
+    
+    if (update_kinematics)
+      kinematics(model,data,q,v);
+    
+    for(Model::Index i=1;i<(Model::Index)(model.nbody);++i)
+      data.kinetic_energy += model.inertias[i].vtiv(data.v[i]);
+    
+    data.kinetic_energy *= .5;
+    return data.kinetic_energy;
+  }
+}
+#endif // __se3_energy_hpp__
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index de7d12a8aecf20589d0bb66f97ee7eb80cc0684d..e4429d2c364f627c709c08216e285070aba7419d 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -1,5 +1,5 @@
 //
-// Copyright (c) 2015 CNRS
+// Copyright (c) 2015-2016 CNRS
 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
 //
 // This file is part of Pinocchio
@@ -153,6 +153,8 @@ namespace se3
 
     Eigen::VectorXd lowerPositionLimit;   // limit for joint lower position
     Eigen::VectorXd upperPositionLimit;   // limit for joint upper position
+    
+    double kinetic_energy; // kinetic energy of the model
 
     Data( const Model& ref );
 
diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt
index 088db0a40ca5277b4f962c6cee5da0edee8a32c5..395ef4becd394f9bd36f5482cc011c4e3190cd61 100644
--- a/unittest/CMakeLists.txt
+++ b/unittest/CMakeLists.txt
@@ -86,3 +86,4 @@ ENDIF(LUA5_1_FOUND)
 #ADD_UNIT_TEST(variant eigen3)
 ADD_UNIT_TEST(joints eigen3)
 ADD_UNIT_TEST(compute-all-terms eigen3)
+ADD_UNIT_TEST(energy eigen3)
diff --git a/unittest/energy.cpp b/unittest/energy.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7308d27d7794d72bbd3c1fc7bcb765f01649d8a6
--- /dev/null
+++ b/unittest/energy.cpp
@@ -0,0 +1,55 @@
+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#include "pinocchio/algorithm/energy.hpp"
+#include "pinocchio/algorithm/crba.hpp"
+
+#include "pinocchio/multibody/parser/sample-models.hpp"
+
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MODULE ComTest
+#include <boost/test/unit_test.hpp>
+#include <boost/utility/binary.hpp>
+#include "pinocchio/tools/matrix-comparison.hpp"
+#include <boost/test/floating_point_comparison.hpp>
+
+BOOST_AUTO_TEST_SUITE(EnergyTest)
+
+BOOST_AUTO_TEST_CASE(test_kinetic_energy)
+{
+  using namespace Eigen;
+  using namespace se3;
+  
+  se3::Model model;
+  se3::buildModels::humanoidSimple(model);
+  se3::Data data(model);
+  
+  VectorXd q = VectorXd::Zero(model.nq);
+  VectorXd v = VectorXd::Ones(model.nv);
+  VectorXd a = VectorXd::Ones(model.nv);
+  
+  data.M.fill(0);  crba(model,data,q);
+  data.M.triangularView<Eigen::StrictlyLower>()
+  = data.M.transpose().triangularView<Eigen::StrictlyLower>();
+  
+  double kinetic_energy_ref = 0.5*v.transpose() * data.M * v;
+  double kinetic_energy = kineticEnergy(model, data, q, v);
+  
+  BOOST_CHECK_SMALL(kinetic_energy_ref - kinetic_energy, 1e-12);
+}
+
+BOOST_AUTO_TEST_SUITE_END()