Calculate Inverse Dynamics only on enabled skeletons

gazebosim · Nov 2, 2020 · e5d6f57 · e5d6f57
1 parent 437aade
commit e5d6f57
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 6 deletions.
diff --git 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
@@ -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
@@ -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);
  }
 }