ArticulatedObjectLocalizerExample.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/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ArticulatedObjectLocalizerExample.h"
 
#include <memory>
#include <optional>
 
#include <Eigen/Geometry>
 
#include <IceUtil/Time.h>
 
#include <SimoxUtility/math/pose/pose.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/ScopedStopWatch.h>
#include <ArmarXCore/core/time/StopWatch.h>
 
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_objects/types.h>
 
namespace armarx::articulated_object
{
    ArticulatedObjectLocalizerExample::ArticulatedObjectLocalizerExample()
    {
        addPlugin(articulatedObjectReaderPlugin);
        addPlugin(articulatedObjectWriterPlugin);
    }
 
    armarx::PropertyDefinitionsPtr
    ArticulatedObjectLocalizerExample::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs =
            new ComponentPropertyDefinitions(getConfigIdentifier());
 
        defs->topic(debugObserver);
 
        defs->optional(p.updateFrequency, "updateFrequency", "Memory update frequency (write).");
 
        defs->optional(p.obj.dataset, "p.obj.dataset", "");
        defs->optional(p.obj.className, "p.obj.class", "");
 
 
        return defs;
    }
 
    std::string
    ArticulatedObjectLocalizerExample::getDefaultName() const
    {
        return "ArticulatedObjectLocalizerExample";
    }
 
    void
    ArticulatedObjectLocalizerExample::onInitComponent()
    {
        // auto& articulatedObjectReader = articulatedObjectReaderPlugin->get();
        auto& articulatedObjectWriter = articulatedObjectWriterPlugin->get();
 
        articulatedObjectWriter.setProviderName(getName());
    }
 
    void
    ArticulatedObjectLocalizerExample::onConnectComponent()
    {
        ARMARX_IMPORTANT << "Running example.";
        start = armem::Time::Now();
 
        task = new PeriodicTask<ArticulatedObjectLocalizerExample>(
            this,
            &ArticulatedObjectLocalizerExample::run,
            static_cast<int>(1000.f / p.updateFrequency));
        task->start();
    }
 
    void
    ArticulatedObjectLocalizerExample::onDisconnectComponent()
    {
        task->stop();
    }
 
    void
    ArticulatedObjectLocalizerExample::onExitComponent()
    {
    }
 
    VirtualRobot::RobotPtr
    ArticulatedObjectLocalizerExample::createArticulatedObject()
    {
        auto& articulatedObjectReader = articulatedObjectReaderPlugin->get();
 
        const std::string dishwasherName = p.obj.dataset + "/" + p.obj.className;
 
        const auto descriptions =
            articulatedObjectReader.queryDescriptions(armem::Time::Now(), std::nullopt);
 
        ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
 
        for (const auto& description : descriptions)
        {
            ARMARX_INFO << "- " << description.name;
        }
 
        const auto it = std::find_if(
            descriptions.begin(),
            descriptions.end(),
            [&](const armem::articulated_object::ArticulatedObjectDescription& desc) -> bool
            { return desc.name == dishwasherName; });
 
        if (it == descriptions.end())
        {
            ARMARX_WARNING << "Articulated object " << dishwasherName << " not (yet) available";
            return nullptr;
        }
 
        auto obj = VirtualRobot::RobotIO::loadRobot(it->xml.toSystemPath(),
                                                    VirtualRobot::RobotIO::eStructure);
 
        if (not obj)
        {
            return nullptr;
        }
 
        obj->setName("0"); // aka instance name
        obj->setType(it->name); // aka dataset/class name
 
        return obj;
    }
 
    void
    ArticulatedObjectLocalizerExample::run()
    {
        if (articulatedObject == nullptr)
        {
            articulatedObject = createArticulatedObject();
 
            if (articulatedObject == nullptr) // still
            {
                return;
            }
        }
 
        const armem::robot_state::RobotState state = [this]()
        {
            // Query state from memory
            {
                const std::optional<armem::robot_state::RobotState> state =
                    articulatedObjectReaderPlugin->get().queryState(
                        articulatedObject->getType() + "/" + articulatedObject->getName(),
                        Clock::Now(),
                        p.obj.readProviderName);
 
                if (state)
                {
                    return state.value();
                }
            }
 
            // The object does not exist in the memory (yet). Therefore, we create an initial state.
            armem::robot_state::RobotState state{.timestamp = Clock::Now(),
                                                 .globalPose = Eigen::Isometry3f::Identity(),
                                                 .jointMap = articulatedObject->getJointValues(),
                                                 .proprioception = std::nullopt};
 
            return state;
        }();
 
        articulatedObject->setGlobalPose(state.globalPose.matrix());
 
        ARMARX_DEBUG << "Reporting articulated objects";
 
        const armem::Time now = armem::Time::Now();
        const float t = static_cast<float>((now - start).toSecondsDouble());
 
        // move joints at certain frequency
        const float k = (1 + std::sin(t / (M_2_PI))) / 2; // in [0,1]
 
        auto jointValues = articulatedObject->getJointValues();
 
        for (auto& [name, jointValue] : jointValues)
        {
            const auto node = articulatedObject->getRobotNode(name);
            jointValue = node->unscaleJointValue(k, 0, 1);
        }
 
        // articulatedObject->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
        articulatedObject->setJointValues(jointValues);
 
        {
            core::time::ScopedStopWatch sw(
                [this](const armarx::Duration& dur)
                { ARMARX_INFO << "Storing object took " << dur << "."; });
 
            auto& articulatedObjectWriter = articulatedObjectWriterPlugin->get();
            articulatedObjectWriter.storeArticulatedObject(articulatedObject, now);
        }
    }
 
} // namespace armarx::articulated_object