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 <RobotAPI/libraries/core/FramedPose.h>
 
#include <SimoxUtility/algorithm/string/string_tools.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";
}