Added joint_features common tests (#408)

Signed-off-by: ahcorde <[email protected]>
gazebosim · Sep 2, 2022 · 78cc33b · 78cc33b
1 parent 99a8c8e
commit 78cc33b
Show file tree

Hide file tree

Showing 11 changed files with 2,285 additions and 1,541 deletions.
diff --git a/bullet/src/JointFeatures.cc b/bullet/src/JointFeatures.cc
@@ -297,6 +297,15 @@ void JointFeatures::SetJointForce(
   {
     const JointInfoPtr &jointInfo = this->joints.at(_id.id);
     const int jointType = jointInfo->constraintType;
+
+    if (!std::isfinite(_value))
+    {
+      gzerr << "Invalid joint force value [" << _value << "] set on joint ["
+             << jointInfo->name << " DOF " << _dof
+             << "]. The value will be ignored\n";
+      return;
+    }
+
     switch(jointInfo->constraintType)
     {
       case static_cast<int>(::sdf::JointType::REVOLUTE) :

diff --git a/bullet/src/SimulationFeatures.cc b/bullet/src/SimulationFeatures.cc
@@ -17,22 +17,69 @@
 
 #include "SimulationFeatures.hh"
 
+#include <unordered_map>
+#include <utility>
+
 namespace gz {
 namespace physics {
 namespace bullet {
 
+/////////////////////////////////////////////////
 void SimulationFeatures::WorldForwardStep(
     const Identity &_worldID,
-    ForwardStep::Output & /*_h*/,
+    ForwardStep::Output & _h,
     ForwardStep::State & /*_x*/,
     const ForwardStep::Input & _u)
 {
-    const WorldInfoPtr &worldInfo = this->worlds.at(_worldID);
+  const WorldInfoPtr &worldInfo = this->worlds.at(_worldID);
 
-    auto *dtDur =
-      _u.Query<std::chrono::steady_clock::duration>();
+  auto *dtDur =
+    _u.Query<std::chrono::steady_clock::duration>();
+  if (dtDur)
+  {
     std::chrono::duration<double> dt = *dtDur;
-    worldInfo->world->stepSimulation(dt.count(), 1, dt.count());
+    stepSize = dt.count();
+  }
+
+  worldInfo->world->stepSimulation(this->stepSize, 1, this->stepSize);
+  this->Write(_h.Get<ChangedWorldPoses>());
+}
+
+/////////////////////////////////////////////////
+void SimulationFeatures::Write(ChangedWorldPoses &_changedPoses) const
+{
+  // remove link poses from the previous iteration
+  _changedPoses.entries.clear();
+  _changedPoses.entries.reserve(this->links.size());
+
+  std::unordered_map<std::size_t, math::Pose3d> newPoses;
+
+  for (const auto &[id, info] : this->links)
+  {
+    // make sure the link exists
+    if (info)
+    {
+      WorldPose wp;
+      wp.pose = info->pose;
+      wp.body = id;
+
+      auto iter = this->prevLinkPoses.find(id);
+      if ((iter == this->prevLinkPoses.end()) ||
+          !iter->second.Pos().Equal(wp.pose.Pos(), 1e-6) ||
+          !iter->second.Rot().Equal(wp.pose.Rot(), 1e-6))
+      {
+        _changedPoses.entries.push_back(wp);
+        newPoses[id] = wp.pose;
+      }
+      else
+        newPoses[id] = iter->second;
+    }
+  }
+
+  // Save the new poses so that they can be used to check for updates in the
+  // next iteration. Re-setting this->prevLinkPoses with the contents of
+  // newPoses ensures that we aren't caching data for links that were removed
+  this->prevLinkPoses = std::move(newPoses);
 }
 
 }  // namespace bullet

diff --git a/bullet/src/SimulationFeatures.hh b/bullet/src/SimulationFeatures.hh
@@ -18,7 +18,9 @@
 #ifndef GZ_PHYSICS_BULLET_SRC_SIMULATIONFEATURES_HH_
 #define GZ_PHYSICS_BULLET_SRC_SIMULATIONFEATURES_HH_
 
+#include <unordered_map>
 #include <vector>
+
 #include <gz/physics/ForwardStep.hh>
 
 #include "Base.hh"
@@ -40,6 +42,14 @@ class SimulationFeatures :
       ForwardStep::Output &_h,
       ForwardStep::State &_x,
       const ForwardStep::Input &_u) override;
+
+  public: void Write(ChangedWorldPoses &_changedPoses) const;
+
+  private: double stepSize = 0.001;
+
+  /// \brief link poses from the most recent pose change/update.
+  /// The key is the link's ID, and the value is the link's pose
+  private: mutable std::unordered_map<std::size_t, math::Pose3d> prevLinkPoses;
 };
 
 }  // namespace bullet