FrameTracking.h

Go to the documentation of this file.

/*
 * 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    RobotAPI::ArmarXObjects::FrameTracking
 * @author     Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
 
#include <RobotAPI/interface/components/FrameTrackingInterface.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
 
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
 
#include <Eigen/Core>
 
namespace armarx
{
    /**
     * @class FrameTrackingPropertyDefinitions
     * @brief
     */
    class FrameTrackingPropertyDefinitions:
        public armarx::ComponentPropertyDefinitions
    {
    public:
        FrameTrackingPropertyDefinitions(std::string prefix):
            armarx::ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the robot state component that should be used");
            defineOptionalProperty<std::string>("KinematicUnitName", "KinematicUnit", "Name of the kinematic unit that should be used");
            defineOptionalProperty<std::string>("KinematicUnitObserverName", "KinematicUnitObserver", "Name of the kinematic unit observer that should be used");
            defineRequiredProperty<std::string>("HeadYawJoint", "Name of the yaw joint of the head.");
            defineRequiredProperty<std::string>("HeadPitchJoint", "Name of the pitch joint of the head.");
            defineRequiredProperty<std::string>("CameraNode", "Name of the camera node.");
            defineOptionalProperty<bool>("EnableTrackingOnStartup", true, "Start Tracking on startup. This needs a frame to be defined.");
            defineOptionalProperty<std::string>("FrameOnStartup", "", "Name of the frame that should be tracked.");
            defineOptionalProperty<std::string>("RemoteGuiName", "RemoteGuiProvider", "Name of the remote gui. Remote gui is disabled if empty.");
            defineOptionalProperty<float>("MaxYawVelocity", 0.8f, "The maximum velocity the yaw joint while tracking objects.");
            defineOptionalProperty<float>("YawAcceleration", 4.f, "The acceleration of the yaw joint while tracking objects.");
            defineOptionalProperty<float>("MaxPitchVelocity", 0.8f, "The maximum velocity the pitch joint while tracking objects.");
            defineOptionalProperty<float>("PitchAcceleration", 4.f, "The acceleration of the pitch joint while tracking objects.");
        }
    };
 
    /**
     * @defgroup Component-FrameTracking FrameTracking
     * @ingroup RobotAPI-Components
     * A description of the component FrameTracking.
     *
     * @class FrameTracking
     * @ingroup Component-FrameTracking
     * @brief Brief description of class FrameTracking.
     *
     * Detailed description of class FrameTracking.
     */
    class FrameTracking :
        virtual public armarx::Component,
        public FrameTrackingInterface
    {
    private:
        struct HeadState
        {
            HeadState() = default;
            HeadState(bool stop, float currentYawPos, float currentYawVel, float desiredYawPos, float currentPitchPos, float currentPitchVel, float desiredPitchPos)
                :
                stop {stop},
                currentYawPos  {currentYawPos  },
                currentYawVel  {currentYawVel  },
                desiredYawPos  {desiredYawPos  },
                currentPitchPos {currentPitchPos},
                currentPitchVel {currentPitchVel},
                desiredPitchPos {desiredPitchPos}
            {}
 
            bool stop = false;
            float currentYawPos;
            float currentYawVel;
            float desiredYawPos;
            float currentPitchPos;
            float currentPitchVel;
            float desiredPitchPos;
        };
    public:
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string getDefaultName() const override
        {
            return "FrameTracking";
        }
 
    protected:
        /**
         * @see armarx::ManagedIceObject::onInitComponent()
         */
        void onInitComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onConnectComponent()
         */
        void onConnectComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onDisconnectComponent()
         */
        void onDisconnectComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onExitComponent()
         */
        void onExitComponent() override;
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
        // FrameTrackingInterface interface
    public:
        void enableTracking(bool enable, const Ice::Current& = Ice::emptyCurrent) override;
        void setFrame(const std::string& frameName, const Ice::Current& = Ice::emptyCurrent) override;
        std::string getFrame(const Ice::Current& = Ice::emptyCurrent) const override;
 
        void lookAtFrame(const std::string& frameName, const Ice::Current& = Ice::emptyCurrent) override;
        void lookAtPointInGlobalFrame(const Vector3f& point, const Ice::Current& = Ice::emptyCurrent) override;
        bool isLookingAtPointInGlobalFrame(const Vector3f& point, float max_diff, const Ice::Current& = Ice::emptyCurrent) override;
        void lookAtPointInRobotFrame(const Vector3f& point, const Ice::Current& = Ice::emptyCurrent) override;
 
        void scanInConfigurationSpace(float yawFrom, float yawTo, const ::Ice::FloatSeq& pitchValues, float velocity, const Ice::Current& = Ice::emptyCurrent) override;
        void scanInWorkspace(const ::armarx::Vector3fSeq& points, float maxVelocity, float acceleration, const Ice::Current& = Ice::emptyCurrent) override;
        void moveJointsTo(float yaw, float pitch, const Ice::Current& = Ice::emptyCurrent) override;
 
    protected:
        void process();
 
        void syncronizeLocalClone();
        void _lookAtFrame(const std::string& frame);
        void _lookAtPoint(const Eigen::Vector3f& point);
        bool _looksAtPoint(const Eigen::Vector3f& point, float max_diff);
        HeadState _calculateJointAnglesContinously(const std::string& frame);
        HeadState _calculateJointAngles(const Eigen::Vector3f& point);
        void _doVelocityControl(const HeadState& headstate, float maxYawVelocity, float yawAcceleration, float maxPitchVelocity, float pitchAcceleration);
        void _doPositionControl(const HeadState& headstate);
        void _enableTracking(bool enable);
 
        RobotStateComponentInterfacePrx robotStateComponent;
        KinematicUnitInterfacePrx kinematicUnitInterfacePrx;// send commands to kinematic unit
        PeriodicTask<FrameTracking>::pointer_type processorTask;
        KinematicUnitObserverInterfacePrx kinematicUnitObserverInterfacePrx;
 
        std::atomic<bool> enabled;
        std::string frameName;
        float maxYawVelocity;
        float yawAcceleration;
        float maxPitchVelocity;
        float pitchAcceleration;
 
        VirtualRobot::RobotPtr localRobot;
        VirtualRobot::RobotNodePtr headYawJoint;
        VirtualRobot::RobotNodePtr headPitchJoint;
        VirtualRobot::RobotNodePtr cameraNode;
 
        RemoteGuiInterfacePrx                       _remoteGui;
        SimplePeriodicTask<>::pointer_type          _guiTask;
        RemoteGui::TabProxy                         _guiTab;
    };
}