From 8bab1cc5457002dbbf50f644da020f7786b1b76a Mon Sep 17 00:00:00 2001
From: Nikolaus Vahrenkamp <vahrenkamp@kit.edu>
Date: Fri, 18 Mar 2016 16:19:50 +0100
Subject: [PATCH] Initial version of a RobotPoseUnit which can be used to move
a robot in 6D (mainly useful in simulated worlds)
---
.../RobotAPI/components/units/CMakeLists.txt | 2 +
.../components/units/RobotPoseUnit.cpp | 74 ++++++++++
.../RobotAPI/components/units/RobotPoseUnit.h | 129 ++++++++++++++++++
source/RobotAPI/interface/CMakeLists.txt | 1 +
.../units/RobotPoseUnitInterface.ice | 95 +++++++++++++
5 files changed, 301 insertions(+)
create mode 100644 source/RobotAPI/components/units/RobotPoseUnit.cpp
create mode 100644 source/RobotAPI/components/units/RobotPoseUnit.h
create mode 100644 source/RobotAPI/interface/units/RobotPoseUnitInterface.ice
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index 451bf415e..503f3e2e3 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -42,6 +42,7 @@ set(LIB_HEADERS
KinematicUnit.h
KinematicUnitSimulation.h
PlatformUnit.h
+ RobotPoseUnit.h
PlatformUnitSimulation.h
KinematicUnitObserver.h
PlatformUnitObserver.h
@@ -69,6 +70,7 @@ set(LIB_FILES
KinematicUnitSimulation.cpp
PlatformUnit.cpp
PlatformUnitSimulation.cpp
+ RobotPoseUnit.cpp
KinematicUnitObserver.cpp
PlatformUnitObserver.cpp
SensorActorUnit.cpp
diff --git a/source/RobotAPI/components/units/RobotPoseUnit.cpp b/source/RobotAPI/components/units/RobotPoseUnit.cpp
new file mode 100644
index 000000000..9c8d5709e
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotPoseUnit.cpp
@@ -0,0 +1,74 @@
+/*
+ * 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 ArmarXCore::units
+ * @author Nikolaus Vahrenkamp (vahrenkamp at kit dot edu)
+ * @date 2016
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include "RobotPoseUnit.h"
+
+
+using namespace armarx;
+
+
+void RobotPoseUnit::onInitComponent()
+{
+ std::string RobotPoseName = getProperty<std::string>("RobotName").getValue();
+
+ // component dependencies
+ listenerChannelName = RobotPoseName + "6DPoseState";
+ offeringTopic(listenerChannelName);
+
+ this->onInitRobotPoseUnit();
+}
+
+
+void RobotPoseUnit::onConnectComponent()
+{
+ ARMARX_INFO << "setting report topic to " << listenerChannelName << flush;
+ listenerPrx = getTopic<RobotPoseUnitListenerPrx>(listenerChannelName);
+
+ this->onStartRobotPoseUnit();
+}
+
+
+void RobotPoseUnit::onDisconnectComponent()
+{
+ listenerPrx = NULL;
+
+ this->onStopRobotPoseUnit();
+}
+
+
+void RobotPoseUnit::onExitComponent()
+{
+ this->onExitRobotPoseUnit();
+}
+
+
+void RobotPoseUnit:: moveTo(PoseBasePtr targetPose, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c)
+{
+}
+
+
+PropertyDefinitionsPtr RobotPoseUnit::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new RobotPoseUnitPropertyDefinitions(
+ getConfigIdentifier()));
+}
diff --git a/source/RobotAPI/components/units/RobotPoseUnit.h b/source/RobotAPI/components/units/RobotPoseUnit.h
new file mode 100644
index 000000000..1017ee3e3
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotPoseUnit.h
@@ -0,0 +1,129 @@
+/*
+ * 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 ArmarXCore::units
+ * @author Nikolaus Vahrenkamp (vahrenkamp at kit dot edu)
+ * @date 2016
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_CORE_UNITS_ROBOTPOSEUNIT_H
+#define _ARMARX_CORE_UNITS_ROBOTPOSEUNIT_H
+
+#include <RobotAPI/components/units/SensorActorUnit.h>
+
+#include <ArmarXCore/core/application/properties/Properties.h>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include <RobotAPI/interface/units/RobotPoseUnitInterface.h>
+
+#include <vector>
+
+namespace armarx
+{
+ /**
+ * \class RobotPoseUnitPropertyDefinitions
+ * \brief Defines all necessary properties for armarx::RobotPoseUnit
+ */
+ class RobotPoseUnitPropertyDefinitions:
+ public ComponentPropertyDefinitions
+ {
+ public:
+ RobotPoseUnitPropertyDefinitions(std::string prefix):
+ ComponentPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::string>("RobotName", "Robot", "Name of the Robot (will publish values on RobotName + '6DPoseState')");
+ }
+ };
+
+
+ /**
+ * \defgroup Component-RobotPoseUnit RobotPoseUnit
+ * \ingroup RobotAPI-SensorActorUnits
+ * \brief Base unit for high-level access to robot poses.
+ *
+ * This class defines an interface for providing high level access to the robot's pose in the world.
+ * This unit usually does not exiust on a real robot (instead, use the @see PlatformUnit for moving a platform based robot around).
+ * In simulated worlds, this unit allows to position the robot globally in the envirnoment (@see RobotPoseUnitDynamicSimulation).
+ * It uses the RobotPoseUnitListener Ice interface to report updates of its current state.
+ */
+
+ /**
+ * @ingroup Component-RobotPoseUnit
+ * @brief The RobotPoseUnit class
+ */
+ class RobotPoseUnit :
+ virtual public RobotPoseUnitInterface,
+ virtual public SensorActorUnit
+ {
+ public:
+ // inherited from Component
+ virtual std::string getDefaultName() const
+ {
+ return "RobotPoseUnit";
+ }
+
+ /**
+ * Retrieve proxy for publishing State information and call
+ * armarx::RobotPoseUnit::onInitRobotPoseUnit().
+ * \see armarx::Component::onInitComponent()
+ */
+ virtual void onInitComponent();
+ /**
+ * Calls armarx::RobotPoseUnit::onStartRobotPoseUnit().
+ * \see armarx::Component::onConnectComponent()
+ */
+ virtual void onConnectComponent();
+
+ virtual void onDisconnectComponent();
+ /**
+ * Calls armarx::RobotPoseUnit::onExitRobotPoseUnit().
+ * \see armarx::Component::onExitComponent()
+ */
+ virtual void onExitComponent();
+
+ virtual void onInitRobotPoseUnit() = 0;
+ virtual void onStartRobotPoseUnit() = 0;
+ virtual void onStopRobotPoseUnit() {}
+ virtual void onExitRobotPoseUnit() = 0;
+
+ /**
+ * moveTo moves the robot to a global pose specified by:
+ * @param targetPose Global target pose
+ * @param postionalAccuracy Robot stops translating if distance to target position gets lower than this threshhold.
+ * @param orientationalAccuracy Robot stops rotating if distance from current to target orientation gets lower than this threshhold.
+ **/
+ virtual void moveTo(PoseBasePtr targetPose, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c = ::Ice::Current());
+
+ void stopMovement(const Ice::Current& c = Ice::Current()) {}
+ /**
+ * \see armarx::PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ protected:
+ /**
+ * RobotPoseUnitListener proxy for publishing state updates
+ */
+ RobotPoseUnitListenerPrx listenerPrx;
+ /**
+ * Ice Topic name on which armarx::RobotPoseUnit::listenerPrx publishes information
+ */
+ std::string listenerChannelName;
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 34dae6a59..250737dce 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -30,6 +30,7 @@ set(SLICE_FILES
units/HeadIKUnit.ice
units/KinematicUnitInterface.ice
units/PlatformUnitInterface.ice
+ units/RobotPoseUnitInterface.ice
units/TCPControlUnit.ice
units/TCPMoverUnitInterface.ice
units/UnitInterface.ice
diff --git a/source/RobotAPI/interface/units/RobotPoseUnitInterface.ice b/source/RobotAPI/interface/units/RobotPoseUnitInterface.ice
new file mode 100644
index 000000000..72dd6d622
--- /dev/null
+++ b/source/RobotAPI/interface/units/RobotPoseUnitInterface.ice
@@ -0,0 +1,95 @@
+/*
+ * 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::Core
+ * @author Nikolaus Vahrenkamp (vahrenkamp at kit dot edu)
+ * @copyright 2016
+ * @license http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_CORE_UNITS_ROBOTPOSEUNIT_SLICE_
+#define _ARMARX_CORE_UNITS_ROBOTPOSEUNIT_SLICE_
+
+#include <RobotAPI/interface/units/UnitInterface.ice>
+#include <RobotAPI/interface/core/PoseBase.ice>
+
+#include <ArmarXCore/interface/core/UserException.ice>
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+
+module armarx
+{
+ /**
+ * Implements an interface to a RobotPoseUnit.
+ * This unit is supposed to move a robot around in 6D (@see PlatformUnit is a similar unit, but with the restriction to move the robot in the plane).
+ * In particular useful for simulated robots.
+ **/
+ interface RobotPoseUnitInterface extends SensorActorUnitInterface
+ {
+ /**
+ * moveTo moves the robot to a global pose specified by:
+ * @param targetPose Global target pose
+ * @param postionalAccuracy Robot stops translating if distance to target position gets lower than this threshhold.
+ * @param orientationalAccuracy Robot stops rotating if distance from current to target orientation gets lower than this threshhold.
+ **/
+ void moveTo(PoseBase targetPose, float positionalAccuracy, float orientationalAccuracy);
+ /**
+ * move moves the robot with given velocities.
+ * @param targetVelocity target velocity defined in gloabl coordinates.
+ **/
+ void move(PoseBase targetVelocity);
+ /**
+ * moveRelative moves to a pose defined in robot's local coordinates.
+ * @param relativeTarget relative targe in robot's coordinate system
+ * @param postionalAccuracy Platform stops translating if distance to target position gets lower than this threshhold.
+ * @param orientationalAccuracy Platform stops rotating if distance from current to target orientation gets lower than this threshhold.
+ **/
+ void moveRelative(PoseBase relativeTarget, float positionalAccuracy, float orientationalAccuracy);
+ /**
+ * setMaxVelocities allows to specify max velocities in translation and orientation.
+ * @param positionalVelocity Max translation velocity.
+ * @param orientationalVelocity Max orientational velocity.
+ **/
+ void setMaxVelocities(float positionalVelocity, float orientationalVelocity);
+ /**
+ * stopMovement stops the movements of the robot.
+ **/
+ void stopMovement();
+ };
+ /**
+ * Implements an interface to an RobotPoseUnitListener.
+ **/
+ interface RobotPoseUnitListener
+ {
+ /**
+ * reportRobotPose reports current robot pose.
+ * @param currentRobotPose Global pose of the robot.
+ **/
+ void reportRobotPose(PoseBase currentRobotPose);
+ /**
+ * reportNewTargetPose reports a newly set target pose of the robot.
+ * @param newRobotPose Global target pose.
+ **/
+ void reportNewTargetPose(PoseBase newRobotPose);
+ /**
+ * reportRobotVelocity reports current robot velocities.
+ * @param currentRobotvelocity Current velocity on global frame.
+ **/
+ void reportRobotVelocity(PoseBase currentRobotvelocity);
+ };
+
+};
+
+#endif
--
GitLab