ArticulatedObjectLocalizerExample.cpp

/*
 * 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 "ArmarXCore/core/services/tasks/PeriodicTask.h"


#include <memory>

#include <Eigen/Geometry>

#include <IceUtil/Time.h>

#include <SimoxUtility/math/pose/pose.h>

#include <VirtualRobot/Robot.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/VirtualRobot.h>

#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/CycleUtil.h>

#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_objects/types.h>


namespace armarx::articulated_object
{
    ArticulatedObjectLocalizerExample::ArticulatedObjectLocalizerExample() :
        articulatedObjectWriter(new ::armarx::armem::articulated_object::Writer(memoryNameSystem)),
        articulatedObjectReader(new ::armarx::armem::articulated_object::Reader(memoryNameSystem))
    {
    }

    armarx::PropertyDefinitionsPtr ArticulatedObjectLocalizerExample::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs =
            new ComponentPropertyDefinitions(getConfigIdentifier());

        defs->topic(debugObserver);

        defs->optional(p.updateFrequency, "updateFrequency", "Memory update frequency (write).");

        articulatedObjectWriter->registerPropertyDefinitions(defs);
        articulatedObjectReader->registerPropertyDefinitions(defs);

        return defs;
    }

    std::string ArticulatedObjectLocalizerExample::getDefaultName() const
    {
        return "ArticulatedObjectLocalizerExample";
    }

    void ArticulatedObjectLocalizerExample::onInitComponent() {}

    void ArticulatedObjectLocalizerExample::onConnectComponent()
    {
        articulatedObjectWriter->connect();
        articulatedObjectReader->connect();

        ARMARX_IMPORTANT << "Running example.";
        start = armem::Time::now();

        task = new PeriodicTask<ArticulatedObjectLocalizerExample>(this, &ArticulatedObjectLocalizerExample::run, 1000.f / p.updateFrequency);
        task->start();
    }

    void ArticulatedObjectLocalizerExample::onDisconnectComponent()
    {
        task->stop();
    }

    void ArticulatedObjectLocalizerExample::onExitComponent() {}

    VirtualRobot::RobotPtr ArticulatedObjectLocalizerExample::createDishwasher()
    {
        const std::string dishwasherName = "CupboardWithDishwasher";

        const auto descriptions = articulatedObjectReader->queryDescriptions(IceUtil::Time::now());

        ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";

        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;
        }

        return VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path), VirtualRobot::RobotIO::eStructure);
    }

    armem::articulated_object::ArticulatedObject convert(const VirtualRobot::Robot& obj, const armem::Time& timestamp)
    {
        ARMARX_DEBUG << "Filename is " << obj.getFilename();

        return
            armem::articulated_object::ArticulatedObject
        {
            .description = {
                .name = obj.getName(),
                .xml = PackagePath(armarx::ArmarXDataPath::getProject({"ArmarXObjects"}, obj.getFilename()), obj.getFilename())
            },
            .instance = "", // TODO(fabian.reister):
            .config = {
                .timestamp = timestamp,
                .globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
                .jointMap = obj.getJointValues()
            },
            .timestamp = timestamp
        };
    }

    void ArticulatedObjectLocalizerExample::run()
    {
        if (dishwasher == nullptr)
        {
            dishwasher = createDishwasher();

            if (dishwasher == nullptr) // still
            {
                return;
            }
        }

        ARMARX_DEBUG << "Reporting articulated objects";

        const IceUtil::Time now = TimeUtil::GetTime();
        const float t = float((now - start).toSecondsDouble());

        // move joints at certain frequency
        const float k = (1 + std::sin(t / (M_2_PIf32))) / 2; // in [0,1]

        const std::map<std::string, float> jointValues
        {
            {"dishwasher_door_joint", M_PIf32 / 2 * k},
            {"drawer_joint", 350 * k}
        };

        dishwasher->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
        dishwasher->setJointValues(jointValues);

        armarx::armem::articulated_object::ArticulatedObject armemDishwasher =
            convert(*dishwasher, IceUtil::Time::now());
        articulatedObjectWriter->store(armemDishwasher);
    }

} // namespace armarx::articulated_object