From d392d0fc3448d54334550fc171c97c7f94806393 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 30 Nov 2021 18:40:19 -0600 Subject: [PATCH] Model Editor: Add Joints to model (#1196) * Model Editor: Add Joints to model Signed-off-by: Michael Carroll * Lint Signed-off-by: Michael Carroll * Style and documentation Signed-off-by: Nate Koenig * Suppress physics warnings on newly-created joints Signed-off-by: Michael Carroll * Added a header Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- include/ignition/gazebo/SdfEntityCreator.hh | 7 + include/ignition/gazebo/gui/GuiEvents.hh | 14 +- src/SdfEntityCreator.cc | 63 +++++-- .../component_inspector/ComponentInspector.cc | 58 +++++- .../component_inspector/ComponentInspector.hh | 27 +++ .../ComponentInspector.qml | 165 +++++++++++++++++- .../component_inspector/ModelEditor.cc | 125 ++++++++++--- src/systems/physics/Physics.cc | 19 ++ 8 files changed, 419 insertions(+), 59 deletions(-) diff --git a/include/ignition/gazebo/SdfEntityCreator.hh b/include/ignition/gazebo/SdfEntityCreator.hh index e5c2442279..c54f8effe3 100644 --- a/include/ignition/gazebo/SdfEntityCreator.hh +++ b/include/ignition/gazebo/SdfEntityCreator.hh @@ -122,6 +122,13 @@ namespace ignition /// \return Joint entity. public: Entity CreateEntities(const sdf::Joint *_joint); + /// \brief Create all entities that exist in the sdf::Joint object and + /// load their plugins. + /// \param[in] _joint SDF joint object. + /// \param[in] _resolved True if all frames are already resolved + /// \return Joint entity. + public: Entity CreateEntities(const sdf::Joint *_joint, bool _resolved); + /// \brief Create all entities that exist in the sdf::Visual object and /// load their plugins. /// \param[in] _visual SDF visual object. diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index ee0e49e2f2..353a85e805 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -18,6 +18,7 @@ #define IGNITION_GAZEBO_GUI_GUIEVENTS_HH_ #include +#include #include #include @@ -191,8 +192,8 @@ namespace events /// \brief Constructor /// \param[in] _tranformModeActive is the transform control mode active public: explicit ModelEditorAddEntity(QString _entity, QString _type, - ignition::gazebo::Entity _parent, QString _uri) : - QEvent(kType), entity(_entity), type(_type), parent(_parent), uri(_uri) + ignition::gazebo::Entity _parent) : + QEvent(kType), entity(_entity), type(_type), parent(_parent) { } @@ -202,12 +203,6 @@ namespace events return this->entity; } - /// \brief Get the URI, if any, associated with the entity to add - public: QString Uri() const - { - return this->uri; - } - /// \brief Get the entity type public: QString EntityType() const { @@ -220,13 +215,12 @@ namespace events return this->parent; } - /// \brief Unique type for this event. static const QEvent::Type kType = QEvent::Type(QEvent::User + 7); + public : QMap data; private: QString entity; private: QString type; private: ignition::gazebo::Entity parent; - private: QString uri; }; } // namespace events diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 11db16cbb5..17c8155765 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -588,6 +588,13 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Link *_link) ////////////////////////////////////////////////// Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint) +{ + return this->CreateEntities(_joint, false); +} + +////////////////////////////////////////////////// +Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint, + bool _resolved) { IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::Joint)"); @@ -645,34 +652,54 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint) this->dataPtr->ecm->CreateComponent(jointEntity , components::ThreadPitch(_joint->ThreadPitch())); + std::string resolvedParentLinkName; - const auto resolveParentErrors = - _joint->ResolveParentLink(resolvedParentLinkName); - if (!resolveParentErrors.empty()) + if (_resolved) { - ignerr << "Failed to resolve parent link for joint '" << _joint->Name() - << "' with parent name '" << _joint->ParentLinkName() << "'" - << std::endl; + resolvedParentLinkName = _joint->ParentLinkName(); + } + else + { + + const auto resolveParentErrors = + _joint->ResolveParentLink(resolvedParentLinkName); + if (!resolveParentErrors.empty()) + { + ignerr << "Failed to resolve parent link for joint '" << _joint->Name() + << "' with parent name '" << _joint->ParentLinkName() << "'" + << std::endl; + for (const auto &error : resolveParentErrors) + { + ignerr << error << std::endl; + } - return kNullEntity; + return kNullEntity; + } } this->dataPtr->ecm->CreateComponent( jointEntity, components::ParentLinkName(resolvedParentLinkName)); std::string resolvedChildLinkName; - const auto resolveChildErrors = - _joint->ResolveChildLink(resolvedChildLinkName); - if (!resolveChildErrors.empty()) - { - ignerr << "Failed to resolve child link for joint '" << _joint->Name() - << "' with child name '" << _joint->ChildLinkName() << "'" - << std::endl; - for (const auto &error : resolveChildErrors) + if (_resolved) + { + resolvedChildLinkName = _joint->ChildLinkName(); + } + else + { + const auto resolveChildErrors = + _joint->ResolveChildLink(resolvedChildLinkName); + if (!resolveChildErrors.empty()) { - ignerr << error << std::endl; - } + ignerr << "Failed to resolve child link for joint '" << _joint->Name() + << "' with child name '" << _joint->ChildLinkName() << "'" + << std::endl; + for (const auto &error : resolveChildErrors) + { + ignerr << error << std::endl; + } - return kNullEntity; + return kNullEntity; + } } this->dataPtr->ecm->CreateComponent( diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index eaf2468e93..ebeab34487 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -103,6 +103,9 @@ namespace ignition::gazebo /// \brief Nested model or not public: bool nestedModel = false; + /// \brief If a model, keep track of available links + public: QStringList modelLinks = {}; + /// \brief Whether currently locked on a given entity public: bool locked{false}; @@ -497,6 +500,25 @@ void ComponentInspector::Update(const UpdateInfo &_info, } this->NestedModelChanged(); + // Get available links for the model. + this->dataPtr->modelLinks.clear(); + this->dataPtr->modelLinks.append("world"); + _ecm.EachNoCache< + components::Name, + components::Link, + components::ParentEntity>([&](const ignition::gazebo::Entity &, + const components::Name *_name, + const components::Link *, + const components::ParentEntity *_parent) -> bool + { + if (_parent->Data() == this->dataPtr->entity) + { + this->dataPtr->modelLinks.push_back( + QString::fromStdString(_name->Data())); + } + return true; + }); + this->ModelLinksChanged(); continue; } @@ -1125,6 +1147,19 @@ bool ComponentInspector::NestedModel() const return this->dataPtr->nestedModel; } +///////////////////////////////////////////////// +void ComponentInspector::SetModelLinks(const QStringList &_modelLinks) +{ + this->dataPtr->modelLinks = _modelLinks; + this->ModelLinksChanged(); +} + +///////////////////////////////////////////////// +QStringList ComponentInspector::ModelLinks() const +{ + return this->dataPtr->modelLinks; +} + ///////////////////////////////////////////////// void ComponentInspector::OnAddEntity(const QString &_entity, const QString &_type) @@ -1132,7 +1167,23 @@ void ComponentInspector::OnAddEntity(const QString &_entity, // currently just assumes parent is the model // todo(anyone) support adding visuals / collisions / sensors to links ignition::gazebo::gui::events::ModelEditorAddEntity addEntityEvent( - _entity, _type, this->dataPtr->entity, QString("")); + _entity, _type, this->dataPtr->entity); + + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &addEntityEvent); +} + +///////////////////////////////////////////////// +void ComponentInspector::OnAddJoint(const QString &_jointType, + const QString &_parentLink, + const QString &_childLink) +{ + ignition::gazebo::gui::events::ModelEditorAddEntity addEntityEvent( + _jointType, "joint", this->dataPtr->entity); + + addEntityEvent.data.insert("parent_link", _parentLink); + addEntityEvent.data.insert("child_link", _childLink); ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), @@ -1157,7 +1208,10 @@ void ComponentInspector::OnLoadMesh(const QString &_entity, } ignition::gazebo::gui::events::ModelEditorAddEntity addEntityEvent( - _entity, _type, this->dataPtr->entity, QString(meshStr.c_str())); + _entity, _type, this->dataPtr->entity); + + addEntityEvent.data.insert("uri", QString(meshStr.c_str())); + ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), &addEntityEvent); diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index 7f756de233..25e38aead9 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -185,6 +185,14 @@ namespace gazebo NOTIFY TypeChanged ) + /// \brief Type + Q_PROPERTY( + QStringList modelLinks + READ ModelLinks + WRITE SetModelLinks + NOTIFY ModelLinksChanged + ) + /// \brief Locked Q_PROPERTY( bool locked @@ -355,6 +363,25 @@ namespace gazebo public: Q_INVOKABLE void OnAddEntity(const QString &_entity, const QString &_type); + /// \brief Callback in Qt thread when a joint is to be added + /// \param[in] _jointType Type of joint to add (revolute, fixed, etc) + /// \param[in] _parentLink Name of the link to be the parent link + /// \param[in] _childLink Name of the link to be the child link + public: Q_INVOKABLE void OnAddJoint(const QString &_jointType, + const QString &_parentLink, + const QString &_childLink); + + /// \brief Return the list of availabe links if a model is selected. + /// \return List of available links. + public: Q_INVOKABLE QStringList ModelLinks() const; + + /// \brief Set the list of availabe links when a model is selected. + /// \param[in] _modelLinks List of available links. + public: Q_INVOKABLE void SetModelLinks(const QStringList &_modelLinks); + + /// \brief Notify that locked has changed. + signals: void ModelLinksChanged(); + /// \brief Callback to insert a new entity /// \param[in] _entity Entity to add, e.g. box, sphere, cylinder, etc /// \param[in] _type Entity type, e.g. link, visual, collision, etc diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 4d6d8b41b4..ea95e6e838 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -157,13 +157,64 @@ Rectangle { height: childrenRect.height Text { - text: sectionText + text: sectionText font.pointSize: 10 padding: 5 } } } + Dialog { + id: jointDialog + modal: true + focus: true + header: ColumnLayout { + id: jointAddHeader + Text { + text:"Add joint" + font.pointSize: 14 + padding: 20 + } + + Text { + text:"Select the parent and child links" + font.pointSize: 12 + leftPadding: 20 + rightPadding: 20 + bottomPadding: 20 + } + } + + standardButtons: Dialog.Ok | Dialog.Cancel + + property string jointType: "empty" + + contentItem: ColumnLayout { + Text { + id: parentBoxText + text: "Parent Link" + } + ComboBox { + id: parentBox + model: ComponentInspector.modelLinks + currentIndex: 0 + } + Text { + id: childBoxText + text: "Child Link" + } + ComboBox { + id: childBox + model: ComponentInspector.modelLinks + currentIndex: 1 + } + } + + onAccepted: { + ComponentInspector.OnAddJoint(jointType, parentBox.currentText, childBox.currentText) + } + } + Rectangle { id: header height: lockButton.height @@ -283,6 +334,7 @@ Rectangle { } } + Menu { id: addLinkMenu @@ -400,7 +452,116 @@ Rectangle { } } - // \todo(anyone) Add joints + MenuSeparator { + padding: 0 + topPadding: 12 + bottomPadding: 12 + contentItem: Rectangle { + implicitWidth: 200 + implicitHeight: 1 + color: "#1E000000" + } + } + + Item { + Layout.fillWidth: true + height: childrenRect.height + Loader { + property string sectionText: "Joint" + sourceComponent: menuSectionHeading + } + } + + MenuItem { + id: revoluteJoint + text: "Revolute" + onClicked: { + jointDialog.jointType = "revolute" + jointDialog.open() + addLinkMenu.close() + } + } + + MenuItem { + id: ballJoint + text: "Ball" + onClicked: { + jointDialog.jointType = "ball" + jointDialog.open() + addLinkMenu.close() + } + } + + MenuItem { + id: continuousJoint + text: "Continuous" + onClicked: { + jointDialog.jointType = "continuous" + jointDialog.open() + addLinkMenu.close() + } + } + + MenuItem { + id: fixedJoint + text: "Fixed" + onClicked: { + jointDialog.jointType = "fixed" + jointDialog.open() + addLinkMenu.close() + } + } + + MenuItem { + id: gearboxJoint + text: "Gearbox" + onClicked: { + jointDialog.jointType = "gearbox" + jointDialog.open() + addLinkMenu.close() + } + } + + MenuItem { + id: prismaticJoint + text: "Prismatic" + onClicked: { + jointDialog.jointType = "prismatic" + jointDialog.open() + addLinkMenu.close() + } + } + + MenuItem { + id: revolute2Joint + text: "Revolute2" + onClicked: { + jointDialog.jointType = "revolute2" + jointDialog.open() + addLinkMenu.close() + } + } + + MenuItem { + id: screwJoint + text: "Screw" + onClicked: { + jointDialog.jointType = "screw" + jointDialog.open() + addLinkMenu.close() + } + } + + MenuItem { + id: universalJoint + text: "Universal" + onClicked: { + jointDialog.jointType = "universal" + jointDialog.open() + addLinkMenu.close() + } + } + } } diff --git a/src/gui/plugins/component_inspector/ModelEditor.cc b/src/gui/plugins/component_inspector/ModelEditor.cc index a40e8117da..2fec07dbc5 100644 --- a/src/gui/plugins/component_inspector/ModelEditor.cc +++ b/src/gui/plugins/component_inspector/ModelEditor.cc @@ -19,7 +19,10 @@ #include #include #include +#include +#include #include + #include #include #include @@ -53,8 +56,8 @@ namespace ignition::gazebo /// \brief Parent entity to add the entity to public: Entity parentEntity; - /// \brief Entity URI, such as a URI for a mesh. - public: std::string uri; + /// \brief Additional entity-specific data needed + public: std::unordered_map data; }; class ModelEditorPrivate @@ -64,11 +67,10 @@ namespace ignition::gazebo /// directional, etc /// \param[in] _entityType Type of entity: link, visual, collision, etc /// \param[in] _parentEntity Name of parent entity - /// \param[in] _uri URI associated with the entity, needed for mesh - /// types. + /// \param[in] _data Additional variable data for specific instances public: void HandleAddEntity(const std::string &_geomOrLightType, const std::string &_entityType, Entity _parentEntity, - const std::string &_uri); + const std::unordered_map &_data); /// \brief Get a SDF string of a geometry /// \param[in] _eta Entity to add. @@ -82,6 +84,10 @@ namespace ignition::gazebo /// \param[in] _eta Entity to add. public: std::string LinkSDFString(const EntityToAdd &_eta) const; + /// \brief Get a SDF string of a link + /// \param[in] _eta Entity to add. + public: sdf::ElementPtr JointSDF(const EntityToAdd &_eta) const; + /// \brief Entity Creator API. public: std::unique_ptr entityCreator{nullptr}; @@ -134,12 +140,20 @@ void ModelEditor::Update(const UpdateInfo &, std::lock_guard lock(this->dataPtr->mutex); // add link entities to the ECM + std::list entities; std::set newEntities; + for (const auto &eta : this->dataPtr->entitiesToAdd) { - sdf::Link linkSdf; + if (eta.parentEntity == kNullEntity) + { + ignerr << "Parent entity not defined." << std::endl; + continue; + } + if (eta.entityType == "link") { + sdf::Link linkSdf; // create an sdf::Link to it can be added to the ECM throught the // CreateEntities call std::string linkSDFStr = this->dataPtr->LinkSDFString(eta); @@ -157,11 +171,6 @@ void ModelEditor::Update(const UpdateInfo &, { continue; } - if (eta.parentEntity == kNullEntity) - { - ignerr << "Parent entity not defined." << std::endl; - continue; - } // generate unique link name // note passing components::Link() as arg to EntityByComponents causes @@ -188,24 +197,54 @@ void ModelEditor::Update(const UpdateInfo &, // traverse the tree and add all new entities created by the entity // creator to the set - std::list entities; entities.push_back(entity); - while (!entities.empty()) + } + else if (eta.entityType == "joint") + { + sdf::ElementPtr jointElem = this->dataPtr->JointSDF(eta); + if (nullptr == jointElem) + continue; + + std::string jointName = "joint"; + Entity jointEnt = _ecm.EntityByComponents( + components::ParentEntity(eta.parentEntity), + components::Name(jointName)); + int64_t counter = 0; + while (jointEnt) { - Entity ent = entities.front(); - entities.pop_front(); + jointName = std::string("joint") + "_" + std::to_string(++counter); + jointEnt = _ecm.EntityByComponents( + components::ParentEntity(eta.parentEntity), + components::Name(jointName)); + } - // add new entity created - newEntities.insert(ent); + jointElem->SetName(jointName); - auto childEntities = _ecm.EntitiesByComponents( - components::ParentEntity(ent)); - for (const auto &child : childEntities) - entities.push_back(child); - } + sdf::Joint jointSdf; + jointSdf.Load(jointElem); + auto entity = + this->dataPtr->entityCreator->CreateEntities(&jointSdf, true); + this->dataPtr->entityCreator->SetParent(entity, eta.parentEntity); + entities.push_back(entity); } } + // traverse the tree and add all new entities created by the entity + // creator to the set + while (!entities.empty()) + { + Entity ent = entities.front(); + entities.pop_front(); + + // add new entity created + newEntities.insert(ent); + + auto childEntities = _ecm.EntitiesByComponents( + components::ParentEntity(ent)); + for (const auto &child : childEntities) + entities.push_back(child); + } + // use tmp AddedRemovedEntities event to update other gui plugins // note this event will be removed in Ignition Garden std::set removedEntities; @@ -226,10 +265,16 @@ bool ModelEditor::eventFilter(QObject *_obj, QEvent *_event) auto event = reinterpret_cast(_event); if (event) { + // Convert to an unordered map of STL strings for convenience + std::unordered_map data; + for (auto key : event->data.toStdMap()) + { + data[key.first.toStdString()] = key.second.toStdString(); + } + this->dataPtr->HandleAddEntity(event->Entity().toStdString(), event->EntityType().toStdString(), - event->ParentEntity(), - event->Uri().toStdString()); + event->ParentEntity(), data); } } @@ -340,7 +385,7 @@ std::string ModelEditorPrivate::GeomSDFString(const EntityToAdd &_eta) const { geomStr << "" - << " " << _eta.uri << "" + << " " << _eta.data.at("uri") << "" << ""; } else @@ -355,6 +400,31 @@ std::string ModelEditorPrivate::GeomSDFString(const EntityToAdd &_eta) const return geomStr.str(); } +///////////////////////////////////////////////// +sdf::ElementPtr ModelEditorPrivate::JointSDF(const EntityToAdd &_eta) const +{ + std::unordered_set validJointTypes = { + "revolute", "ball", "continuous", "fixed", "gearbox", "prismatic", + "revolute2", "screw", "universal"}; + + if (validJointTypes.count(_eta.geomOrLightType) == 0) + { + ignwarn << "Joint type not supported: " + << _eta.geomOrLightType << std::endl; + return nullptr; + } + + auto joint = std::make_shared(); + sdf::initFile("joint.sdf", joint); + + joint->GetAttribute("name")->Set("joint"); + joint->GetAttribute("type")->Set(_eta.geomOrLightType); + joint->GetElement("parent")->Set(_eta.data.at("parent_link")); + joint->GetElement("child")->Set(_eta.data.at("child_link")); + + return joint; +} + ///////////////////////////////////////////////// std::string ModelEditorPrivate::LinkSDFString(const EntityToAdd &_eta) const { @@ -398,7 +468,7 @@ std::string ModelEditorPrivate::LinkSDFString(const EntityToAdd &_eta) const ///////////////////////////////////////////////// void ModelEditorPrivate::HandleAddEntity(const std::string &_geomOrLightType, const std::string &_type, Entity _parentEntity, - const std::string &_uri) + const std::unordered_map &_data) { std::lock_guard lock(this->mutex); std::string entType = common::lowercase(_type); @@ -408,6 +478,7 @@ void ModelEditorPrivate::HandleAddEntity(const std::string &_geomOrLightType, eta.entityType = entType; eta.geomOrLightType = geomLightType; eta.parentEntity = _parentEntity; - eta.uri = _uri; + eta.data = _data; + this->entitiesToAdd.push_back(eta); } diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index f2414936ba..dcaeb8ef30 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -621,6 +621,12 @@ class ignition::gazebo::systems::PhysicsPrivate /// of links and collision elements. This also lets us suppress some /// invalid error messages. public: std::set linkAddedToModel; + + /// \brief Set of joints that were added to an existing model. This set + /// is used to track joints that were added to an existing model, such as + /// through the GUI model editor, so that we can avoid premature creation + /// of joints. This also lets us suppress some invalid error messages. + public: std::set jointAddedToModel; }; ////////////////////////////////////////////////// @@ -789,6 +795,7 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) { // Clear the set of links that were added to a model. this->linkAddedToModel.clear(); + this->jointAddedToModel.clear(); this->CreateWorldEntities(_ecm); this->CreateModelEntities(_ecm); @@ -1301,6 +1308,18 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) const components::ParentLinkName *_parentLinkName, const components::ChildLinkName *_childLinkName) -> bool { + // If the parent model is scheduled for recreation, then do not + // try to create a new link. This situation can occur when a link + // is added to a model from the GUI model editor. + if (_ecm.EntityHasComponentType(_parentModel->Data(), + components::Recreate::typeId)) + { + // Add this entity to the set of newly added links to existing + // models. + this->jointAddedToModel.insert(_entity); + return true; + } + // Check if joint already exists if (this->entityJointMap.HasEntity(_entity)) {