diff --git a/VirtualRobot/examples/InverseDynamics/InverseDynamics.cpp b/VirtualRobot/examples/InverseDynamics/InverseDynamics.cpp
index 5a671b4b2b3c777e119869514b61fc572a15afd2..f8208dcec9579229279631d69c07b84d0fa88162 100644
--- a/VirtualRobot/examples/InverseDynamics/InverseDynamics.cpp
+++ b/VirtualRobot/examples/InverseDynamics/InverseDynamics.cpp
@@ -17,7 +17,6 @@ int main(int argc, char* argv[])
     std::string filename("robots/ArmarIII/ArmarIII.xml");
     VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(filename);
     VirtualRobot::RuntimeEnvironment::processCommandLine(argc, argv);
-
     cout << "Using robot at " << filename << endl;
     RobotPtr rob;
 
@@ -35,21 +34,80 @@ int main(int argc, char* argv[])
     {
 
         RobotNodeSetPtr ns = rob->getRobotNodeSet("RightArm");
+        RobotNodeSetPtr bodyNs = rob->getRobotNodeSet("RightArmHandColModel");
+//        RobotNodeSetPtr bodyNs = rob->getRobotNodeSet("RightArmCol");
 
-        VirtualRobot::Dynamics dynamics = VirtualRobot::Dynamics(ns);
+        Gravity g(rob, ns, bodyNs);
+        for(auto& pair:g.getMasses())
+        {
+            cout << pair.first <<": " << pair.second << endl;
+        }
+        VirtualRobot::Dynamics dynamics = VirtualRobot::Dynamics(ns, bodyNs, true);
+        dynamics.print();
         int nDof = dynamics.getnDoF();
-        Eigen::VectorXd q = Eigen::VectorXd::Random(nDof);
+        Eigen::VectorXd q = Eigen::VectorXd::Zero(nDof);
+        ns->setJointValues(q.cast<float>());
+        q = ns->getJointValuesEigen().cast<double>(); // get joint values with joint limits applied
         Eigen::VectorXd qdot = Eigen::VectorXd::Random(nDof);
         Eigen::VectorXd qddot = Eigen::VectorXd::Random(nDof);
         Eigen::VectorXd tau = Eigen::VectorXd::Random(nDof);
 
+        auto start = std::chrono::system_clock::now();
+        int c = 1000;
+        for (int i = 0; i < c; ++i) {
+            Eigen::VectorXd q = Eigen::VectorXd::Random(nDof);
+            Eigen::VectorXd qdot = Eigen::VectorXd::Random(nDof);
+            Eigen::VectorXd qddot = Eigen::VectorXd::Random(nDof);
+            Eigen::VectorXd tau = Eigen::VectorXd::Random(nDof);
+            Eigen::VectorXd invDyn = dynamics.getInverseDynamics(q, qdot, qddot);            
+        }
+        auto end = std::chrono::system_clock::now();
+        auto elapsed =
+            std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+        std::cout << "duration:" << (elapsed.count()/c) << '\n';
+
+        for (int i = 0; i < c; ++i) {
+//            break;
+            Eigen::VectorXd q = Eigen::VectorXd::Random(nDof);
+            Eigen::VectorXd qdot = Eigen::VectorXd::Random(nDof);
+            Eigen::VectorXd qddot = Eigen::VectorXd::Random(nDof);
+            Eigen::VectorXd tau = Eigen::VectorXd::Random(nDof);
+            ns->setJointValues(q.cast<float>());
+            q = ns->getJointValuesEigen().cast<double>(); // get joint values with joint limits applied
+            Eigen::VectorXd gravityRBDL = dynamics.getGravityMatrix(q);
+            int d=0;
+            std::vector<float> gravityVR;
+            g.computeGravityTorque(gravityVR);
+            for(auto & val: gravityVR)
+            {
+                auto diff = val- gravityRBDL(d);
+                if(std::abs(diff)> 0.01)
+                    throw std::runtime_error((std::to_string(i) + " dim: " + std::to_string(d) + " diff: " + std::to_string(diff).c_str()));
+                d++;
+            }
+
+        }
+
+        Eigen::VectorXd invDyn = dynamics.getInverseDynamics(q, qdot, qddot);
+        cout << "Joint values:\n" << q << endl;
+        ns->setJointValues(q.cast<float>());
+        cout << "Joint values in VR:\n" << q << endl;
+        std::vector<float> gravityVR;
+        g.computeGravityTorque(gravityVR);
 
-        cout << "joint torques from inverse dynamics: " << endl << dynamics.getInverseDynamics(q, qdot, qddot) << endl;
-        cout << "joint asdasdspace inertia matrix: " << endl << dynamics.getInertiaMatrix(q) << endl;
-        cout << "joint space gravitational matrix:" << endl << dynamics.getGravityMatrix(q, nDof) << endl;
-        cout << "joint space coriolis matrix:" << endl << dynamics.getCoriolisMatrix(q, qdot, nDof) << endl;
+//        cout << "joint torques from inverse dynamics: " << endl << invDyn << endl;
+        cout << "joint space inertia matrix: " << endl << dynamics.getInertiaMatrix(q) << endl;
+        cout << "joint space gravitational matrix:" << endl << dynamics.getGravityMatrix(q) << endl;
+        cout << "joint space VR gravity :" << endl;
+        int i=0;
+        for(auto & val: gravityVR)
+        {
+            cout << ns->getNode(i)->getName() << ": " << val << endl;
+            i++;
+        }
+        cout << "joint space coriolis matrix:" << endl << dynamics.getCoriolisMatrix(q, qdot) << endl;
         cout << "joint space accelerations from forward dynamics:" << endl << dynamics.getForwardDynamics(q, qdot, tau) << endl;
-        cout << "Identifier for Elbow R:" << endl << dynamics.getIdentifier("Elbow R") << endl;
+//        cout << "Identifier for Elbow R:" << endl << dynamics.getIdentifier("Elbow R") << endl;
 
     }
     else