Initial version of a RobotPoseUnit which can be used to move a robot in 6D...

Initial version of a RobotPoseUnit which can be used to move a robot in 6D (mainly useful in simulated worlds)

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

source/RobotAPI/components/units/RobotPoseUnit.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 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()));
+}

source/RobotAPI/components/units/RobotPoseUnit.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 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

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

source/RobotAPI/interface/units/RobotPoseUnitInterface.ice

+/*
+ * 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