ForceTorqueUnitDynamicSimulation.h

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     Nikolaus Vahrenkamp <vahrenkamp at kit dot edu>
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/Properties.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <RobotAPI/components/units/SensorActorUnit.h>
 
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/components/units/ForceTorqueUnit.h>
#include <ArmarXSimulation/interface/simulator/SimulatorInterface.h>
#include <ArmarXCore/observers/filters/ButterworthFilter.h>
 
#include <string>
#include <mutex>
 
namespace armarx
{
    class ForceTorqueUnitDynamicSimulationPropertyDefinitions : public ForceTorqueUnitPropertyDefinitions
    {
    public:
        ForceTorqueUnitDynamicSimulationPropertyDefinitions(std::string prefix) :
            ForceTorqueUnitPropertyDefinitions(prefix)
        {
 
            //defineRequiredProperty<std::string>("RobotNodeName","Name of the robot node as stored in the XML file. There must be a force/torque sensor attached to this node.");
            defineOptionalProperty<std::string>("ReportFrames", "", "Comma separated list of robotNodes in which the force information should be transformed and reported.");
            defineOptionalProperty<std::string>("SensorRobotNodeMapping", "FT L:TCP L,FT R:TCP R", "Comma separated list of a sensor->robotNodes mapping. The sensors will be reported with the robot node name as as sensor name and in the frame of the robot node.");
            //defineOptionalProperty<std::string>("SimulatorProxyName", "Simulator", "Name of the simulator proxy to use.");
            defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the RobotStateComponent that should be used");
            defineOptionalProperty<bool>("ReportInSensorFrame", true, "Report the sensor values also in the frame of the sensor.");
 
        }
    };
 
    /**
    * @class ForceTorqueUnitDynamicSimulation
    * @brief This unit connects to the physics simulator topic (default: "Simulator") and reports force torque values.
    *
    * @ingroup SensorActorUnits
    * @ingroup ArmarXSimulatorComponents
    */
    class ForceTorqueUnitDynamicSimulation :
        virtual public ForceTorqueUnit,
        virtual public ForceTorqueUnitDynamicSimulationInterface
    {
    public:
        std::string getDefaultName() const override
        {
            return "ForceTorqueUnitDynamicSimulation";
        }
 
        void onInitForceTorqueUnit() override;
        void onStartForceTorqueUnit() override;
        void onExitForceTorqueUnit() override;
 
        PropertyDefinitionsPtr createPropertyDefinitions() override;
    protected:
 
        // implement SimulatorForceTorqueListenerInterface to receive robot updates from simulator
        void reportForceTorque(const Vector3BasePtr& force, const Vector3BasePtr& torque, const std::string& sensorName, const std::string& nodeName, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
        Vector3BasePtr filterValues(const std::string& sensorName, const Vector3BasePtr& torques, std::map<std::string, DatafieldFilterBasePtr>& filters);
        std::string agentName;
        // ForceTorqueUnitInterface interface
    public:
        void setOffset(const FramedDirectionBasePtr&, const FramedDirectionBasePtr&, const Ice::Current&) override;
        void setToNull(const Ice::Current&) override;
 
        VirtualRobot::RobotPtr remoteRobot;
        SharedRobotInterfacePrx sharedRobot;
        std::vector<std::string> frames;
        armarx::StringStringDictionary sensorRobotNodeMapping;
        std::map<std::string, DatafieldFilterBasePtr> torqueFilters;
        std::map<std::string, DatafieldFilterBasePtr> forceFilters;
        bool reportInSensorFrame = true;
        std::mutex reportMutex;
    };
}