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 <Eigen/Core>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
 
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
 
#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
#include <RobotAPI/interface/components/FrameTrackingInterface.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
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;
    };
} // namespace armarx