diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index cc191d3708b6549237caa52575e45da8014ef8c6..360485075f6eee0063e2cc23b79e3a0e3b12a202 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -143,6 +143,7 @@ Import/SimoxXMLFactory.cpp
 Import/RobotImporterFactory.cpp
 Import/MeshImport/STLReader.cpp
 VirtualRobot.cpp
+Tools/Gravity.cpp
 )
 
 SET(INCLUDES
@@ -258,6 +259,7 @@ SphereApproximator.h
 Import/SimoxXMLFactory.h
 Import/RobotImporterFactory.h
 Import/MeshImport/STLReader.h
+Tools/Gravity.h
 )
 
 if (Simox_USE_RBDL AND RBDL_FOUND)
diff --git a/VirtualRobot/Tools/Gravity.cpp b/VirtualRobot/Tools/Gravity.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a563eb21a161247b308b5ce5989bb08c3e43fa4b
--- /dev/null
+++ b/VirtualRobot/Tools/Gravity.cpp
@@ -0,0 +1,73 @@
+#include "Gravity.h"
+
+
+#include "../Nodes/RobotNodeRevolute.h"
+#include "../RobotNodeSet.h"
+
+using namespace VirtualRobot;
+
+Gravity::Gravity(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr rnsJoints, VirtualRobot::RobotNodeSetPtr rnsBodies) :
+    robot(robot),
+    rns(rnsJoints),
+    rnsBodies(rnsBodies)
+{
+}
+
+Gravity::~Gravity()
+{
+}
+
+std::map<std::string, float> Gravity::computeGravityTorque()
+{
+    std::map<std::string, float> torques;
+    if (!rns || !robot)
+    {
+        VR_ERROR << "Gravity compensation not properly set up" << endl;
+        return torques;
+    }
+
+    std::vector<VirtualRobot::RobotNodePtr> nodes = rns->getAllRobotNodes();
+    std::vector<VirtualRobot::RobotNodePtr> nodesBodies = rnsBodies->getAllRobotNodes();
+
+    for (auto& node : nodes)
+    {
+        if (!node->isRotationalJoint())
+        {
+            VR_WARNING << "Not a rotational joint:" << node->getName() << endl;
+            continue;
+        }
+
+        VirtualRobot::RobotNodeRevolutePtr rnRevolute = boost::dynamic_pointer_cast<VirtualRobot::RobotNodeRevolute>(node);
+        Eigen::Vector3f axisGlobal = rnRevolute->getJointRotationAxis();
+        Eigen::Vector3f jointPosGlobal = rnRevolute->getGlobalPose().block(0, 3, 3, 1);
+        axisGlobal.normalize();
+
+        float gravityJoint = 0;
+
+        for (auto& body : nodesBodies)
+        {
+            if (body->getMass() <= 0)
+            {
+                VR_WARNING << "No mass for body" << body->getName() << endl;
+                continue;
+            }
+
+            if (node->hasChild(body, true))
+            {
+                Eigen::Vector3f comGlobal = body->getCoMGlobal();
+                VirtualRobot::MathTools::BaseLine<Eigen::Vector3f> l(jointPosGlobal, axisGlobal);
+                Eigen::Vector3f pointOnAxis = VirtualRobot::MathTools::nearestPointOnLine<Eigen::Vector3f>(l, comGlobal);
+                Eigen::Vector3f r = comGlobal - pointOnAxis; // vector from axis to com (change sign?)
+                r *= 0.001f; // mm -> m
+                Eigen::Vector3f F(0, 0, -9.81);
+                F *= body->getMass();
+                gravityJoint += (r.cross(F)).dot(axisGlobal);
+            }
+        }
+
+        // gravity compensation: invert the gravity torque
+        gravityJoint *= -1.0f;
+        torques[node->getName()] = gravityJoint;
+    }
+    return torques;
+}
diff --git a/VirtualRobot/Tools/Gravity.h b/VirtualRobot/Tools/Gravity.h
new file mode 100644
index 0000000000000000000000000000000000000000..d17ada3f0493ed3c7d79c1c42a7cd33f5dc41e30
--- /dev/null
+++ b/VirtualRobot/Tools/Gravity.h
@@ -0,0 +1,56 @@
+/**
+* This file is part of Simox.
+*
+* Simox 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 2 of
+* the License, or (at your option) any later version.
+*
+* Simox 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 Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package    VirtualRobot
+* @author     Nikolaus Vahrenkamp
+* @copyright  2017 Nikolaus Vahrenkamp
+*             GNU Lesser General Public License
+*
+*/
+
+#ifndef _VirtualRobot_Gravity_h_
+#define _VirtualRobot_Gravity_h_
+
+#include "../VirtualRobot.h"
+
+namespace VirtualRobot
+{
+    class Gravity
+    {
+    public:
+        Gravity(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr rnsJoints, VirtualRobot::RobotNodeSetPtr rnsBodies);
+        virtual ~Gravity();
+
+        /*!
+         * \brief computeGravityTorque Computes the torque that is needed to counteract gravity.
+         * \return name value map with the computed torques
+         */
+        std::map<std::string, float> computeGravityTorque();
+
+    protected:
+        VirtualRobot::RobotPtr robot;
+
+        // this rns is used to update the current pose of the robot
+        VirtualRobot::RobotNodeSetPtr rns;
+
+        // this rns is used to update the current pose of the robot
+        VirtualRobot::RobotNodeSetPtr rnsBodies;
+    };
+
+    typedef boost::shared_ptr<Gravity> GravityPtr;
+}
+
+#endif