ForceTorqueObserver.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/>.
*
* @package    ArmarX::
* @author     Mirko Waechter ( mirko.waechter at kit dot edu)
* @date       2013
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "ForceTorqueObserver.h"
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckEqualsWithTolerance.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
 
#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
#include <RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h>
#include <RobotAPI/libraries/core/observerfilters/OffsetFilter.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#define RAWFORCE "_rawforcevectors"
#define OFFSETFORCE "_forceswithoffsetvectors"
#define FILTEREDFORCE "_filteredforcesvectors"
#define RAWTORQUE "_rawtorquevectors"
#define OFFSETTORQUE "_torqueswithoffsetvectors"
#define FORCETORQUEVECTORS "_forceTorqueVectors"
#define POD_SUFFIX "_pod"
 
namespace armarx
{
    ForceTorqueObserver::ForceTorqueObserver()
    {
    }
 
    void
    ForceTorqueObserver::setTopicName(std::string topicName)
    {
        this->topicName = topicName;
    }
 
    void
    ForceTorqueObserver::onInitObserver()
    {
        if (topicName.empty())
        {
            usingTopic(getProperty<std::string>("ForceTorqueTopicName").getValue());
        }
        else
        {
            usingTopic(topicName);
        }
 
        // register all checks
        offerConditionCheck("updated", new ConditionCheckUpdated());
        offerConditionCheck("larger", new ConditionCheckLarger());
        offerConditionCheck("equals", new ConditionCheckEquals());
        offerConditionCheck("smaller", new ConditionCheckSmaller());
        offerConditionCheck("magnitudeinrange", new ConditionCheckMagnitudeInRange());
        offerConditionCheck("approx", new ConditionCheckEqualsWithTolerance());
        offerConditionCheck("magnitudelarger", new ConditionCheckMagnitudeLarger());
        offerConditionCheck("magnitudesmaller", new ConditionCheckMagnitudeSmaller());
 
        usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
        offeringTopic("DebugDrawerUpdates");
 
        auto sensorRobotNodeSplit =
            armarx::Split(getProperty<std::string>("SensorRobotNodeMapping").getValue(), ",");
        for (auto& elem : sensorRobotNodeSplit)
        {
            simox::alg::trim(elem);
            auto split = armarx::Split(elem, ":");
            if (split.size() >= 2)
            {
                sensorRobotNodeMapping.emplace(
                    simox::alg::trim_copy(split.at(0)),
                    std::make_pair(simox::alg::trim_copy(split.at(1)),
                                   split.size() >= 3 ? simox::alg::trim_copy(split.at(2)) : ""));
            }
        }
    }
 
    void
    ForceTorqueObserver::onConnectObserver()
    {
        robot = getProxy<RobotStateComponentInterfacePrx>(
            getProperty<std::string>("RobotStateComponentName").getValue());
        localRobot =
            RemoteRobot::createLocalCloneFromFile(robot, VirtualRobot::RobotIO::eStructure);
        debugDrawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
        if (getProperty<bool>("VisualizeForce").getValue())
        {
            visualizerTask = new PeriodicTask<ForceTorqueObserver>(
                this,
                &ForceTorqueObserver::visualizerFunction,
                1000 / getProperty<int>("VisualizeForceUpdateFrequency").getValue(),
                false,
                "visualizerFunction");
            visualizerTask->start();
        }
        updateRobotTask = new PeriodicTask<ForceTorqueObserver>(
            this,
            &ForceTorqueObserver::updateRobot,
            1000 / getProperty<int>("RobotUpdateFrequency").getValue(),
            false,
            "updateRobot",
            false);
        updateRobotTask->start();
    }
 
    void
    ForceTorqueObserver::visualizerFunction()
    {
        if (!localRobot)
        {
            return;
        }
        float maxTorque = getProperty<float>("MaxExpectedTorqueValue");
        float torqueVisuDeadZone = getProperty<float>("TorqueVisuDeadZone");
        float arrowLength = getProperty<float>("MaxForceArrowLength").getValue();
        float forceFactor = getProperty<float>("ForceVisualizerFactor").getValue();
        auto channels = getAvailableChannels(false);
        auto batchPrx = debugDrawer->ice_batchOneway();
        {
            std::unique_lock lock(dataMutex);
 
            std::set<std::string> frameAlreadyReported;
            for (auto& channel : channels)
            {
                try
                {
                    //            if (localRobot->hasRobotNode(channel.first))
                    {
                        DatafieldRefPtr field = DatafieldRefPtr::dynamicCast(
                            getForceDatafield(channel.first, Ice::emptyCurrent));
 
                        FramedDirectionPtr value = field->getDataField()->get<FramedDirection>();
                        if (frameAlreadyReported.count(value->frame) > 0 ||
                            (value->frame != GlobalFrame && !value->frame.empty() &&
                             !localRobot->hasRobotNode(value->frame)))
                        {
                            continue;
                        }
                        frameAlreadyReported.insert(value->frame);
                        auto force = value->toGlobal(localRobot);
                        ARMARX_DEBUG << deactivateSpam(5, channel.first) << "new force "
                                     << channel.first << " : " << force->toEigen()
                                     << " original frame: " << value->frame;
 
                        float forceMag = force->toEigen().norm() * forceFactor;
                        Vector3Ptr pos =
                            new Vector3(localRobot->getRobotNode(value->frame)->getGlobalPose());
                        forceMag = std::min(1.0f, forceMag);
                        batchPrx->setArrowVisu("Forces",
                                               "Force" + channel.first,
                                               pos,
                                               force,
                                               DrawColor{forceMag, 1.0f - forceMag, 0.0f, 0.5f},
                                               std::max(arrowLength * forceMag, 10.f),
                                               3);
 
                        field = DatafieldRefPtr::dynamicCast(
                            getTorqueDatafield(channel.first, Ice::emptyCurrent));
                        value = field->getDataField()->get<FramedDirection>();
                        auto torque = value;
                        //                    ARMARX_INFO << deactivateSpam(1, torque->frame) << "Reporting for " << channel.first << " in frame " << torque->frame;
 
                        Eigen::Vector3f dirXglobal =
                            FramedDirection(Eigen::Vector3f::UnitX(), torque->frame, torque->agent)
                                .toGlobalEigen(localRobot);
                        Eigen::Vector3f dirYglobal =
                            FramedDirection(Eigen::Vector3f::UnitY(), torque->frame, torque->agent)
                                .toGlobalEigen(localRobot);
                        Eigen::Vector3f dirZglobal =
                            FramedDirection(Eigen::Vector3f::UnitZ(), torque->frame, torque->agent)
                                .toGlobalEigen(localRobot);
                        //                ARMARX_INFO << channel.first << VAROUT(torque) << VAROUT(dirXglobal) << VAROUT(dirYglobal) << VAROUT(dirZglobal);
                        if (fabs(torque->x) > torqueVisuDeadZone)
                        {
                            batchPrx->setCircleArrowVisu("Torques",
                                                         "TorqueX" + channel.first,
                                                         pos,
                                                         new Vector3(dirXglobal),
                                                         50,
                                                         torque->x / maxTorque,
                                                         3 * std::max(1.0f, torque->x / maxTorque),
                                                         DrawColor{1.0f, 0.0f, 0.0f, 0.5f});
                        }
                        else
                        {
                            batchPrx->removeCircleVisu("Torques", "TorqueX" + channel.first);
                        }
                        if (fabs(torque->y) > torqueVisuDeadZone)
                        {
                            batchPrx->setCircleArrowVisu("Torques",
                                                         "TorqueY" + channel.first,
                                                         pos,
                                                         new Vector3(dirYglobal),
                                                         50,
                                                         torque->y / maxTorque,
                                                         3 * std::max(1.0f, torque->y / maxTorque),
                                                         DrawColor{0.0f, 1.0f, 0.0f, 0.5f});
                        }
                        else
                        {
                            batchPrx->removeCircleVisu("Torques", "TorqueY" + channel.first);
                        }
                        if (fabs(torque->z) > torqueVisuDeadZone)
                        {
                            batchPrx->setCircleArrowVisu("Torques",
                                                         "TorqueZ" + channel.first,
                                                         pos,
                                                         new Vector3(dirZglobal),
                                                         50,
                                                         torque->z / maxTorque,
                                                         3 * std::max(1.0f, torque->z / maxTorque),
                                                         DrawColor{0.0f, 0.0f, 1.0f, 0.5f});
                        }
                        else
                        {
                            batchPrx->removeCircleVisu("Torques", "TorqueZ" + channel.first);
                        }
                    }
 
                    //            else
                    //            {
                    //                //            ARMARX_INFO << deactivateSpam(5,channel.first) << "No node name " << channel.first;
                    //            }
                }
                catch (...)
                {
                }
            }
        }
        batchPrx->ice_flushBatchRequests();
    }
 
    PropertyDefinitionsPtr
    ForceTorqueObserver::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(
            new ForceTorqueObserverPropertyDefinitions(getConfigIdentifier()));
    }
 
    void
    ForceTorqueObserver::updateRobot()
    {
        std::unique_lock lock(dataMutex);
        RemoteRobot::synchronizeLocalClone(localRobot, robot);
    }
 
    void
    ForceTorqueObserver::offerValue(const std::string& nodeName,
                                    const std::string& type,
                                    const FramedDirectionBasePtr& value,
                                    const DataFieldIdentifierPtr& id)
    {
        FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(value);
 
 
        try
        {
            setDataFieldFlatCopy(id->channelName, id->datafieldName, new Variant(value));
        }
        catch (...)
        {
            ARMARX_INFO << "Creating force/torque fields for node " << nodeName << " with id "
                        << id->getIdentifierStr();
            if (!existsDataField(id->channelName, id->datafieldName))
            {
                if (!existsChannel(id->channelName))
                {
                    offerChannel(id->channelName,
                                 type + " vectors on specific parts of the robot.");
                }
                offerDataFieldWithDefault(id->channelName,
                                          id->datafieldName,
                                          Variant(value),
                                          "3D vector for " + type + " of " + nodeName);
            }
        }
 
 
        // pod = plain old data
        std::string pod_channelName = id->channelName + POD_SUFFIX;
 
        try
        {
            StringVariantBaseMap values;
            values[id->datafieldName + "_x"] = new Variant(vec->x);
            values[id->datafieldName + "_y"] = new Variant(vec->y);
            values[id->datafieldName + "_z"] = new Variant(vec->z);
            values[id->datafieldName + "_frame"] = new Variant(vec->frame);
            setDataFieldsFlatCopy(pod_channelName, values);
        }
        catch (...)
        {
            ARMARX_INFO << "Creating force/torque pod fields";
            if (!existsChannel(pod_channelName))
            {
                offerChannel(pod_channelName,
                             id->datafieldName + " on " + nodeName + " as plain old data (pod)");
            }
            offerOrUpdateDataField(
                pod_channelName, id->datafieldName + "_x", Variant(vec->x), type + " on X axis");
            offerOrUpdateDataField(
                pod_channelName, id->datafieldName + "_y", Variant(vec->y), type + " on Y axis");
            offerOrUpdateDataField(
                pod_channelName, id->datafieldName + "_z", Variant(vec->z), type + " on Z axis");
            offerOrUpdateDataField(pod_channelName,
                                   id->datafieldName + "_frame",
                                   Variant(vec->frame),
                                   "Frame of " + value->frame);
        }
    }
 
    void
    armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNodeName,
                                                    const FramedDirectionBasePtr& forces,
                                                    const FramedDirectionBasePtr& torques,
                                                    const Ice::Current& c)
    {
        std::unique_lock lock(dataMutex);
 
        auto offerForceAndTorqueValue = [=, this](std::string const& sensorNodeName,
                                                  std::string const& frame,
                                                  std::string channelName,
                                                  const FramedDirectionBasePtr& forces,
                                                  const FramedDirectionBasePtr& torques)
        {
            try
            {
                DataFieldIdentifierPtr id;
 
                if (forces)
                {
                    if (channelName.empty())
                    {
                        id = getForceDatafieldId(sensorNodeName, frame);
                    }
                    else
                    {
                        id = new DataFieldIdentifier(getName(), channelName, "forces");
                    }
 
                    offerValue(sensorNodeName, id->datafieldName, forces, id);
                }
 
                if (torques)
                {
                    if (channelName.empty())
                    {
                        id = getTorqueDatafieldId(sensorNodeName, frame);
                    }
                    else
                    {
                        id = new DataFieldIdentifier(getName(), channelName, "torques");
                    }
 
                    offerValue(sensorNodeName, id->datafieldName, torques, id);
                }
 
                updateChannel(id->channelName);
                updateChannel(id->channelName + POD_SUFFIX);
            }
            catch (std::exception&)
            {
                ARMARX_ERROR << "Reporting force torque for " << sensorNodeName << " failed! ";
                handleExceptions();
            }
        };
 
        if (!localRobot)
        {
            return;
        }
 
 
        FramedDirectionPtr realTorques = FramedDirectionPtr::dynamicCast(torques);
        realTorques->changeFrame(localRobot, forces->frame);
 
        FramedDirectionPtr realForces = FramedDirectionPtr::dynamicCast(forces);
        offerForceAndTorqueValue(sensorNodeName, forces->frame, "", realForces, realTorques);
 
        auto sensorMappingRange = sensorRobotNodeMapping.equal_range(sensorNodeName);
        for (auto iter = sensorMappingRange.first; iter != sensorMappingRange.second; ++iter)
        {
            auto& sensorName = iter->first;
            auto& robotNodeName = iter->second.first;
            auto& channelName = iter->second.second;
            FramedDirectionPtr forcesCopy = FramedDirectionPtr::dynamicCast(realForces->clone());
            FramedDirectionPtr torquesCopy = FramedDirectionPtr::dynamicCast(realTorques->clone());
            forcesCopy->changeFrame(localRobot, robotNodeName);
            torquesCopy->changeFrame(localRobot, robotNodeName);
 
            offerForceAndTorqueValue(
                sensorName, robotNodeName, channelName, forcesCopy, torquesCopy);
        }
    }
 
    DatafieldRefBasePtr
    armarx::ForceTorqueObserver::createNulledDatafield(
        const DatafieldRefBasePtr& forceTorqueDatafieldRef,
        const Ice::Current&)
    {
        return createFilteredDatafield(new filters::OffsetFilter(), forceTorqueDatafieldRef);
    }
 
    DatafieldRefBasePtr
    ForceTorqueObserver::getForceDatafield(const std::string& nodeName, const Ice::Current&)
    {
        auto id = getForceDatafieldId(nodeName, nodeName);
 
        if (!existsChannel(id->channelName))
        {
            throw UserException("No sensor for node '" + nodeName + "'available: channel " +
                                id->channelName);
        }
 
        if (!existsDataField(id->channelName, id->datafieldName))
        {
            throw UserException("No sensor for node '" + nodeName + "'available: datafield " +
                                id->datafieldName);
        }
 
        return new DatafieldRef(this, id->channelName, id->datafieldName);
    }
 
    DatafieldRefBasePtr
    ForceTorqueObserver::getTorqueDatafield(const std::string& nodeName, const Ice::Current&)
    {
        auto id = getTorqueDatafieldId(nodeName, nodeName);
 
        if (!existsChannel(id->channelName))
        {
            throw UserException("No sensor for node '" + nodeName + "'available: channel " +
                                id->channelName);
        }
 
        if (!existsDataField(id->channelName, id->datafieldName))
        {
            throw UserException("No sensor for node '" + nodeName + "'available: datafield " +
                                id->datafieldName);
        }
 
        return new DatafieldRef(this, id->channelName, id->datafieldName);
    }
 
    DataFieldIdentifierPtr
    ForceTorqueObserver::getForceDatafieldId(const std::string& nodeName, const std::string& frame)
    {
        std::string channelName;
 
        if (frame == nodeName)
        {
            channelName = nodeName;
        }
        else
        {
            channelName = nodeName + "_" + frame;
        }
 
        std::string datafieldName = "forces";
        DataFieldIdentifierPtr id = new DataFieldIdentifier(getName(), channelName, datafieldName);
        return id;
    }
 
    DataFieldIdentifierPtr
    ForceTorqueObserver::getTorqueDatafieldId(const std::string& nodeName, const std::string& frame)
    {
        std::string channelName;
 
        if (frame == nodeName)
        {
            channelName = nodeName;
        }
        else
        {
            channelName = nodeName + "_" + frame;
        }
 
        std::string datafieldName = "torques";
        DataFieldIdentifierPtr id = new DataFieldIdentifier(getName(), channelName, datafieldName);
        return id;
    }
 
    void
    ForceTorqueObserver::onDisconnectComponent()
    {
        if (visualizerTask)
        {
            visualizerTask->stop();
        }
        if (updateRobotTask)
        {
            updateRobotTask->stop();
        }
    }
 
    void
    ForceTorqueObserver::onExitObserver()
    {
    }
} // namespace armarx