Skip to content

Commit

Permalink
Fix Joint Position Controller Behaviour Described in #1997 (#2001)
Browse files Browse the repository at this point in the history 
* Fix Joint Position Controller Behaviour Described in #1997 

There are several issues at play. First the target velcity calculation
was wrong as described in #1997. Secondly, even if we corrected that
there was still incorrect behaviour. This is due to the fact that we use
the PID's CmdMax to determine what the maximum velocity for the joint
is. However, in the event a user does not set a `<cmd_max>` this
defaults to zero and the joint does not move. Finally this PR updates
the tests. Previously our tests were only testing the case where the
position command was well above the position's maximum velocity, hence
it would slide into position. This PR introduces a test where we snap
the position of the joint into place instead.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Address PR feedback with regards to style.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Just one more thing

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* style

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* More minor style fixes

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* More minor style fixes

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Tracks periodic changes in scene broadcaster (#2010)

* Tracks periodic changes in scene broadcaster

This commit proposes a change to the scene broadcaster which enables
tracking of all components with periodic changes. This way if a
component has a periodic change the scene broadcaster does not miss it.

For more info see this discussion #2001 (comment) where @azeey proposes this solution.

This commit is WIP and I still need to handle entity/component removal
and add tests.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Rework changes

* Removes clone made of BaseComponent.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* More reworks of added APIs to ECM.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Fix tests

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Add ECM related tests.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Address spelling issue

Co-authored-by: Michael Carroll <mjcarroll@intrinsic.ai>
Signed-off-by: Arjo Chakravarty <arjo129@gmail.com>

* Get rid of TODO

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

---------

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
Signed-off-by: Arjo Chakravarty <arjo129@gmail.com>
Co-authored-by: Michael Carroll <mjcarroll@intrinsic.ai>

* Migrate to new header.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Update test paths

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Update include/gz/sim/EntityComponentManager.hh

Co-authored-by: Addisu Z. Taddese <addisu@openrobotics.org>
Signed-off-by: Arjo Chakravarty <arjo129@gmail.com>

* Rename methods

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* style

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Flipped data structure around

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Fix GCC warning

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

---------

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
Signed-off-by: Arjo Chakravarty <arjo129@gmail.com>
Co-authored-by: Michael Carroll <mjcarroll@intrinsic.ai>
Co-authored-by: Addisu Z. Taddese <addisu@openrobotics.org>
  • Loading branch information
@arjo129 @mjcarroll @azeey
3 people authored Aug 3, 2023
1 parent 8a4bc30 commit 7713f2b
Show file tree
Hide file tree
Showing 7 changed files with 379 additions and 21 deletions.
24 changes: 24 additions & 0 deletions include/gz/sim/EntityComponentManager.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -601,6 +601,17 @@ namespace ignition
public: std::unordered_set<ComponentTypeId>
ComponentTypesWithPeriodicChanges() const;

/// \brief Get a cache of components with periodic changes.
/// \param[inout] _changes A list of components with the latest periodic
/// changes. If a component has a periodic change, it is added to the
/// hash map. It the component or entity was removed, it is removed from
/// the hashmap. This way the hashmap stores a list of components and
/// entities which have had periodic changes in the past and still
/// exist within the ECM.
/// \sa EntityComponentManager::PeriodicStateFromCache
public: void UpdatePeriodicChangeCache(std::unordered_map<ComponentTypeId,
std::unordered_set<Entity>>&) const;

/// \brief Set the absolute state of the ECM from a serialized message.
/// Entities / components that are in the new state but not in the old
/// one will be created.
Expand Down Expand Up @@ -628,6 +639,19 @@ namespace ignition
const std::unordered_set<ComponentTypeId> &_types = {},
bool _full = false) const;

/// \brief Populate a message with relevant changes to the state given
/// a periodic change cache.
/// \details The header of the message will not be populated, it is the
/// responsibility of the caller to timestamp it before use. Additionally,
/// changes such as addition or removal will not be populated.
/// \param[inout] _state The serialized state message to populate.
/// \param[in] _cache A map of entities and components to serialize.
/// \sa EntityComponenetManager::UpdatePeriodicChangeCache
public: void PeriodicStateFromCache(
msgs::SerializedStateMap &_state,
const std::unordered_map<ComponentTypeId,
std::unordered_set<Entity>> &_cache) const;

/// \brief Get a message with the serialized state of all entities and
/// components that are changing in the current iteration
///
Expand Down
78 changes: 78 additions & 0 deletions src/EntityComponentManager.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -963,6 +963,42 @@ std::unordered_set<ComponentTypeId>
return periodicComponents;
}

/////////////////////////////////////////////////
void EntityComponentManager::UpdatePeriodicChangeCache(
std::unordered_map<ComponentTypeId,
std::unordered_set<Entity>> &_changes) const
{
// Get all changes
for (const auto &[componentType, entities] :
this->dataPtr->periodicChangedComponents)
{
_changes[componentType].insert(
entities.begin(), entities.end());
}

// Get all removed components
for (const auto &[entity, components] :
this->dataPtr->componentsMarkedAsRemoved)
{
for (const auto &comp : components)
{
_changes[comp].erase(entity);
}
}

// Get all removed entities
for (const auto &entity : this->dataPtr->toRemoveEntities) {
for (
auto components = _changes.begin();
components != _changes.end(); components++) {
// Its ok to leave component entries empty, the serialization
// code will simply ignore it. In any case the number of components
// is limited, so this cache will never grow too large.
components->second.erase(entity);
}
}
}

/////////////////////////////////////////////////
bool EntityComponentManager::HasEntity(const Entity _entity) const
{
Expand Down Expand Up @@ -1678,6 +1714,48 @@ void EntityComponentManager::State(
});
}

//////////////////////////////////////////////////
void EntityComponentManager::PeriodicStateFromCache(
msgs::SerializedStateMap &_state,
const std::unordered_map<ComponentTypeId,
std::unordered_set<Entity>> &_cache) const
{
for (auto &[typeId, entities] : _cache) {
// Serialize components that have changed
for (auto &entity : entities) {
// Add entity to message if it does not exist
auto entIter = _state.mutable_entities()->find(entity);
if (entIter == _state.mutable_entities()->end())
{
msgs::SerializedEntityMap ent;
ent.set_id(entity);
(*_state.mutable_entities())[static_cast<uint64_t>(entity)] = ent;
entIter = _state.mutable_entities()->find(entity);
}

// Find the component in the message
auto compIter = entIter->second.mutable_components()->find(typeId);
if (compIter != entIter->second.mutable_components()->end())
{
// If the component is present we don't need to update it.
continue;
}

auto compIdx = this->dataPtr->componentTypeIndex[entity][typeId];
auto &comp = this->dataPtr->componentStorage[entity][compIdx];

// Add the component to the message
msgs::SerializedComponent cmp;
cmp.set_type(comp->TypeId());
std::ostringstream ostr;
comp->Serialize(ostr);
cmp.set_component(ostr.str());
(*(entIter->second.mutable_components()))[
static_cast<int64_t>(typeId)] = cmp;
}
}
}

//////////////////////////////////////////////////
void EntityComponentManager::SetState(
const msgs::SerializedState &_stateMsg)
Expand Down
66 changes: 66 additions & 0 deletions src/EntityComponentManager_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2204,6 +2204,72 @@ TEST_P(EntityComponentManagerFixture, Descendants)
}
}

//////////////////////////////////////////////////
TEST_P(EntityComponentManagerFixture,
IGN_UTILS_TEST_DISABLED_ON_WIN32(UpdatePeriodicChangeCache))
{
Entity e1 = manager.CreateEntity();
auto c1 = manager.CreateComponent<IntComponent>(e1, IntComponent(123));

std::unordered_map<ComponentTypeId,
std::unordered_set<Entity>> changeTracker;

// No periodic changes keep cache empty.
manager.UpdatePeriodicChangeCache(changeTracker);
EXPECT_EQ(changeTracker.size(), 0u);

// Create a periodic change.
manager.SetChanged(e1, c1->TypeId(), ComponentState::PeriodicChange);

// 1 periodic change, should be reflected in cache.
manager.UpdatePeriodicChangeCache(changeTracker);
EXPECT_EQ(changeTracker.size(), 1u);
EXPECT_EQ(changeTracker[c1->TypeId()].count(e1), 1u);

manager.RunSetAllComponentsUnchanged();

// Has periodic change. Cache should be full.
manager.UpdatePeriodicChangeCache(changeTracker);
EXPECT_EQ(changeTracker.size(), 1u);
EXPECT_EQ(changeTracker[c1->TypeId()].count(e1), 1u);
EXPECT_EQ(changeTracker[c1->TypeId()].size(), 1u);

// Serialize state
msgs::SerializedStateMap state;
manager.PeriodicStateFromCache(state, changeTracker);
EXPECT_EQ(state.entities().size(), 1u);
EXPECT_EQ(
state.entities().find(e1)->second.components().size(), 1u);
EXPECT_NE(state.entities().find(e1)->second
.components().find(c1->TypeId()),
state.entities().find(e1)->second.components().end());

// Component removed cache should be updated.
manager.RemoveComponent<IntComponent>(e1);
manager.UpdatePeriodicChangeCache(changeTracker);
EXPECT_EQ(changeTracker.size(), 1u);
EXPECT_EQ(changeTracker[c1->TypeId()].size(), 0u);

manager.RunSetAllComponentsUnchanged();

// Add another component to the entity
auto c2 = manager.CreateComponent<IntComponent>(e1, IntComponent(123));
manager.UpdatePeriodicChangeCache(changeTracker);

// Cache does not track additions, only PeriodicChanges
EXPECT_EQ(changeTracker[c2->TypeId()].size(), 0u);

// Track change
manager.SetChanged(e1, c1->TypeId(), ComponentState::PeriodicChange);
manager.UpdatePeriodicChangeCache(changeTracker);
EXPECT_EQ(changeTracker[c2->TypeId()].size(), 1u);

// Entity removed cache should be updated.
manager.RequestRemoveEntity(e1);
manager.UpdatePeriodicChangeCache(changeTracker);
EXPECT_EQ(changeTracker[c2->TypeId()].size(), 0u);
}

//////////////////////////////////////////////////
TEST_P(EntityComponentManagerFixture,
IGN_UTILS_TEST_DISABLED_ON_WIN32(SetChanged))
Expand Down
8 changes: 6 additions & 2 deletions src/systems/joint_position_controller/JointPositionController.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,9 @@ class ignition::gazebo::systems::JointPositionControllerPrivate
/// \brief mutex to protect joint commands
public: std::mutex jointCmdMutex;

/// \brief Is the maximum PID gain set.
public: bool isMaxSet {false};

/// \brief Model interface
public: Model model{kNullEntity};

Expand Down Expand Up @@ -149,6 +152,7 @@ void JointPositionController::Configure(const Entity &_entity,
if (_sdf->HasElement("cmd_max"))
{
cmdMax = _sdf->Get<double>("cmd_max");
this->dataPtr->isMaxSet = true;
}
if (_sdf->HasElement("cmd_min"))
{
Expand Down Expand Up @@ -306,14 +310,14 @@ void JointPositionController::PreUpdate(
auto maxMovement = this->dataPtr->posPid.CmdMax() * dt;

// Limit the maximum change to maxMovement
if (abs(error) > maxMovement)
if (abs(error) > maxMovement && this->dataPtr->isMaxSet)
{
targetVel = (error < 0) ? this->dataPtr->posPid.CmdMax() :
-this->dataPtr->posPid.CmdMax();
}
else
{
targetVel = -error;
targetVel = - error / dt;
}

// Set velocity and return
Expand Down
27 changes: 18 additions & 9 deletions src/systems/scene_broadcaster/SceneBroadcaster.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include <condition_variable>
#include <map>
#include <string>
#include <unordered_map>
#include <unordered_set>

#include <gz/common/Profiler.hh>
Expand Down Expand Up @@ -257,8 +258,15 @@ class ignition::gazebo::systems::SceneBroadcasterPrivate
/// \brief Flag used to indicate if periodic changes need to be published
/// This is currently only used in playback mode.
public: bool pubPeriodicChanges{false};

/// \brief Stores a cache of components that are changed. (This prevents
/// dropping of periodic change components which may not be updated
/// frequently enough)
public: std::unordered_map<ComponentTypeId,
std::unordered_set<Entity>> changedComponents;
};


//////////////////////////////////////////////////
SceneBroadcaster::SceneBroadcaster()
: System(), dataPtr(std::make_unique<SceneBroadcasterPrivate>())
Expand Down Expand Up @@ -341,6 +349,9 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info,
// removed entities are removed from the scene graph for the next update cycle
this->dataPtr->SceneGraphRemoveEntities(_manager);

// Iterate through entities and their changes to cache them.
_manager.UpdatePeriodicChangeCache(this->dataPtr->changedComponents);

// Publish state only if there are subscribers and
// * throttle rate to 60 Hz
// * also publish off-rate if there are change events:
Expand Down Expand Up @@ -383,15 +394,7 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info,
else if (!_info.paused)
{
IGN_PROFILE("SceneBroadcast::PostUpdate UpdateState");

if (_manager.HasPeriodicComponentChanges())
{
auto periodicComponents = _manager.ComponentTypesWithPeriodicChanges();
_manager.State(*this->dataPtr->stepMsg.mutable_state(),
{}, periodicComponents);
this->dataPtr->pubPeriodicChanges = false;
}
else
if (!_manager.HasPeriodicComponentChanges())
{
// log files may be recorded at lower rate than sim time step. So in
// playback mode, the scene broadcaster may not see any periodic
Expand All @@ -416,6 +419,12 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info,
// we may be able to remove this in the future and update tests
this->dataPtr->stepMsg.mutable_state();
}

// Apply changes that were caught by the periodic state tracker and then
// clear the change tracker.
_manager.PeriodicStateFromCache(*this->dataPtr->stepMsg.mutable_state(),
this->dataPtr->changedComponents);
this->dataPtr->changedComponents.clear();
}

// Full state on demand
Expand Down
Loading

0 comments on commit 7713f2b

Please sign in to comment.