SceneEditor.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotAPI::ArmarXObjects::SceneEditor
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "SceneEditor.h"
 
#include <algorithm>
#include <cmath>
#include <limits>
#include <set>
#include <utility>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <SimoxUtility/json.h>
#include <VirtualRobot/BoundingBox.h>
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/Obstacle.h>
 
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/CycleUtil.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/json_conversions.h>
 
namespace armarx
{
    namespace
    {
        const std::vector<std::string> contextMenuEntries = {
            "Flip X (+90°)",
            "Flip Y (+90°)",
            "Flip Z (+90°)",
            "Toggle ground lock",
            "Delete",
        };
 
        // Placeholder entry: ComboBoxes must never have an empty option list,
        // otherwise the RemoteGuiProvider rejects the whole tab.
        const std::string noneOption = "<none>";
 
        constexpr float rad2deg = 180.0f / M_PI;
        constexpr float deg2rad = M_PI / 180.0f;
    } // namespace
 
    SceneEditorPropertyDefinitions::SceneEditorPropertyDefinitions(std::string prefix) :
        ComponentPropertyDefinitions(prefix)
    {
        defineOptionalProperty<std::string>("ObjectsPackage",
                                            ObjectFinder::DefaultObjectsPackageName,
                                            "Package containing the object models.");
        defineOptionalProperty<std::string>(
            "ScenesPackage",
            ObjectFinder::DefaultObjectsPackageName,
            "Package to whose data/<package> directory scenes are saved to and "
            "loaded from (if SceneStorageDirectory is empty and the scene file "
            "is not an absolute path).");
        defineOptionalProperty<std::string>(
            "SceneStorageDirectory",
            "",
            "Directory that directly holds the scene files (new scenes are "
            "created here and loaded from here). Grounding info is written to a "
            "parallel 'groundings' directory (the scene path with its last "
            "'scenes' component replaced by 'groundings'). If empty, the "
            "ScenesPackage's <data>/<package>/scenes directory is used. "
            "Use an absolute path for reliable resolution (a relative path is "
            "resolved against the process working directory, not the package).");
        defineOptionalProperty<std::string>(
            "GroundObject",
            "",
            "Optional class ID (Dataset/ClassName) of an object that is spawned "
            "at the origin and designated as the ground object.");
        defineOptionalProperty<float>(
            "GroundZ", 0.0f, "Initial height (z, in mm) of the ground plane objects are locked to.");
        defineOptionalProperty<std::string>(
            "SceneFile",
            "NewScene",
            "Initial scene file (name in the scenes directory or absolute path).");
    }
 
    std::string
    SceneEditor::getDefaultName() const
    {
        return "SceneEditor";
    }
 
    PropertyDefinitionsPtr
    SceneEditor::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(new SceneEditorPropertyDefinitions(getConfigIdentifier()));
    }
 
    void
    SceneEditor::onInitComponent()
    {
        objectsPackage_ = getProperty<std::string>("ObjectsPackage").getValue();
        scenesPackage_ = getProperty<std::string>("ScenesPackage").getValue();
        sceneStorageDirectory_ = getProperty<std::string>("SceneStorageDirectory").getValue();
        initialGroundClass_ = getProperty<std::string>("GroundObject").getValue();
        groundZ_ = getProperty<float>("GroundZ").getValue();
        initialSceneFile_ = getProperty<std::string>("SceneFile").getValue();
 
        objectFinder_ = ObjectFinder(objectsPackage_);
    }
 
    void
    SceneEditor::onConnectComponent()
    {
        {
            std::scoped_lock lock(mutex_);
 
            try
            {
                for (const auto& [dataset, infos] : objectFinder_.findAllObjectsByDataset(true))
                {
                    std::vector<std::string>& classNames = objectsByDataset_[dataset];
                    for (const ObjectInfo& info : infos)
                    {
                        classNames.push_back(info.id().className());
                    }
                    std::sort(classNames.begin(), classNames.end());
                }
            }
            catch (const std::exception& e)
            {
                ARMARX_WARNING << "Failed to enumerate objects of package '" << objectsPackage_
                               << "': " << e.what();
            }
            if (!objectsByDataset_.empty())
            {
                currentDataset_ = objectsByDataset_.begin()->first;
            }
            ARMARX_INFO << "Found " << objectsByDataset_.size() << " datasets in package '"
                        << objectsPackage_ << "'.";
 
            if (!initialGroundClass_.empty())
            {
                if (objectFinder_.findObject(initialGroundClass_))
                {
                    Entry& ground = addObject(initialGroundClass_);
                    // The ground object itself is not locked to anything.
                    ground.manualPose = true;
                    ground.groundRef.clear();
                }
                else
                {
                    ARMARX_WARNING << "Ground object '" << initialGroundClass_ << "' not found.";
                }
            }
        }
 
        createRemoteGuiTab();
        RemoteGui_startRunningTask();
 
        task_ = new RunningTask<SceneEditor>(this, &SceneEditor::run);
        task_->start();
    }
 
    void
    SceneEditor::onDisconnectComponent()
    {
        if (task_)
        {
            const bool join = true;
            task_->stop(join);
            task_ = nullptr;
        }
    }
 
    void
    SceneEditor::onExitComponent()
    {
    }
 
    void
    SceneEditor::createRemoteGuiTab()
    {
        using namespace RemoteGui::Client;
 
        std::vector<std::string> datasets;
        for (const auto& datasetEntry : objectsByDataset_)
        {
            datasets.push_back(datasetEntry.first);
        }
        if (datasets.empty())
        {
            datasets.push_back(noneOption);
        }
        tab_.dataset.setOptions(datasets);
        if (!currentDataset_.empty())
        {
            tab_.dataset.setValue(currentDataset_);
        }
 
        std::vector<std::string> classNames;
        if (auto it = objectsByDataset_.find(currentDataset_); it != objectsByDataset_.end())
        {
            classNames = it->second;
        }
        if (classNames.empty())
        {
            classNames.push_back(noneOption);
        }
        tab_.objectClass.setOptions(classNames);
 
        std::vector<std::string> instanceOptions = {noneOption};
        for (const Entry& entry : entries_)
        {
            instanceOptions.push_back(entry.data.instanceName);
        }
        tab_.groundObject.setOptions(instanceOptions);
        {
            const Entry* selected = findEntry(selected_);
            tab_.groundObject.setValue(
                (selected && !selected->groundRef.empty()) ? selected->groundRef : noneOption);
        }
 
        // Reading widget values throws before the tab exists (first build).
        const std::string previousAlignTarget =
            guiInitialized_ ? tab_.alignTarget.getValue() : std::string();
        tab_.alignTarget.setOptions(instanceOptions);
        if (std::find(instanceOptions.begin(), instanceOptions.end(), previousAlignTarget) !=
            instanceOptions.end())
        {
            tab_.alignTarget.setValue(previousAlignTarget);
        }
 
        if (!guiInitialized_)
        {
            guiInitialized_ = true;
 
            tab_.addObject.setLabel("Add object");
 
            tab_.groundZ.setRange(-100000.0f, 100000.0f);
            tab_.groundZ.setDecimals(1);
            tab_.groundZ.setSteps(2000);
            tab_.groundZ.setValue(groundZ_);
 
            tab_.selectedInfo.setText("<none>");
            tab_.instanceName.setValue("");
            tab_.renameObject.setLabel("Rename");
            for (FloatSpinBox* spin : {&tab_.posX, &tab_.posY, &tab_.posZ})
            {
                spin->setRange(-100000.0f, 100000.0f);
                spin->setDecimals(1);
                spin->setSteps(2000);
                spin->setValue(0.0f);
            }
            tab_.yaw.setRange(-180.0f, 180.0f);
            tab_.yaw.setDecimals(1);
            tab_.yaw.setSteps(720);
            tab_.yaw.setValue(0.0f);
 
            tab_.alignOutside.setValue(false);
            tab_.frontXZ.setLabel("Front XZ plane (y min)");
            tab_.backXZ.setLabel("Back XZ plane (y max)");
            tab_.frontYZ.setLabel("Front YZ plane (x min)");
            tab_.backYZ.setLabel("Back YZ plane (x max)");
 
            tab_.isStatic.setValue(true);
            tab_.liveApply.setValue(false);
            tab_.applyPose.setLabel("Apply pose (manual)");
            tab_.lockToGround.setLabel("Lock to ground");
            tab_.flipX.setLabel("Flip X (+90°)");
            tab_.flipY.setLabel("Flip Y (+90°)");
            tab_.flipZ.setLabel("Flip Z (+90°)");
            tab_.deleteObject.setLabel("Delete object");
 
            tab_.sceneFile.setValue(initialSceneFile_);
            tab_.saveScene.setLabel("Save scene");
            tab_.loadScene.setLabel("Load scene");
            tab_.clearScene.setLabel("Clear scene");
            tab_.status.setText("");
        }
 
        GroupBox importGroup;
        importGroup.setLabel("Import object");
        {
            GridLayout grid;
            grid.add(Label("Dataset"), {0, 0}).add(tab_.dataset, {0, 1});
            grid.add(Label("Object"), {1, 0}).add(tab_.objectClass, {1, 1});
            grid.add(tab_.addObject, {2, 0}, {1, 2});
            importGroup.addChild(grid);
        }
 
        GroupBox groundGroup;
        groundGroup.setLabel("Default ground");
        {
            GridLayout grid;
            grid.add(Label("Default ground z (mm)"), {0, 0}).add(tab_.groundZ, {0, 1});
            groundGroup.addChild(grid);
        }
 
        GroupBox selectedGroup;
        selectedGroup.setLabel("Selected object (click an object in ArViz to select)");
        {
            GridLayout grid;
            int row = 0;
            grid.add(tab_.selectedInfo, {row++, 0}, {1, 4});
            grid.add(Label("Instance name"), {row, 0})
                .add(tab_.instanceName, {row, 1}, {1, 2})
                .add(tab_.renameObject, {row, 3});
            ++row;
            grid.add(Label("Grounded to"), {row, 0}).add(tab_.groundObject, {row, 1}, {1, 3});
            ++row;
            grid.add(Label("Static"), {row, 0}).add(tab_.isStatic, {row, 1});
            ++row;
            grid.add(Label("x (mm)"), {row, 0})
                .add(tab_.posX, {row, 1})
                .add(Label("y (mm)"), {row, 2})
                .add(tab_.posY, {row, 3});
            ++row;
            grid.add(Label("z (mm)"), {row, 0})
                .add(tab_.posZ, {row, 1})
                .add(Label("yaw (°)"), {row, 2})
                .add(tab_.yaw, {row, 3});
            ++row;
            grid.add(Label("Apply immediately"), {row, 0}).add(tab_.liveApply, {row, 1});
            ++row;
            grid.add(tab_.applyPose, {row, 0}, {1, 2}).add(tab_.lockToGround, {row, 2}, {1, 2});
            ++row;
            grid.add(tab_.flipX, {row, 0}).add(tab_.flipY, {row, 1}).add(tab_.flipZ, {row, 2});
            grid.add(tab_.deleteObject, {row, 3});
            selectedGroup.addChild(grid);
        }
 
        GroupBox alignGroup;
        alignGroup.setLabel(
            "Align the selected object's bounding box face with a reference object");
        {
            GridLayout grid;
            int row = 0;
            grid.add(Label("Reference object"), {row, 0}).add(tab_.alignTarget, {row, 1});
            ++row;
            grid.add(Label("Align from outside (touching)"), {row, 0})
                .add(tab_.alignOutside, {row, 1});
            ++row;
            grid.add(tab_.frontYZ, {row, 0}).add(tab_.backYZ, {row, 1});
            ++row;
            grid.add(tab_.frontXZ, {row, 0}).add(tab_.backXZ, {row, 1});
            alignGroup.addChild(grid);
        }
 
        GroupBox fileGroup;
        fileGroup.setLabel("Scene file");
        {
            GridLayout grid;
            grid.add(Label("File"), {0, 0}).add(tab_.sceneFile, {0, 1}, {1, 3});
            grid.add(tab_.saveScene, {1, 0})
                .add(tab_.loadScene, {1, 1})
                .add(tab_.clearScene, {1, 2});
            fileGroup.addChild(grid);
        }
 
        VBoxLayout root = {importGroup, groundGroup, selectedGroup, alignGroup,
                           fileGroup,   tab_.status, VSpacer()};
        RemoteGui_createTab(getName(), root, &tab_);
    }
 
    void
    SceneEditor::RemoteGui_update()
    {
        // An uncaught exception would terminate the RemoteGui update task
        // and freeze the whole GUI, so log and continue instead.
        try
        {
            remoteGuiUpdate();
        }
        catch (const std::exception& e)
        {
            ARMARX_WARNING << "RemoteGui update failed: " << e.what();
        }
    }
 
    void
    SceneEditor::remoteGuiUpdate()
    {
        std::scoped_lock lock(mutex_);
 
        if (tab_.dataset.hasValueChanged())
        {
            const std::string dataset = tab_.dataset.getValue();
            if (!dataset.empty() && dataset != currentDataset_)
            {
                currentDataset_ = dataset;
                tabRebuildNeeded_ = true;
            }
        }
 
        // The ground combo box sets the ground of the currently selected object.
        if (tab_.groundObject.hasValueChanged())
        {
            const std::string value = tab_.groundObject.getValue();
            const std::string ref = (value == noneOption) ? "" : value;
            if (Entry* entry = findEntry(selected_); entry && ref != entry->groundRef)
            {
                if (ref == entry->data.instanceName)
                {
                    status_ = "An object cannot be grounded to itself.";
                    syncGui_ = true;
                }
                else
                {
                    entry->groundRef = ref;
                    if (!entry->manualPose)
                    {
                        entry->data.position.z() = groundSnappedZ(*entry, poseOf(entry->data));
                        sceneLayerDirty_ = true;
                    }
                    syncGui_ = true;
                    status_ = "Grounded '" + selected_ + "' to " +
                              (ref.empty() ? std::string("the default ground.")
                                           : "'" + ref + "'.");
                }
            }
        }
 
        if (tab_.addObject.wasClicked())
        {
            const std::string className = tab_.objectClass.getValue();
            if (className.empty() || className == noneOption || currentDataset_.empty() ||
                currentDataset_ == noneOption)
            {
                status_ = "No object class selected.";
            }
            else
            {
                Entry& entry = addObject(currentDataset_ + "/" + className);
                selectEntry(entry.data.instanceName);
                status_ = "Added '" + entry.data.instanceName + "'.";
            }
        }
 
        if (tab_.groundZ.hasValueChanged())
        {
            const float value = tab_.groundZ.getValue();
            if (std::abs(value - groundZ_) > 0.01f)
            {
                setGroundZ(value);
            }
        }
 
        Entry* selected = findEntry(selected_);
 
        if (tab_.isStatic.hasValueChanged())
        {
            const bool isStatic = tab_.isStatic.getValue();
            if (selected)
            {
                selected->data.isStatic = isStatic;
                status_ = "'" + selected_ + "' is now " +
                          (isStatic ? "static." : "dynamic.");
            }
        }
 
        const bool applyClicked = tab_.applyPose.wasClicked();
        bool poseEdited = false;
        // Reading the values consumes the change flags, so read them all.
        poseEdited |= tab_.posX.hasValueChanged();
        poseEdited |= tab_.posY.hasValueChanged();
        poseEdited |= tab_.posZ.hasValueChanged();
        poseEdited |= tab_.yaw.hasValueChanged();
        const bool liveApply = tab_.liveApply.getValue() && poseEdited;
 
        if (applyClicked || liveApply)
        {
            if (selected)
            {
                const Eigen::Vector3f position(
                    tab_.posX.getValue(), tab_.posY.getValue(), tab_.posZ.getValue());
                const float yawDeg = tab_.yaw.getValue();
 
                // Skip if the values merely echo the current pose (e.g. after
                // the GUI was synced to a new selection).
                const float yawDiff = std::remainder(
                    yawDeg * deg2rad - yawOf(selected->data.orientation), 2.0 * M_PI);
                const bool differs = (position - selected->data.position).norm() > 0.01f ||
                                     std::abs(yawDiff) > 1e-3f;
                if (applyClicked || differs)
                {
                    if (applyManualPose(*selected, position, yawDeg))
                    {
                        // Manually set coordinates override the ground locking.
                        status_ = "Applied manual pose to '" + selected_ +
                                  "' (unlocked from ground).";
                    }
                }
            }
            else if (applyClicked)
            {
                status_ = "No object selected.";
            }
        }
 
        if (tab_.renameObject.wasClicked())
        {
            if (selected)
            {
                renameEntry(*selected, tab_.instanceName.getValue());
            }
            else
            {
                status_ = "No object selected.";
            }
        }
 
        if (tab_.lockToGround.wasClicked())
        {
            if (!selected)
            {
                status_ = "No object selected.";
            }
            else
            {
                selected->manualPose = false;
                selected->data.position.z() = groundSnappedZ(*selected, poseOf(selected->data));
                sceneLayerDirty_ = true;
                syncGui_ = true;
                status_ = "Locked '" + selected_ + "' to " +
                          (selected->groundRef.empty() ? std::string("the default ground.")
                                                       : "'" + selected->groundRef + "'.");
            }
        }
 
        const Eigen::Vector3f flipAxes[] = {
            Eigen::Vector3f::UnitX(), Eigen::Vector3f::UnitY(), Eigen::Vector3f::UnitZ()};
        RemoteGui::Client::Button* flipButtons[] = {&tab_.flipX, &tab_.flipY, &tab_.flipZ};
        for (int axis = 0; axis < 3; ++axis)
        {
            if (flipButtons[axis]->wasClicked())
            {
                if (selected)
                {
                    flipEntry(*selected, flipAxes[axis]);
                }
                else
                {
                    status_ = "No object selected.";
                }
            }
        }
 
        if (tab_.deleteObject.wasClicked())
        {
            if (selected)
            {
                status_ = "Deleted '" + selected_ + "'.";
                deleteEntry(selected_);
                selected = nullptr;
            }
            else
            {
                status_ = "No object selected.";
            }
        }
 
        if (tab_.saveScene.wasClicked())
        {
            saveScene(tab_.sceneFile.getValue());
        }
 
        if (tab_.loadScene.wasClicked())
        {
            loadScene(tab_.sceneFile.getValue());
        }
 
        if (tab_.frontYZ.wasClicked())
        {
            alignToPlane(0, true, "front yz-plane");
        }
        if (tab_.backYZ.wasClicked())
        {
            alignToPlane(0, false, "back yz-plane");
        }
        if (tab_.frontXZ.wasClicked())
        {
            alignToPlane(1, true, "front xz-plane");
        }
        if (tab_.backXZ.wasClicked())
        {
            alignToPlane(1, false, "back xz-plane");
        }
 
        if (tab_.clearScene.wasClicked())
        {
            entries_.clear();
            pendingTransforms_.clear();
            collisionModels_.clear();
            selected_.clear();
            sceneLayerDirty_ = true;
            groundLayerDirty_ = true;
            syncGui_ = true;
            tabRebuildNeeded_ = true;
            status_ = "Cleared scene.";
        }
 
        if (syncGui_)
        {
            syncGui_ = false;
            if (Entry* entry = findEntry(selected_))
            {
                const std::string grounding =
                    entry->manualPose
                        ? std::string(", manual pose)")
                        : (entry->groundRef.empty()
                               ? std::string(", locked to default ground)")
                               : ", locked to '" + entry->groundRef + "')");
                tab_.selectedInfo.setText(entry->data.instanceName + "  (" +
                                          entry->data.className + grounding);
                tab_.instanceName.setValue(entry->data.instanceName);
                tab_.isStatic.setValue(entry->data.isStatic.value_or(true));
                tab_.posX.setValue(entry->data.position.x());
                tab_.posY.setValue(entry->data.position.y());
                tab_.posZ.setValue(entry->data.position.z());
                tab_.yaw.setValue(yawOf(entry->data.orientation) * rad2deg);
                tab_.groundObject.setValue(entry->groundRef.empty() ? noneOption
                                                                    : entry->groundRef);
            }
            else
            {
                tab_.selectedInfo.setText("<none>");
                tab_.instanceName.setValue("");
                tab_.groundObject.setValue(noneOption);
            }
            tab_.groundZ.setValue(groundZ_);
        }
 
        tab_.status.setText(status_);
 
        if (tabRebuildNeeded_)
        {
            tabRebuildNeeded_ = false;
            createRemoteGuiTab();
        }
    }
 
    void
    SceneEditor::run()
    {
        viz::StagedCommit stage;
 
        // Immediately clear any layers left over from a previous run by
        // committing them empty.
        {
            std::scoped_lock lock(mutex_);
            sceneLayer_ = arviz.layer("Scene");
            groundLayer_ = arviz.layer("Ground");
            stage.add(sceneLayer_);
            stage.add(groundLayer_);
        }
        viz::CommitResult result = arviz.commit(stage);
        stage.reset();
 
        // Stage the initial scene content and the first interaction request.
        {
            std::scoped_lock lock(mutex_);
            rebuildSceneLayer();
            rebuildGroundLayer();
            stage.add(sceneLayer_);
            stage.add(groundLayer_);
            sceneLayerDirty_ = false;
            groundLayerDirty_ = false;
            stage.requestInteraction(sceneLayer_);
        }
 
        // This loop is structured like in ArVizInteractExample: commit the
        // stage, then reset and rebuild it (interaction request plus all
        // layers changed by interactions or by the RemoteGui thread) for the
        // commit in the next cycle.
        CycleUtil cycle(10.0f);
        while (!task_->isStopped())
        {
            result = arviz.commit(stage);
 
            stage.reset();
 
            {
                std::scoped_lock lock(mutex_);
 
                stage.requestInteraction(sceneLayer_);
 
                for (const viz::InteractionFeedback& interaction : result.interactions())
                {
                    handleInteraction(interaction);
                }
 
                if (sceneLayerDirty_)
                {
                    rebuildSceneLayer();
                    stage.add(sceneLayer_);
                    sceneLayerDirty_ = false;
                }
                if (groundLayerDirty_)
                {
                    rebuildGroundLayer();
                    stage.add(groundLayer_);
                    groundLayerDirty_ = false;
                }
            }
 
            cycle.waitForCycleDuration();
        }
    }
 
    void
    SceneEditor::handleInteraction(const viz::InteractionFeedback& interaction)
    {
        switch (interaction.type())
        {
            case viz::InteractionFeedbackType::Select:
            {
                selectEntry(interaction.element());
            }
            break;
 
            case viz::InteractionFeedbackType::Transform:
            {
                // The transformation is cumulative over the whole selection
                // (the manipulator stays active between drags), so only track
                // it here and apply it once, on deselection — applying it
                // earlier would double it up.
                pendingTransforms_[interaction.element()] = interaction.transformation();
            }
            break;
 
            case viz::InteractionFeedbackType::Deselect:
            {
                applyPendingTransform(interaction.element());
            }
            break;
 
            case viz::InteractionFeedbackType::ContextMenuChosen:
            {
                Entry* entry = findEntry(interaction.element());
                if (!entry)
                {
                    break;
                }
                switch (interaction.chosenContextMenuEntry())
                {
                    case 0:
                        flipEntry(*entry, Eigen::Vector3f::UnitX());
                        break;
                    case 1:
                        flipEntry(*entry, Eigen::Vector3f::UnitY());
                        break;
                    case 2:
                        flipEntry(*entry, Eigen::Vector3f::UnitZ());
                        break;
                    case 3:
                        entry->manualPose = !entry->manualPose;
                        if (!entry->manualPose)
                        {
                            entry->data.position.z() =
                                groundSnappedZ(*entry, poseOf(entry->data));
                        }
                        sceneLayerDirty_ = true;
                        syncGui_ = true;
                        break;
                    case 4:
                        deleteEntry(interaction.element());
                        break;
                    default:
                        break;
                }
            }
            break;
 
            default:
                break;
        }
    }
 
    void
    SceneEditor::applyPendingTransform(const std::string& instanceName)
    {
        auto it = pendingTransforms_.find(instanceName);
        if (it == pendingTransforms_.end())
        {
            return;
        }
        const Eigen::Matrix4f transform = it->second;
        pendingTransforms_.erase(it);
 
        Entry* entry = findEntry(instanceName);
        if (!entry)
        {
            return;
        }
 
        // The interaction in ArViz allows a full 6-DOF transform (restricting
        // the axes viewer-side is unreliable). Constrain it here instead.
        const Eigen::Matrix4f candidate = transform * poseOf(entry->data);
 
        Eigen::Vector3f newPosition = candidate.block<3, 1>(0, 3);
        Eigen::Quaternionf newOrientation;
        if (entry->manualPose)
        {
            newOrientation = Eigen::Quaternionf(candidate.block<3, 3>(0, 0)).normalized();
        }
        else
        {
            // Ground lock: keep only the yaw component of the applied
            // rotation and stay on the ground plane.
            const float deltaYaw = yawOf(Eigen::Quaternionf(transform.block<3, 3>(0, 0)));
            newOrientation = (Eigen::AngleAxisf(deltaYaw, Eigen::Vector3f::UnitZ()) *
                              entry->data.orientation)
                                 .normalized();
        }
 
        Eigen::Matrix4f newPose = Eigen::Matrix4f::Identity();
        newPose.block<3, 3>(0, 0) = newOrientation.toRotationMatrix();
        newPose.block<3, 1>(0, 3) = newPosition;
 
        if (!entry->manualPose)
        {
            // Rest the lowest point of the bounding box on the ground plane.
            newPosition.z() = groundSnappedZ(*entry, newPose);
            newPose(2, 3) = newPosition.z();
        }
 
        // Even when the move is rejected, the layer must be re-committed to
        // snap the visualization back to the entry's pose.
        sceneLayerDirty_ = true;
        syncGui_ = true;
 
        if (!isMoveAllowed(*entry, newPose))
        {
            return;
        }
 
        entry->data.position = newPosition;
        entry->data.orientation = newOrientation;
        afterEntryPoseChanged(*entry);
    }
 
    bool
    SceneEditor::applyManualPose(Entry& entry, const Eigen::Vector3f& position, float yawDeg)
    {
        const float deltaYaw =
            std::remainder(yawDeg * deg2rad - yawOf(entry.data.orientation), 2.0 * M_PI);
        const Eigen::Quaternionf newOrientation =
            (Eigen::AngleAxisf(deltaYaw, Eigen::Vector3f::UnitZ()) * entry.data.orientation)
                .normalized();
 
        Eigen::Matrix4f newPose = Eigen::Matrix4f::Identity();
        newPose.block<3, 3>(0, 0) = newOrientation.toRotationMatrix();
        newPose.block<3, 1>(0, 3) = position;
 
        if (!isMoveAllowed(entry, newPose))
        {
            // Snap the GUI back to the entry's actual pose.
            syncGui_ = true;
            return false;
        }
 
        entry.manualPose = true;
        entry.data.position = position;
        entry.data.orientation = newOrientation;
        sceneLayerDirty_ = true;
        syncGui_ = true;
        afterEntryPoseChanged(entry);
        return true;
    }
 
    std::string
    SceneEditor::collidesWith(const Entry& entry, const Eigen::Matrix4f& candidatePose)
    {
        VirtualRobot::ObstaclePtr model = collisionModelOf(entry);
        if (!model || !model->getCollisionModel())
        {
            return "";
        }
        model->setGlobalPose(candidatePose);
 
        auto checker = VirtualRobot::CollisionChecker::getGlobalCollisionChecker();
        for (const Entry& other : entries_)
        {
            if (&other == &entry)
            {
                continue;
            }
            // Objects rest on their ground, so do not count that contact.
            if (other.data.instanceName == entry.groundRef ||
                other.groundRef == entry.data.instanceName)
            {
                continue;
            }
            VirtualRobot::ObstaclePtr otherModel = collisionModelOf(other);
            if (!otherModel || !otherModel->getCollisionModel())
            {
                continue;
            }
            otherModel->setGlobalPose(poseOf(other.data));
            if (checker->checkCollision(model->getCollisionModel(),
                                        otherModel->getCollisionModel()))
            {
                return other.data.instanceName;
            }
        }
        return "";
    }
 
    bool
    SceneEditor::isMoveAllowed(Entry& entry, const Eigen::Matrix4f& candidatePose)
    {
        // Entries that already collide may always be moved, so that existing
        // collisions can be resolved.
        const bool wasColliding = !collidesWith(entry, poseOf(entry.data)).empty();
        if (wasColliding)
        {
            return true;
        }
        const std::string hit = collidesWith(entry, candidatePose);
        if (!hit.empty())
        {
            status_ = "Move of '" + entry.data.instanceName + "' rejected: collides with '" +
                      hit + "'.";
            return false;
        }
        return true;
    }
 
    const std::optional<simox::AxisAlignedBoundingBox>&
    SceneEditor::localAabbOf(const Entry& entry)
    {
        auto it = localAabbs_.find(entry.data.className);
        if (it == localAabbs_.end())
        {
            std::optional<simox::AxisAlignedBoundingBox> aabb;
            try
            {
                if (std::optional<ObjectInfo> info =
                        objectFinder_.findObject(entry.data.className))
                {
                    info->setLogError(false);
                    aabb = info->loadAABB();
                }
            }
            catch (const std::exception& e)
            {
                ARMARX_WARNING << "Failed to load AABB of '" << entry.data.className
                               << "': " << e.what();
            }
            if (!aabb)
            {
                ARMARX_INFO << "No AABB (aabb.json) for '" << entry.data.className
                            << "', falling back to the collision model's bounding box.";
            }
            it = localAabbs_.emplace(entry.data.className, aabb).first;
        }
        return it->second;
    }
 
    std::optional<simox::AxisAlignedBoundingBox>
    SceneEditor::globalAabb(const Entry& entry, const Eigen::Matrix4f& pose)
    {
        // Prefer the precomputed local AABB shipped with the object data;
        // fall back to the collision model's local bounding box.
        Eigen::Vector3f min;
        Eigen::Vector3f max;
        if (const std::optional<simox::AxisAlignedBoundingBox>& aabb = localAabbOf(entry))
        {
            min = aabb->min();
            max = aabb->max();
        }
        else if (VirtualRobot::ObstaclePtr model = collisionModelOf(entry);
                 model && model->getCollisionModel())
        {
            const VirtualRobot::BoundingBox bbox =
                model->getCollisionModel()->getBoundingBox(false);
            min = bbox.getMin();
            max = bbox.getMax();
        }
        else
        {
            return std::nullopt;
        }
 
        // Transform all 8 corners of the local box and take the extrema.
        const Eigen::Matrix3f rotation = pose.block<3, 3>(0, 0);
        const Eigen::Vector3f translation = pose.block<3, 1>(0, 3);
        Eigen::Vector3f globalMin =
            Eigen::Vector3f::Constant(std::numeric_limits<float>::max());
        Eigen::Vector3f globalMax =
            Eigen::Vector3f::Constant(std::numeric_limits<float>::lowest());
        for (int i = 0; i < 8; ++i)
        {
            const Eigen::Vector3f corner((i & 1) ? max.x() : min.x(),
                                         (i & 2) ? max.y() : min.y(),
                                         (i & 4) ? max.z() : min.z());
            const Eigen::Vector3f global = rotation * corner + translation;
            globalMin = globalMin.cwiseMin(global);
            globalMax = globalMax.cwiseMax(global);
        }
        return simox::AxisAlignedBoundingBox(globalMin, globalMax);
    }
 
    float
    SceneEditor::groundHeightOf(const std::string& ref)
    {
        if (ref.empty())
        {
            return groundZ_;
        }
        Entry* ground = findEntry(ref);
        if (!ground)
        {
            return groundZ_;
        }
        if (const auto aabb = globalAabb(*ground, poseOf(ground->data)))
        {
            return aabb->max().z();
        }
        return groundZ_;
    }
 
    float
    SceneEditor::groundHeightFor(const Entry& entry)
    {
        return groundHeightOf(entry.groundRef);
    }
 
    void
    SceneEditor::reseatEntriesGroundedTo(const std::string& ref)
    {
        for (Entry& entry : entries_)
        {
            if (!entry.manualPose && entry.groundRef == ref &&
                entry.data.instanceName != ref)
            {
                entry.data.position.z() = groundSnappedZ(entry, poseOf(entry.data));
                sceneLayerDirty_ = true;
            }
        }
    }
 
    void
    SceneEditor::afterEntryPoseChanged(const Entry& entry)
    {
        reseatEntriesGroundedTo(entry.data.instanceName);
    }
 
    float
    SceneEditor::groundSnappedZ(const Entry& entry, const Eigen::Matrix4f& candidatePose)
    {
        const float ground = groundHeightFor(entry);
        const auto aabb = globalAabb(entry, candidatePose);
        if (!aabb)
        {
            return ground;
        }
        return candidatePose(2, 3) + (ground - aabb->min().z());
    }
 
    VirtualRobot::ObstaclePtr
    SceneEditor::collisionModelOf(const Entry& entry)
    {
        auto it = collisionModels_.find(entry.data.instanceName);
        if (it != collisionModels_.end())
        {
            return it->second;
        }
 
        VirtualRobot::ObstaclePtr model;
        try
        {
            model = ObjectFinder::loadObstacle(objectFinder_.findObject(entry.data.className));
        }
        catch (const std::exception& e)
        {
            ARMARX_WARNING << "Failed to load collision model of '" << entry.data.className
                           << "': " << e.what();
        }
        if (!model)
        {
            ARMARX_WARNING << "No collision model for '" << entry.data.className
                           << "', collision checks are skipped for '" << entry.data.instanceName
                           << "'.";
        }
        collisionModels_[entry.data.instanceName] = model;
        return model;
    }
 
    SceneEditor::Entry*
    SceneEditor::findEntry(const std::string& instanceName)
    {
        for (Entry& entry : entries_)
        {
            if (entry.data.instanceName == instanceName)
            {
                return &entry;
            }
        }
        return nullptr;
    }
 
    SceneEditor::Entry&
    SceneEditor::addObject(const std::string& classId)
    {
        Entry& entry = entries_.emplace_back();
        entry.data.className = classId;
        entry.data.instanceName = makeUniqueInstanceName(classId);
        entry.groundRef.clear(); // New objects start on the default ground.
        entry.data.position = Eigen::Vector3f(0.0f, 0.0f, groundZ_);
        entry.data.orientation = Eigen::Quaternionf::Identity();
        entry.data.isStatic = true;
        collisionModelOf(entry); // Preload, so the first move does not stall.
        entry.data.position.z() = groundSnappedZ(entry, poseOf(entry.data));
        sceneLayerDirty_ = true;
        tabRebuildNeeded_ = true;
        return entry;
    }
 
    void
    SceneEditor::renameEntry(Entry& entry, const std::string& newName)
    {
        const std::string oldName = entry.data.instanceName;
        if (newName.empty() || newName == noneOption)
        {
            status_ = "Cannot rename '" + oldName + "': invalid name '" + newName + "'.";
            syncGui_ = true;
            return;
        }
        if (newName == oldName)
        {
            return;
        }
        if (findEntry(newName) != nullptr)
        {
            status_ = "Cannot rename '" + oldName + "': an object named '" + newName +
                      "' already exists.";
            syncGui_ = true;
            return;
        }
 
        entry.data.instanceName = newName;
 
        // Update all references to the old name.
        for (Entry& other : entries_)
        {
            if (other.groundRef == oldName)
            {
                other.groundRef = newName;
            }
        }
        if (auto node = collisionModels_.extract(oldName); !node.empty())
        {
            node.key() = newName;
            collisionModels_.insert(std::move(node));
        }
        if (auto it = pendingTransforms_.find(oldName); it != pendingTransforms_.end())
        {
            pendingTransforms_[newName] = it->second;
            pendingTransforms_.erase(it);
        }
        if (selected_ == oldName)
        {
            selected_ = newName;
        }
 
        sceneLayerDirty_ = true;
        syncGui_ = true;
        tabRebuildNeeded_ = true;
        status_ = "Renamed '" + oldName + "' to '" + newName + "'.";
    }
 
    void
    SceneEditor::deleteEntry(const std::string& instanceName)
    {
        // Requires GCC >8
        // std::erase_if(entries_,
        //               [&](const Entry& e) { return e.data.instanceName == instanceName; });
 
        entries_.erase(
            std::remove_if(
                entries_.begin(),
                entries_.end(),
                [&](const Entry& e)
                {
                    return e.data.instanceName == instanceName;
                }),
            entries_.end());
 
 
        pendingTransforms_.erase(instanceName);
        collisionModels_.erase(instanceName);
        // Entries grounded to the deleted object fall back to the default ground.
        for (Entry& entry : entries_)
        {
            if (entry.groundRef == instanceName)
            {
                entry.groundRef.clear();
                if (!entry.manualPose)
                {
                    entry.data.position.z() = groundSnappedZ(entry, poseOf(entry.data));
                }
            }
        }
        if (selected_ == instanceName)
        {
            selected_.clear();
        }
        sceneLayerDirty_ = true;
        syncGui_ = true;
        tabRebuildNeeded_ = true;
    }
 
    void
    SceneEditor::flipEntry(Entry& entry, const Eigen::Vector3f& axis)
    {
        const Eigen::Quaternionf newOrientation =
            (Eigen::Quaternionf(Eigen::AngleAxisf(M_PI_2, axis)) * entry.data.orientation)
                .normalized();
 
        Eigen::Matrix4f newPose = Eigen::Matrix4f::Identity();
        newPose.block<3, 3>(0, 0) = newOrientation.toRotationMatrix();
        newPose.block<3, 1>(0, 3) = entry.data.position;
 
        // Flipping changes the bounding box, so re-snap locked objects.
        if (!entry.manualPose)
        {
            newPose(2, 3) = groundSnappedZ(entry, newPose);
        }
 
        if (!isMoveAllowed(entry, newPose))
        {
            return;
        }
 
        entry.data.orientation = newOrientation;
        entry.data.position.z() = newPose(2, 3);
        sceneLayerDirty_ = true;
        syncGui_ = true;
        status_ = "Flipped '" + entry.data.instanceName + "'.";
        afterEntryPoseChanged(entry);
    }
 
    void
    SceneEditor::selectEntry(const std::string& instanceName)
    {
        if (findEntry(instanceName))
        {
            selected_ = instanceName;
            syncGui_ = true;
        }
    }
 
    void
    SceneEditor::setGroundZ(float groundZ)
    {
        groundZ_ = groundZ;
        reseatEntriesGroundedTo("");
        groundLayerDirty_ = true;
        syncGui_ = true;
    }
 
    void
    SceneEditor::alignToPlane(int axis, bool minSide, const std::string& planeName)
    {
        Entry* selected = findEntry(selected_);
        if (!selected)
        {
            status_ = "No object selected.";
            return;
        }
        const std::string targetName = tab_.alignTarget.getValue();
        Entry* target = findEntry(targetName);
        if (!target)
        {
            status_ = "No reference object selected.";
            return;
        }
        if (target == selected)
        {
            status_ = "Cannot align an object with itself.";
            return;
        }
 
        const auto selectedBox = globalAabb(*selected, poseOf(selected->data));
        const auto targetBox = globalAabb(*target, poseOf(target->data));
        if (!selectedBox || !targetBox)
        {
            status_ = "Cannot align: missing bounding box for '" +
                      (selectedBox ? targetName : selected_) + "'.";
            return;
        }
 
        // Inside: make the selected object's face coplanar with the
        // reference's face. Outside: place the selected object beyond that
        // plane, touching it with its opposite face (with a small clearance
        // so that exact contact does not count as a collision).
        const bool outside = tab_.alignOutside.getValue();
        constexpr float clearance = 0.5f;
        float delta = 0.0f;
        if (outside)
        {
            delta = minSide
                        ? (targetBox->min()(axis) - clearance) - selectedBox->max()(axis)
                        : (targetBox->max()(axis) + clearance) - selectedBox->min()(axis);
        }
        else
        {
            delta = minSide ? targetBox->min()(axis) - selectedBox->min()(axis)
                            : targetBox->max()(axis) - selectedBox->max()(axis);
        }
 
        Eigen::Vector3f newPosition = selected->data.position;
        newPosition(axis) += delta;
 
        Eigen::Matrix4f newPose = poseOf(selected->data);
        newPose.block<3, 1>(0, 3) = newPosition;
 
        if (!isMoveAllowed(*selected, newPose))
        {
            syncGui_ = true;
            return;
        }
 
        selected->data.position = newPosition;
        sceneLayerDirty_ = true;
        syncGui_ = true;
        status_ = "Aligned '" + selected_ + "' with the " + planeName + " of '" + targetName +
                  (outside ? "' (outside)." : "' (inside).");
        afterEntryPoseChanged(*selected);
    }
 
    std::string
    SceneEditor::makeUniqueInstanceName(const std::string& classId)
    {
        const std::string className = ObjectID(classId).className();
        std::string name;
        do
        {
            name = className + "_" + std::to_string(nextId_++);
        } while (findEntry(name) != nullptr);
        return name;
    }
 
    void
    SceneEditor::rebuildSceneLayer()
    {
        sceneLayer_ = arviz.layer("Scene");
        for (const Entry& entry : entries_)
        {
            viz::Object object(entry.data.instanceName);
            object.fileByObjectFinder(entry.data.className, objectsPackage_);
            object.position(entry.data.position).orientation(entry.data.orientation);
 
            // Enable the full transform and constrain it client-side when the
            // resulting transformation is applied (see applyPendingTransform).
            viz::InteractionDescription interaction = viz::interaction();
            interaction.contextMenu(contextMenuEntries).hideDuringTransform().transform();
            object.enable(interaction);
 
            sceneLayer_.add(object);
        }
    }
 
    void
    SceneEditor::rebuildGroundLayer()
    {
        groundLayer_ = arviz.layer("Ground");
        // The default ground plane is always shown; a designated ground
        // object is drawn (and interactable) in the scene layer on top.
        viz::Box plane("GroundPlane");
        plane.position(Eigen::Vector3f(0.0f, 0.0f, groundZ_ - 5.0f))
            .size(Eigen::Vector3f(10000.0f, 10000.0f, 10.0f))
            .color(viz::Color(128, 128, 128, 64));
        groundLayer_.add(plane);
    }
 
    std::filesystem::path
    SceneEditor::resolveScenePath(std::string name) const
    {
        if (name.empty())
        {
            name = "NewScene";
        }
        if (not simox::alg::ends_with(name, ".json"))
        {
            name += ".json";
        }
        std::filesystem::path path(name);
        if (!path.is_absolute())
        {
            if (!sceneStorageDirectory_.empty())
            {
                // SceneStorageDirectory holds the scene files directly.
                path = std::filesystem::path(sceneStorageDirectory_) / path;
            }
            else
            {
                // Resolve relative to the scenes package's data directory.
                CMakePackageFinder packageFinder(scenesPackage_);
                const std::string dataDir = packageFinder.getDataDir();
                if (!packageFinder.packageFound() || dataDir.empty())
                {
                    throw LocalException(
                        "Could not locate the scenes package '" + scenesPackage_ +
                        "'. Set the SceneStorageDirectory property to an absolute path.");
                }
                path = std::filesystem::path(dataDir) / scenesPackage_ / "scenes" / path;
            }
        }
 
        // Make the path absolute so it does not depend on the process's
        // current working directory (which is not where scenes live).
        if (!path.is_absolute())
        {
            path = std::filesystem::absolute(path);
        }
        return path.lexically_normal();
    }
 
    std::filesystem::path
    SceneEditor::groundingPathFor(const std::filesystem::path& scenePath) const
    {
        std::vector<std::filesystem::path> parts(scenePath.begin(), scenePath.end());
        int lastScenes = -1;
        for (std::size_t i = 0; i < parts.size(); ++i)
        {
            if (parts[i].string() == "scenes")
            {
                lastScenes = static_cast<int>(i);
            }
        }
        if (lastScenes < 0)
        {
            return scenePath.parent_path() / (scenePath.stem().string() + ".groundings.json");
        }
        std::filesystem::path result;
        for (std::size_t i = 0; i < parts.size(); ++i)
        {
            result /= (static_cast<int>(i) == lastScenes ? std::filesystem::path("groundings")
                                                         : parts[i]);
        }
        return result;
    }
 
    void
    SceneEditor::saveScene(const std::string& fileArg)
    {
        try
        {
            const std::filesystem::path path = resolveScenePath(fileArg);
 
            objects::Scene scene;
            // Instance names are kept as they are; only unnamed entries get a
            // generated name (per class, not colliding with the kept names).
            std::set<std::string> takenNames;
            for (const Entry& entry : entries_)
            {
                takenNames.insert(entry.data.instanceName);
            }
            std::map<std::string, int> perClassCount;
            for (const Entry& entry : entries_)
            {
                objects::SceneObject& obj = scene.objects.emplace_back(entry.data);
                if (!obj.instanceName.empty())
                {
                    continue;
                }
                const std::string className = ObjectID(entry.data.className).className();
                do
                {
                    obj.instanceName =
                        className + "_" + std::to_string(perClassCount[className]++);
                } while (!takenNames.insert(obj.instanceName).second);
            }
 
            std::filesystem::create_directories(path.parent_path());
            const simox::json::json j = scene;
            simox::json::write(path.string(), j, 2);
 
            // Save the grounding info to a parallel directory with the same
            // file name, so it can be restored when the scene is loaded.
            const std::filesystem::path groundingPath = groundingPathFor(path);
            // Objects are referenced by their index in the scene file.
            const auto indexOf = [this](const std::string& instanceName) -> int
            {
                for (std::size_t i = 0; i < entries_.size(); ++i)
                {
                    if (entries_[i].data.instanceName == instanceName)
                    {
                        return static_cast<int>(i);
                    }
                }
                return -1;
            };
            simox::json::json grounding;
            grounding["groundings"] = simox::json::json::array();
            for (std::size_t i = 0; i < entries_.size(); ++i)
            {
                const Entry& entry = entries_[i];
                grounding["groundings"].push_back({
                    {"index", static_cast<int>(i)},
                    {"instanceName", scene.objects[i].instanceName},
                    {"ground", entry.groundRef.empty() ? -1 : indexOf(entry.groundRef)},
                    {"locked", !entry.manualPose},
                });
            }
            std::filesystem::create_directories(groundingPath.parent_path());
            simox::json::write(groundingPath.string(), grounding, 2);
 
            status_ = "Saved " + std::to_string(scene.objects.size()) + " objects to " +
                      path.string() + " (grounding info: " + groundingPath.string() + ").";
            ARMARX_INFO << status_;
        }
        catch (const std::exception& e)
        {
            status_ = std::string("Saving scene failed: ") + e.what();
            ARMARX_WARNING << status_;
        }
    }
 
    void
    SceneEditor::loadScene(const std::string& fileArg)
    {
        try
        {
            const std::filesystem::path path = resolveScenePath(fileArg);
            ARMARX_INFO << "Loading scene from " << path << ".";
            if (!std::filesystem::exists(path))
            {
                status_ = "Scene file does not exist: " + path.string();
                ARMARX_WARNING << status_;
                return;
            }
 
            const simox::json::json j = simox::json::read(path.string());
            const auto scene = j.get<objects::Scene>();
 
            entries_.clear();
            pendingTransforms_.clear();
            collisionModels_.clear();
            selected_.clear();
 
            for (const objects::SceneObject& obj : scene.objects)
            {
                Entry& entry = entries_.emplace_back();
                entry.data = obj;
                if (entry.data.instanceName.empty() ||
                    findEntry(entry.data.instanceName) != &entry)
                {
                    entry.data.instanceName = makeUniqueInstanceName(entry.data.className);
                }
            }
 
            // Restore the grounding info from the parallel groundings file.
            bool groundingLoaded = false;
            const std::filesystem::path groundingPath = groundingPathFor(path);
            if (std::filesystem::exists(groundingPath))
            {
                try
                {
                    const simox::json::json grounding =
                        simox::json::read(groundingPath.string());
                    for (const auto& item : grounding.at("groundings"))
                    {
                        // Objects are referenced by their index in the scene
                        // file; the instance name is a fallback.
                        Entry* entry = nullptr;
                        const int index = item.value("index", -1);
                        if (index >= 0 && index < static_cast<int>(entries_.size()))
                        {
                            entry = &entries_[index];
                        }
                        else if (item.contains("instanceName"))
                        {
                            entry = findEntry(item.at("instanceName").get<std::string>());
                        }
                        if (!entry)
                        {
                            continue;
                        }
 
                        entry->groundRef.clear();
                        if (item.contains("ground"))
                        {
                            const auto& ground = item.at("ground");
                            if (ground.is_number_integer())
                            {
                                const int groundIndex = ground.get<int>();
                                if (groundIndex >= 0 &&
                                    groundIndex < static_cast<int>(entries_.size()))
                                {
                                    entry->groundRef =
                                        entries_[groundIndex].data.instanceName;
                                }
                            }
                            else if (ground.is_string())
                            {
                                entry->groundRef = ground.get<std::string>();
                            }
                        }
                        entry->manualPose = !item.value("locked", true);
                    }
                    groundingLoaded = true;
                }
                catch (const std::exception& e)
                {
                    ARMARX_WARNING << "Failed to load grounding info from " << groundingPath
                                   << ": " << e.what();
                }
            }
 
            for (Entry& entry : entries_)
            {
                if (!entry.groundRef.empty() &&
                    (entry.groundRef == entry.data.instanceName ||
                     findEntry(entry.groundRef) == nullptr))
                {
                    entry.groundRef.clear();
                }
                if (groundingLoaded)
                {
                    if (!entry.manualPose)
                    {
                        entry.data.position.z() = groundSnappedZ(entry, poseOf(entry.data));
                    }
                }
                else
                {
                    // No grounding info: treat objects resting on the default
                    // ground as locked, everything else as manually posed.
                    entry.manualPose =
                        std::abs(entry.data.position.z() -
                                 groundSnappedZ(entry, poseOf(entry.data))) > 0.5f;
                }
            }
 
            sceneLayerDirty_ = true;
            groundLayerDirty_ = true;
            syncGui_ = true;
            tabRebuildNeeded_ = true;
            status_ = "Loaded " + std::to_string(entries_.size()) + " objects from " +
                      path.string() + ".";
            ARMARX_INFO << status_;
        }
        catch (const std::exception& e)
        {
            status_ = std::string("Loading scene failed: ") + e.what();
            ARMARX_WARNING << status_;
        }
    }
 
    float
    SceneEditor::yawOf(const Eigen::Quaternionf& q)
    {
        const Eigen::Matrix3f rot = q.toRotationMatrix();
        return std::atan2(rot(1, 0), rot(0, 0));
    }
 
    Eigen::Matrix4f
    SceneEditor::poseOf(const objects::SceneObject& obj)
    {
        Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
        pose.block<3, 3>(0, 0) = obj.orientation.toRotationMatrix();
        pose.block<3, 1>(0, 3) = obj.position;
        return pose;
    }
} // namespace armarx