From 437aade63f8d218ef817c834f9cdb76c4c1b859f Mon Sep 17 00:00:00 2001
From: Diego <diego.ferigo@iit.it>
Date: Thu, 22 Oct 2020 11:42:49 +0200
Subject: [PATCH 1/2] Compute Inverse Dynamics to populate joint forces

Signed-off-by: Diego Ferigo <diego.ferigo@iit.it>
---
 dartsim/src/SimulationFeatures.cc | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc
index 27f135212..810652249 100644
--- a/dartsim/src/SimulationFeatures.cc
+++ b/dartsim/src/SimulationFeatures.cc
@@ -53,6 +53,14 @@ void SimulationFeatures::WorldForwardStep(
   // TODO(MXG): Parse input
   world->step();
   // TODO(MXG): Fill in output and state
+
+  // Compute ID to populate the joint forces
+  for (size_t i = 0; i < world->getNumSkeletons(); ++i) {
+    world->getSkeleton(i)->computeInverseDynamics(
+      /*_withExternalForces=*/true,
+      /*_withDampingForces=*/true,
+      /*_withSpringForces=*/true);
+  }
 }
 
 std::vector<SimulationFeatures::ContactInternal>

From e5d6f5763dd06b2d8247e72238c2313e57a2780d Mon Sep 17 00:00:00 2001
From: Diego <diego.ferigo@iit.it>
Date: Mon, 2 Nov 2020 18:05:37 +0100
Subject: [PATCH 2/2] Calculate Inverse Dynamics only on enabled skeletons

---
 dartsim/src/Base.hh               |  2 ++
 dartsim/src/JointFeatures.cc      | 10 ++++++++++
 dartsim/src/SimulationFeatures.cc | 12 ++++++------
 3 files changed, 18 insertions(+), 6 deletions(-)

diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh
index 1e0aa88f0..0a4e129f6 100644
--- a/dartsim/src/Base.hh
+++ b/dartsim/src/Base.hh
@@ -338,6 +338,7 @@ class Base : public Implements3d<FeatureList<Feature>>
       this->links.RemoveEntity(bn);
     }
     this->models.RemoveEntity(skel);
+    this->skeletonsWithInverseDynamics.erase(skel);
     world->removeSkeleton(skel);
   }
 
@@ -347,6 +348,7 @@ class Base : public Implements3d<FeatureList<Feature>>
   public: EntityStorage<JointInfoPtr, const DartJoint*> joints;
   public: EntityStorage<ShapeInfoPtr, const DartShapeNode*> shapes;
   public: std::unordered_map<std::size_t, const dart::dynamics::Frame*> frames;
+  public: std::set<dart::dynamics::SkeletonPtr> skeletonsWithInverseDynamics;
 };
 
 }
diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc
index c15152d01..37c9d554a 100644
--- a/dartsim/src/JointFeatures.cc
+++ b/dartsim/src/JointFeatures.cc
@@ -53,6 +53,16 @@ double JointFeatures::GetJointAcceleration(
 double JointFeatures::GetJointForce(
     const Identity &_id, const std::size_t _dof) const
 {
+  // Enable the computation of Inverse Dynamics on this skeleton
+  const auto &joint = this->ReferenceInterface<JointInfo>(_id)->joint;
+  if (this->skeletonsWithInverseDynamics.find(joint->getSkeleton()) ==
+      this->skeletonsWithInverseDynamics.end())
+  {
+    auto* thisNonConst = const_cast<JointFeatures*>(this);
+    thisNonConst->skeletonsWithInverseDynamics.insert(joint->getSkeleton());
+    joint->getSkeleton()->computeInverseDynamics();
+  }
+
   return this->ReferenceInterface<JointInfo>(_id)->joint->getForce(_dof);
 }
 
diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc
index 810652249..3faa76e98 100644
--- a/dartsim/src/SimulationFeatures.cc
+++ b/dartsim/src/SimulationFeatures.cc
@@ -54,12 +54,12 @@ void SimulationFeatures::WorldForwardStep(
   world->step();
   // TODO(MXG): Fill in output and state
 
-  // Compute ID to populate the joint forces
-  for (size_t i = 0; i < world->getNumSkeletons(); ++i) {
-    world->getSkeleton(i)->computeInverseDynamics(
-      /*_withExternalForces=*/true,
-      /*_withDampingForces=*/true,
-      /*_withSpringForces=*/true);
+  for (auto &skeleton: this->skeletonsWithInverseDynamics)
+  {
+    skeleton->computeInverseDynamics(
+        /*_withExternalForces=*/true,
+        /*_withDampingForces=*/true,
+        /*_withSpringForces=*/true);
   }
 }