diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt index 22b602a95a1b56abc243668df62037f66548a89d..8699b02474e07073e095fbe9c8dbae808f1b8bb5 100644 --- a/source/RobotAPI/applications/CMakeLists.txt +++ b/source/RobotAPI/applications/CMakeLists.txt @@ -21,4 +21,5 @@ add_subdirectory(PlatformUnitObserver) add_subdirectory(RobotStateComponent) add_subdirectory(RobotStateObserver) add_subdirectory(HandUnitObserver) +add_subdirectory(ForceTorqueUnitSimulation) diff --git a/source/RobotAPI/applications/ForceTorqueUnitSimulation/CMakeLists.txt b/source/RobotAPI/applications/ForceTorqueUnitSimulation/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..173374347e61e8c78524947ac2fddb0f05c23a52 --- /dev/null +++ b/source/RobotAPI/applications/ForceTorqueUnitSimulation/CMakeLists.txt @@ -0,0 +1,8 @@ + +armarx_component_set_name(ForceTorqueUnitSimulation) + +set(COMPONENT_LIBS ArmarXInterfaces ArmarXCore RobotAPIUnits) + +set(SOURCES main.cpp ForceTorqueUnitSimulationApp.h) + +armarx_add_component_executable("${SOURCES}") diff --git a/source/RobotAPI/applications/ForceTorqueUnitSimulation/ForceTorqueUnitSimulationApp.h b/source/RobotAPI/applications/ForceTorqueUnitSimulation/ForceTorqueUnitSimulationApp.h new file mode 100644 index 0000000000000000000000000000000000000000..84c6d7d278680a2d4e194354663ad222c28940c0 --- /dev/null +++ b/source/RobotAPI/applications/ForceTorqueUnitSimulation/ForceTorqueUnitSimulationApp.h @@ -0,0 +1,44 @@ +/* + * 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 ®istry, Ice::PropertiesPtr properties) + { + registry->addObject(Component::create<ForceTorqueUnitSimulation>(properties)); + } + }; +} diff --git a/source/RobotAPI/applications/ForceTorqueUnitSimulation/main.cpp b/source/RobotAPI/applications/ForceTorqueUnitSimulation/main.cpp new file mode 100644 index 0000000000000000000000000000000000000000..398862ed8b6d5b6159043ea6bad6cc5e539ef054 --- /dev/null +++ b/source/RobotAPI/applications/ForceTorqueUnitSimulation/main.cpp @@ -0,0 +1,32 @@ +/* + * 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); +} diff --git a/source/RobotAPI/units/CMakeLists.txt b/source/RobotAPI/units/CMakeLists.txt index 0164bfeccee201feabbaae3a2da9ef9e4ddb8ec7..f06e2564c422776ea1f5798cfd075f15ed1f934c 100644 --- a/source/RobotAPI/units/CMakeLists.txt +++ b/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 diff --git a/source/RobotAPI/units/ForceTorqueUnitSimulation.cpp b/source/RobotAPI/units/ForceTorqueUnitSimulation.cpp new file mode 100644 index 0000000000000000000000000000000000000000..d4e0061bc5d0ada1af0471cf85f3bd7f9f2f5176 --- /dev/null +++ b/source/RobotAPI/units/ForceTorqueUnitSimulation.cpp @@ -0,0 +1,87 @@ +/* + * 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())); +} + diff --git a/source/RobotAPI/units/ForceTorqueUnitSimulation.h b/source/RobotAPI/units/ForceTorqueUnitSimulation.h new file mode 100644 index 0000000000000000000000000000000000000000..b62fb0a07222e0aafefbe2fdb9888c643f0ac219 --- /dev/null +++ b/source/RobotAPI/units/ForceTorqueUnitSimulation.h @@ -0,0 +1,98 @@ +/* + * 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