ForceTorqueUnitDynamicSimulation.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2013-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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    ArmarXCore::units
 * @author     nv
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ForceTorqueUnitDynamicSimulation.h"
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
using namespace armarx;
 
void
ForceTorqueUnitDynamicSimulation::onInitForceTorqueUnit()
{
    usingTopic("ForceTorqueDynamicSimulationValues");
    agentName = getProperty<std::string>("AgentName").getValue();
 
    std::string framesStr = getProperty<std::string>("ReportFrames").getValue();
    frames = simox::alg::split(framesStr, ",;");
    ARMARX_INFO << "ForceTorqueUnit initialized." << std::endl
                << "AgentName: " << agentName << ". " << std::endl
                << "Report Frames: " << frames;
    auto sensorRobotNodeSplit =
        armarx::Split(getProperty<std::string>("SensorRobotNodeMapping").getValue(), ",");
    for (auto& elem : sensorRobotNodeSplit)
    {
        simox::alg::trim(elem);
        std::vector<std::string> split = simox::alg::split(elem, ":");
        if (split.size() == 2)
        {
            sensorRobotNodeMapping[simox::alg::trim_copy(split[0])] =
                simox::alg::trim_copy(split[1]);
        }
    }
    reportInSensorFrame = getProperty<bool>("ReportInSensorFrame").getValue();
    usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
}
 
void
ForceTorqueUnitDynamicSimulation::onStartForceTorqueUnit()
{
    ARMARX_INFO << "Starting ForceTorqueUnit, requesting robot from RobotStateComponent...";
 
    try
    {
        RobotStateComponentInterfacePrx robotStateComponent =
            getProxy<RobotStateComponentInterfacePrx>(
                getProperty<std::string>("RobotStateComponentName").getValue());
        sharedRobot = robotStateComponent->getSynchronizedRobot();
        remoteRobot = RemoteRobot::createLocalClone(sharedRobot);
    }
    catch (...)
    {
        handleExceptions();
        terminate();
        return;
    }
 
    for (auto& frame : frames)
    {
        if (!frame.empty() && !remoteRobot->hasRobotNode(frame))
        {
            ARMARX_WARNING << "Robot " << remoteRobot->getName() << " does not know report frame '"
                           << frame << "'. Will not report FT values in this frame...";
        }
    }
}
 
void
ForceTorqueUnitDynamicSimulation::onExitForceTorqueUnit()
{
}
 
PropertyDefinitionsPtr
ForceTorqueUnitDynamicSimulation::createPropertyDefinitions()
{
    return PropertyDefinitionsPtr(
        new ForceTorqueUnitDynamicSimulationPropertyDefinitions(getConfigIdentifier()));
}
 
void
ForceTorqueUnitDynamicSimulation::reportForceTorque(const Vector3BasePtr& force,
                                                    const Vector3BasePtr& torque,
                                                    const std::string& sensorName,
                                                    const std::string& nodeName,
                                                    bool aValueChanged,
                                                    const Ice::Current& c)
{
    std::unique_lock lock(reportMutex);
    Vector3Ptr f = Vector3Ptr::dynamicCast(filterValues(sensorName, force, forceFilters));
    Vector3Ptr t = Vector3Ptr::dynamicCast(filterValues(sensorName, torque, torqueFilters));
 
    FramedDirectionPtr framedForce = new FramedDirection(f->toEigen(), nodeName, agentName);
    FramedDirectionPtr framedTorque = new FramedDirection(t->toEigen(), nodeName, agentName);
    RemoteRobot::synchronizeLocalClone(remoteRobot, sharedRobot);
    try
    {
        auto prx = listenerPrx->ice_batchOneway();
        // if sensor is named as a robot node, we use the robot node as report frame
        if (sensorName != nodeName && remoteRobot->hasRobotNode(sensorName))
        {
            framedForce->changeFrame(remoteRobot, sensorName);
            framedTorque->changeFrame(remoteRobot, sensorName);
        }
 
        if (reportInSensorFrame)
        {
            prx->reportSensorValues(sensorName, framedForce, framedTorque);
        }
 
        for (auto& frame : frames)
        {
            if (!frame.empty() && remoteRobot->hasRobotNode(frame))
            {
                FramedDirectionPtr framedForce1 =
                    new FramedDirection(f->toEigen(), nodeName, agentName);
                FramedDirectionPtr framedTorque1 =
                    new FramedDirection(t->toEigen(), nodeName, agentName);
                framedForce1->changeFrame(remoteRobot, frame);
                framedTorque1->changeFrame(remoteRobot, frame);
                prx->reportSensorValues(sensorName, framedForce1, framedTorque1);
            }
        }
 
        auto it = sensorRobotNodeMapping.find(sensorName);
        if (it != sensorRobotNodeMapping.end())
        {
            if (remoteRobot->hasRobotNode(it->second))
            {
                FramedDirectionPtr framedForce1 =
                    new FramedDirection(f->toEigen(), nodeName, agentName);
                FramedDirectionPtr framedTorque1 =
                    new FramedDirection(t->toEigen(), nodeName, agentName);
                framedForce1->changeFrame(remoteRobot, it->second);
                framedTorque1->changeFrame(remoteRobot, it->second);
                prx->reportSensorValues(it->second, framedForce1, framedTorque1);
                ARMARX_DEBUG << deactivateSpam(1, it->second) << "Reporting " << it->second << ": "
                             << framedForce1->output() << "\n"
                             << framedTorque1->output();
            }
            else
            {
                ARMARX_WARNING << deactivateSpam(10000, it->second) << "Robot does not have node "
                               << it->second;
            }
        }
        prx->ice_flushBatchRequests();
    }
    catch (...)
    {
        ARMARX_WARNING << deactivateSpam() << "Could not report FT vlaues:";
        handleExceptions();
    }
}
 
Vector3BasePtr
ForceTorqueUnitDynamicSimulation::filterValues(
    const std::string& sensorName,
    const Vector3BasePtr& torques,
    std::map<std::string, DatafieldFilterBasePtr>& filters)
{
    auto filter = [&, this](const std::string& dim, float value)
    {
        const auto name = sensorName + dim;
        auto itX = filters.find(name);
        if (itX == filters.end())
        {
            filters.insert(
                std::make_pair(name, new filters::ButterworthFilter(2, 100, Lowpass, 1)));
            itX = filters.find(name);
        }
        auto& filterX = *itX->second;
        filterX.update(0, new Variant(value));
        double r = filterX.getValue()->getDouble();
        return r;
    };
 
 
    Vector3BasePtr result =
        new Vector3(filter("x", torques->x), filter("y", torques->y), filter("z", torques->z));
    return result;
}
 
void
armarx::ForceTorqueUnitDynamicSimulation::setOffset(const armarx::FramedDirectionBasePtr&,
                                                    const armarx::FramedDirectionBasePtr&,
                                                    const Ice::Current&)
{
    ARMARX_INFO_S << "NYI";
}
 
void
armarx::ForceTorqueUnitDynamicSimulation::setToNull(const Ice::Current&)
{
    ARMARX_INFO_S << "NYI";
}
 
std::string
armarx::ForceTorqueUnitDynamicSimulation::GetDefaultName()
{
    return "ForceTorqueUnitDynamicSimulation";
}
 
ARMARX_REGISTER_COMPONENT_EXECUTABLE(::armarx::ForceTorqueUnitDynamicSimulation, ::armarx::ForceTorqueUnitDynamicSimulation::GetDefaultName());