diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index 208716192..2c4ef890d 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/dartsim/src/SimulationFeatures.cc @@ -59,15 +59,15 @@ void SimulationFeatures::WorldForwardStep( // TODO(MXG): Parse input world->step(); - this->WriteRequiredData(_h); + this->Write(_h.Get()); // TODO(MXG): Fill in state } -void SimulationFeatures::Write(WorldPoses &_poses) const +void SimulationFeatures::Write(ChangedWorldPoses &_changedPoses) const { // remove link poses from the previous iteration - _poses.entries.clear(); - _poses.entries.reserve(this->links.size()); + _changedPoses.entries.clear(); + _changedPoses.entries.reserve(this->links.size()); std::unordered_map newPoses; @@ -88,7 +88,7 @@ void SimulationFeatures::Write(WorldPoses &_poses) const !iter->second.Pos().Equal(wp.pose.Pos(), 1e-6) || !iter->second.Rot().Equal(wp.pose.Rot(), 1e-6)) { - _poses.entries.push_back(wp); + _changedPoses.entries.push_back(wp); newPoses[id] = wp.pose; } else diff --git a/dartsim/src/SimulationFeatures.hh b/dartsim/src/SimulationFeatures.hh index b597beed4..c89fa55c4 100644 --- a/dartsim/src/SimulationFeatures.hh +++ b/dartsim/src/SimulationFeatures.hh @@ -26,6 +26,7 @@ #include #include #include +#include #include "Base.hh" @@ -39,7 +40,8 @@ struct SimulationFeatureList : FeatureList< > { }; class SimulationFeatures : - public CanWriteRequiredData, + public CanWriteExpectedData>, public virtual Base, public virtual Implements3d { @@ -49,7 +51,7 @@ class SimulationFeatures : ForwardStep::State &_x, const ForwardStep::Input &_u) override; - public: void Write(WorldPoses &_poses) const; + public: void Write(ChangedWorldPoses &_changedPoses) const; public: std::vector GetContactsFromLastStep( const Identity &_worldID) const override; diff --git a/dartsim/src/SimulationFeatures_TEST.cc b/dartsim/src/SimulationFeatures_TEST.cc index f9001f3af..4e8ee41f5 100644 --- a/dartsim/src/SimulationFeatures_TEST.cc +++ b/dartsim/src/SimulationFeatures_TEST.cc @@ -117,7 +117,7 @@ bool StepWorld(const TestWorldPtr &_world, bool _firstTime, if (_firstTime && (i == 0)) { EXPECT_FALSE( - output.Get().entries.empty()); + output.Get().entries.empty()); checkedOutput = true; } } diff --git a/include/ignition/physics/ForwardStep.hh b/include/ignition/physics/ForwardStep.hh index 508e33dc9..f02992091 100644 --- a/include/ignition/physics/ForwardStep.hh +++ b/include/ignition/physics/ForwardStep.hh @@ -49,6 +49,15 @@ namespace ignition std::string annotation; }; + /// \brief ChangedWorldPoses has the same definition as WorldPoses. + /// This type provides a way to keep track of which poses have changed in a + /// simulation step. + struct ChangedWorldPoses + { + std::vector entries; + std::string annotation; + }; + struct Point { ignition::math::Vector3d point; @@ -150,7 +159,7 @@ namespace ignition public: using Output = SpecifyData< RequireData, - ExpectData >; + ExpectData >; public: using State = CompositeData; diff --git a/tpe/plugin/src/SimulationFeatures.cc b/tpe/plugin/src/SimulationFeatures.cc index 543c14f39..30db7b47f 100644 --- a/tpe/plugin/src/SimulationFeatures.cc +++ b/tpe/plugin/src/SimulationFeatures.cc @@ -62,14 +62,14 @@ void SimulationFeatures::WorldForwardStep( } } world->Step(); - this->WriteRequiredData(_h); + this->Write(_h.Get()); } -void SimulationFeatures::Write(WorldPoses &_poses) const +void SimulationFeatures::Write(ChangedWorldPoses &_changedPoses) const { // remove link poses from the previous iteration - _poses.entries.clear(); - _poses.entries.reserve(this->links.size()); + _changedPoses.entries.clear(); + _changedPoses.entries.reserve(this->links.size()); std::unordered_map newPoses; @@ -90,7 +90,7 @@ void SimulationFeatures::Write(WorldPoses &_poses) const WorldPose wp; wp.pose = nextPose; wp.body = id; - _poses.entries.push_back(wp); + _changedPoses.entries.push_back(wp); newPoses[id] = nextPose; } else diff --git a/tpe/plugin/src/SimulationFeatures.hh b/tpe/plugin/src/SimulationFeatures.hh index d6004110f..221c397dc 100644 --- a/tpe/plugin/src/SimulationFeatures.hh +++ b/tpe/plugin/src/SimulationFeatures.hh @@ -26,6 +26,7 @@ #include #include #include +#include #include "Base.hh" @@ -39,7 +40,8 @@ struct SimulationFeatureList : FeatureList< > { }; class SimulationFeatures : - public CanWriteRequiredData, + public CanWriteExpectedData>, public virtual Base, public virtual Implements3d { @@ -49,7 +51,7 @@ class SimulationFeatures : ForwardStep::State &_x, const ForwardStep::Input &_u) override; - public: void Write(WorldPoses &_poses) const; + public: void Write(ChangedWorldPoses &_changedPoses) const; public: std::vector GetContactsFromLastStep( const Identity &_worldID) const override; diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc index 62a635c4f..9901d09b1 100644 --- a/tpe/plugin/src/SimulationFeatures_TEST.cc +++ b/tpe/plugin/src/SimulationFeatures_TEST.cc @@ -116,7 +116,7 @@ bool StepWorld(const TestWorldPtr &_world, bool _firstTime, if (_firstTime && (i == 0)) { EXPECT_FALSE( - output.Get().entries.empty()); + output.Get().entries.empty()); checkedOutput = true; } }