Added simple ForceTorqueUnitSimulation

source/RobotAPI/applications/CMakeLists.txt

@@ -21,4 +21,5 @@ add_subdirectory(PlatformUnitObserver)
 add_subdirectory(RobotStateComponent)
 add_subdirectory(RobotStateObserver)
 add_subdirectory(HandUnitObserver)
+add_subdirectory(ForceTorqueUnitSimulation)
 

source/RobotAPI/applications/ForceTorqueUnitSimulation/CMakeLists.txt

+
+armarx_component_set_name(ForceTorqueUnitSimulation)
+
+set(COMPONENT_LIBS ArmarXInterfaces ArmarXCore RobotAPIUnits)
+
+set(SOURCES main.cpp ForceTorqueUnitSimulationApp.h)
+
+armarx_add_component_executable("${SOURCES}")

source/RobotAPI/applications/ForceTorqueUnitSimulation/ForceTorqueUnitSimulationApp.h

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * 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 Lesser 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    RobotAPI::applications
+ * @author     Peter Kaiser
+ * @date       2014
+ * @copyright  http://www.gnu.org/licenses/gpl.txt
+ *             GNU General Public License
+ */
+
+
+#include <Core/core/application/Application.h>
+#include <RobotAPI/units/ForceTorqueUnitSimulation.h>
+
+namespace armarx
+{
+    /**
+     * Application for testing the armarx::ForceTorqueUnitSimulation
+     */
+    class ForceTorqueUnitSimulationApp :
+        virtual public armarx::Application
+    {
+        /**
+         * @see armarx::Application::setup()
+         */
+        void setup(const ManagedIceObjectRegistryInterfacePtr &registry, Ice::PropertiesPtr properties) 
+        { 
+            registry->addObject(Component::create<ForceTorqueUnitSimulation>(properties));
+        }
+    };
+}

source/RobotAPI/applications/ForceTorqueUnitSimulation/main.cpp

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * 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 Lesser 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    RobotAPI::application::ForceTorqueUnitSimulation
+ * @author     Peter Kaiser
+ * @date       2014
+ * @copyright  http://www.gnu.org/licenses/gpl.txt
+ *             GNU General Public License
+ */
+
+#include "ForceTorqueUnitSimulationApp.h"
+
+int main(int argc, char* argv[])
+{
+    armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::ForceTorqueUnitSimulationApp>();
+    app->setName("ForceTorqueUnitSimulation");
+
+    return app->main(argc, argv);
+}

source/RobotAPI/units/CMakeLists.txt

@@ -30,6 +30,7 @@ set(LIBS RobotAPIInterfaces
 set(LIB_HEADERS
     ForceTorqueObserver.h
     ForceTorqueUnit.h
+    ForceTorqueUnitSimulation.h
     HeadIKUnit.h
     HapticUnit.h
     HapticObserver.h
@@ -52,6 +53,7 @@ set(LIB_HEADERS
 set(LIB_FILES
     ForceTorqueObserver.cpp
     ForceTorqueUnit.cpp
+    ForceTorqueUnitSimulation.cpp
     HeadIKUnit.cpp
     HapticUnit.cpp
     HapticObserver.cpp

source/RobotAPI/units/ForceTorqueUnitSimulation.cpp

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * 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 Lesser 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    RobotAPI::units
+ * @author     Peter Kaiser (peter dot kaiser at kit dot edu)
+ * @date       2014
+ * @copyright  http://www.gnu.org/licenses/gpl.txt
+ *             GNU General Public License
+ */
+
+
+#include "ForceTorqueUnitSimulation.h"
+
+#include <RobotAPI/robotstate/remote/ArmarPose.h>
+
+#include <boost/algorithm/string.hpp>
+
+using namespace armarx;
+
+void ForceTorqueUnitSimulation::onInitForceTorqueUnit()
+{
+    int intervalMs = getProperty<int>("IntervalMs").getValue();
+    std::string sensorNames = getProperty<std::string>("SensorNames").getValue();
+
+    std::vector<std::string> sensorNamesList;
+    boost::split(sensorNamesList, sensorNames, boost::is_any_of(","));
+
+    for(std::vector<std::string>::iterator s = sensorNamesList.begin(); s != sensorNamesList.end(); s++)
+    {
+        forces[*s] = new armarx::FramedVector3(Eigen::Vector3f(0, 0, 0), *s);
+        torques[*s] = new armarx::FramedVector3(Eigen::Vector3f(0, 0, 0), *s);
+    }
+
+    ARMARX_VERBOSE << "Starting ForceTorqueUnitSimulation with " << intervalMs << " ms interval";
+    simulationTask = new PeriodicTask<ForceTorqueUnitSimulation>(this, &ForceTorqueUnitSimulation::simulationFunction, intervalMs, false, "ForceTorqueUnitSimulation");
+}
+
+void ForceTorqueUnitSimulation::onStartForceTorqueUnit()
+{
+    simulationTask->start();
+}
+
+
+void ForceTorqueUnitSimulation::onExitForceTorqueUnit()
+{
+    simulationTask->stop();
+}
+
+
+void ForceTorqueUnitSimulation::simulationFunction()
+{
+    listenerPrx->reportSensorForceValues(forces);
+    listenerPrx->reportSensorTorqueValues(torques);
+
+    listenerPrx->reportSensorValues("force", forces);
+    listenerPrx->reportSensorValues("torque", torques);
+}
+
+void ForceTorqueUnitSimulation::setOffset(const FramedVector3Map &forceTorqueOffsets, const Ice::Current &c)
+{
+    // Ignore
+}
+
+void ForceTorqueUnitSimulation::setToNull(const StringMap &sensorNames, const Ice::Current &c)
+{
+    // Ignore
+}
+
+
+PropertyDefinitionsPtr ForceTorqueUnitSimulation::createPropertyDefinitions()
+{
+    return PropertyDefinitionsPtr(new ForceTorqueUnitSimulationPropertyDefinitions(getConfigIdentifier()));
+}
+

source/RobotAPI/units/ForceTorqueUnitSimulation.h

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * 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 Lesser 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    RobotAPI::units
+ * @author     Peter Kaiser (peter dot kaiser at kit dot edu)
+ * @date       2014
+ * @copyright  http://www.gnu.org/licenses/gpl.txt
+ *             GNU General Public License
+ */
+
+#ifndef ARMARX_COMPONENT_FORCE_TORQUE_UNIT_SIMULATION_H
+#define ARMARX_COMPONENT_FORCE_TORQUE_UNIT_SIMULATION_H
+
+#include "ForceTorqueUnit.h"
+
+#include <Core/core/services/tasks/PeriodicTask.h>
+#include <Core/core/system/ImportExportComponent.h>
+
+#include <IceUtil/Time.h>
+
+#include <string>
+
+namespace armarx
+{
+    /**
+     * @class ForceTorqueUnitSimulationPropertyDefinitions
+     * @brief
+     * @ingroup SensorActorUnits
+     */
+    class ForceTorqueUnitSimulationPropertyDefinitions :
+            public ForceTorqueUnitPropertyDefinitions
+    {
+    public:
+        ForceTorqueUnitSimulationPropertyDefinitions(std::string prefix) :
+            ForceTorqueUnitPropertyDefinitions(prefix)
+        {
+            defineRequiredProperty<std::string>("SensorNames", "Comma-separated list of simulated sensor names");
+            defineOptionalProperty<int>("IntervalMs", 10, "The time in milliseconds between two calls to the simulation method.");
+        }
+    };
+
+    /**
+     * @class PlatformUnitSimulation
+     * @brief
+     * @ingroup SensorActorUnits
+     *
+     * Simulates a set of Force/Torque sensors.
+     * The unit is given a list of sensor names as a property. It then publishes force/torque values under these names.
+     * The published values will always be zero.
+     */
+    class ForceTorqueUnitSimulation :
+        virtual public ForceTorqueUnit
+    {
+    public:
+        // inherited from Component
+        virtual std::string getDefaultName() const
+        {
+            return "ForceTorqueUnitSimulation";
+        }
+
+        virtual void onInitForceTorqueUnit();
+        virtual void onStartForceTorqueUnit();
+        virtual void onExitForceTorqueUnit();
+
+        void simulationFunction();
+
+        virtual void setOffset(const armarx::FramedVector3Map &forceTorqueOffsets, const Ice::Current& c = ::Ice::Current());
+        virtual void setToNull(const armarx::StringMap &sensorNames, const Ice::Current& c = ::Ice::Current());
+
+        /**
+         * @see PropertyUser::createPropertyDefinitions()
+         */
+        virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+    protected:
+        armarx::FramedVector3Map forces;
+        armarx::FramedVector3Map torques;
+
+        PeriodicTask<ForceTorqueUnitSimulation>::pointer_type simulationTask;
+
+
+    };
+}
+
+#endif