VirtualRobotWriterExample.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 "VirtualRobotWriterExample.h"
 
#include <memory>
 
#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/system/ArmarXDataPath.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/Time.h>
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/armem_objects/types.h>
 
namespace armarx::virtual_robot_writer_example
{
    VirtualRobotWriterExample::VirtualRobotWriterExample()
    {
        addPlugin(virtualRobotWriterPlugin);
    }
 
    armarx::PropertyDefinitionsPtr
    VirtualRobotWriterExample::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs =
            new ComponentPropertyDefinitions(getConfigIdentifier());
 
        // defs->topic(debugObserver);
 
        defs->optional(p.updateFrequency, "updateFrequency", "Memory update frequency (write).");
        defs->optional(p.robot.package, "robot.package", "");
        defs->optional(p.robot.path, "robot.path", "");
 
        return defs;
    }
 
    std::string
    VirtualRobotWriterExample::getDefaultName() const
    {
        return "VirtualRobotWriterExample";
    }
 
    void
    VirtualRobotWriterExample::onInitComponent()
    {
    }
 
    void
    VirtualRobotWriterExample::onConnectComponent()
    {
        ARMARX_IMPORTANT << "Running example.";
        start = armem::Time::Now();
 
        task = new PeriodicTask<VirtualRobotWriterExample>(
            this, &VirtualRobotWriterExample::run, static_cast<int>(1000.f / p.updateFrequency));
        task->start();
    }
 
    void
    VirtualRobotWriterExample::onDisconnectComponent()
    {
        task->stop();
    }
 
    void
    VirtualRobotWriterExample::onExitComponent()
    {
    }
 
    VirtualRobot::RobotPtr
    VirtualRobotWriterExample::loadRobot() const
    {
        auto robot =
            VirtualRobot::RobotIO::loadRobot(PackagePath(p.robot.package, p.robot.path).toSystemPath(),
                                             VirtualRobot::RobotIO::eStructure);
        return robot;
    }
 
 
    void
    VirtualRobotWriterExample::run()
    {
        if (robot == nullptr)
        {
            robot = loadRobot();
 
            if (robot == nullptr) // still
            {
                ARMARX_ERROR << "Loading robot failed";
                return;
            }
        }
 
        ARMARX_DEBUG << "Reporting robot";
        ARMARX_CHECK(virtualRobotWriterPlugin->get().storeDescription(*robot));
 
        const armem::Time now = armem::Time::Now();
        const float t = float((now - start).toSecondsDouble());
 
        // move joints at certain frequency
        const float m = (1 + std::sin(t / (M_2_PI * 10))) / 2; // in [0,1]
 
        auto jointValues = robot->getJointValues();
        for (auto& [k, v] : jointValues)
        {
            const auto node = robot->getRobotNode(k);
 
            if (node->isLimitless())
            {
                continue;
            }
 
            v = node->getJointLimitLow() +
                (node->getJointLimitHigh() - node->getJointLimitLow()) * m;
        }
 
        robot->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
        robot->setJointValues(jointValues);
 
        ARMARX_CHECK(virtualRobotWriterPlugin->get().storeState(*robot, armarx::Clock::Now()));
    }
 
} // namespace armarx::virtual_robot_writer_example