ReflexCombination.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    RobotComponents::ArmarXObjects::ReflexCombination
 * @author     [Author Name] ( [Author Email] )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
 
#include <VirtualRobot/IK/GazeIK.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/observers/DebugObserver.h>
#include <ArmarXCore/observers/filters/ButterworthFilter.h>
#include <ArmarXCore/observers/filters/DerivationFilter.h>
#include <ArmarXCore/observers/filters/MedianFilter.h>
 
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <VisionX/interface/components/ImageSourceSelectionInterface.h>
 
#include <RobotComponents/interface/GazeStabilizationInterface.h>
 
#include "feedforward.h"
#include "jointik.h"
#include "okr.h"
#include "reflex.h"
#include "vor.h"
 
namespace armarx
{
    enum COMBINATION_METHOD
    {
        WeightedSum,
        Reafference
    };
 
    /**
     * @class ReflexCombinationPropertyDefinitions
     * @brief
     */
    class ReflexCombinationPropertyDefinitions : public armarx::ComponentPropertyDefinitions
    {
    public:
        ReflexCombinationPropertyDefinitions(std::string prefix) :
            armarx::ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<bool>(
                "VelocityBasedControl", true, "Use velocity based reflexes");
 
            defineOptionalProperty<float>("kp", 1.0, "p part of regulator");
            defineOptionalProperty<float>("ki", 0.0, "i part of regulator");
            defineOptionalProperty<float>("kd", 0.0, "d part of regulator");
 
            defineOptionalProperty<float>("VOR", 0, "Vestibulo-ocular reflex weight");
            defineOptionalProperty<float>("OKR", 1, "Opto-kinetic reflex weight");
            defineOptionalProperty<float>("JointIK", 0, "Joint IK weight");
            defineOptionalProperty<float>(
                "AdvancedVOR", 0, "Advanced Vestibulo-ocular reflex weight");
 
            defineOptionalProperty<std::string>("KinematicUnitName",
                                                "Armar3KinematicUnit",
                                                "Name of the KinematicUnit of the Robot");
            defineOptionalProperty<std::string>(
                "IMUObserverName",
                "InertialMeasurementUnitObserver",
                "Name of the InertialMeasurementUnitObserver of the Robot");
            defineOptionalProperty<std::string>(
                "RobotStateName", "RobotStateComponent", "Name of the RobotState of the Robot");
            defineOptionalProperty<std::string>(
                "HeadIKName", "IKVirtualGazeNoEyes", "Name of the HeadIK");
            defineOptionalProperty<std::string>(
                "RobotNodeSetName", "Robot", "Name of the RobotNodeSet");
 
            defineOptionalProperty<std::string>("ImageSourceSelectionName",
                                                "ImageSourceSelection",
                                                "Name of the ImageSourceSelection component");
            defineOptionalProperty<Ice::StringSeq>(
                "ImageProviders",
                {"Armar3ImageProvider", "Armar3FovealImageProvider"},
                "Name of the image provider to choose from. If image quality is high, the second "
                "provider will be used.");
 
            defineOptionalProperty<std::string>("DebugObserverName",
                                                "DebugObserver",
                                                "Name of the topic the DebugObserver listens on");
 
            defineOptionalProperty<std::string>(
                "CalibrationFile",
                "ArmarXSimulation/camera_simulator_640x480.txt",
                "Camera calibration file, will be made available in the SLICE interface");
 
            defineOptionalProperty<bool>(
                "NeckPerturbation",
                false,
                "Add sinusoidal motion in the neck joint to test the reflexes");
            defineOptionalProperty<bool>("reafferenceCombination",
                                         false,
                                         "Combined reflexes with IK based on the reafference "
                                         "principle (sensory cancelation)");
        }
    };
 
    class Reflex;
    class VOR;
    class JointIK;
    class FeedforwardReflex;
    class OKR;
    class AdvancedVOR;
 
    /**
     * @defgroup Component-ReflexCombination ReflexCombination
     * @ingroup RobotComponents-Components
     * A description of the component ReflexCombination.
     *
     * @class ReflexCombination
     * @ingroup Component-ReflexCombination
     * @brief Brief description of class ReflexCombination.
     *
     * Detailed description of class ReflexCombination.
     */
    class ReflexCombination :
        virtual public armarx::Component,
        virtual public armarx::GazeStabilizationInterface
    {
    public:
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string
        getDefaultName() const override
        {
            return "ReflexCombination";
        }
 
        void updateWeights(float vor,
                           float okr,
                           float jointIK,
                           const Ice::Current& c = Ice::emptyCurrent) override;
 
        void setReafferenceMethod(bool isReafference,
                                  const Ice::Current& c = Ice::emptyCurrent) override;
 
 
    protected:
        /**
         * @see armarx::ManagedIceObject::onInitComponent()
         */
        void onInitComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onConnAdvancedVORectComponent()
         */
        void onConnectComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onDisconnectComponent()
         */
        void onDisconnectComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onExitComponent()
         */
        void onExitComponent() override;
 
        /**
         * @see PropertyUser::createPropertyDefiAdvancedVORnitions()
         */
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
 
    private:
        void setImageQuality(float quality);
 
        std::mutex mutex;
        std::map<std::string, NameValueMap> reflexValues;
        std::map<std::string, ReflexPtr> reflexes;
        std::vector<std::string> headJointNames;
 
        VOR* vor;
        FeedforwardReflex* jointIK;
        OKR* okr;
 
 
        bool armar4, velocityBased, neckPerturbation;
        bool vorEnabled, jointIKEnabled, okrEnabled;
        std::string eye_pitch_left, eye_pitch_right, eye_yaw_left, eye_yaw_right, neck_roll;
        float vorWeight, okrWeight, jointIKWeight;
        float kp, ki, kd;
 
        COMBINATION_METHOD combinationMethod;
 
        PeriodicTask<ReflexCombination>::pointer_type execCombineReflexesTask;
 
 
        //required for JointIK
        VirtualRobot::RobotPtr robot;
        VirtualRobot::RobotNodeSetPtr robotNodeSet;
 
        std::string headIKName;
        RobotStateComponentInterfacePrx robotStateComponent;
        Eigen::Quaternionf initial;
 
        NameValueMap targetJointAngles;
 
        KinematicUnitInterfacePrx kinUnitPrx;
        DebugObserverInterfacePrx debugObserver;
        DebugDrawerInterfacePrx drawer;
        ImageSourceSelectionInterfacePrx imageSourceSelection;
        bool newHeadTarget;
        //RobotStateComponentInterfacePrx robotStateComponent;
 
        std::map<std::string, IceInternal::Handle<armarx::filters::DerivationFilter>>
            VelocityFilters;
        std::map<std::string, IceInternal::Handle<armarx::filters::MedianFilter>> IMU_GyroFilters;
        std::map<std::string, IceInternal::Handle<armarx::filters::ButterworthFilter>>
            PreVelocityFilters;
        std::map<std::string, IceInternal::Handle<armarx::filters::ButterworthFilter>> FlowFilters;
 
        double t_init;
 
        // PlatformUnitListener interface
    public:
        void reportPlatformVelocity(Ice::Float currentPlatformVelocityX,
                                    Ice::Float currentPlatformVelocityY,
                                    Ice::Float currentPlatformVelocityRotation,
                                    const Ice::Current& c = Ice::emptyCurrent) override;
        void reportPlatformOdometryPose(Ice::Float,
                                        Ice::Float,
                                        Ice::Float,
                                        const Ice::Current&) override;
 
        // KinematicUnitListener interface
    public:
        void reportControlModeChanged(const NameControlModeMap&,
                                      Ice::Long timestamp,
                                      bool,
                                      const Ice::Current& c = Ice::emptyCurrent) override;
        void reportJointAngles(const NameValueMap& values,
                               Ice::Long timestamp,
                               bool valueChanged,
                               const Ice::Current& c = Ice::emptyCurrent) override;
        void reportJointVelocities(const NameValueMap& values,
                                   Ice::Long timestamp,
                                   bool valueChanged,
                                   const Ice::Current& c = Ice::emptyCurrent) override;
        void reportJointTorques(const NameValueMap&,
                                Ice::Long timestamp,
                                bool,
                                const Ice::Current& c = Ice::emptyCurrent) override;
        void reportJointAccelerations(const NameValueMap&,
                                      Ice::Long timestamp,
                                      bool,
                                      const Ice::Current& c = Ice::emptyCurrent) override;
        void reportJointCurrents(const NameValueMap&,
                                 Ice::Long timestamp,
                                 bool,
                                 const Ice::Current& c = Ice::emptyCurrent) override;
        void reportJointMotorTemperatures(const NameValueMap&,
                                          Ice::Long timestamp,
                                          bool,
                                          const Ice::Current& c = Ice::emptyCurrent) override;
        void reportJointStatuses(const NameStatusMap&,
                                 Ice::Long timestamp,
                                 bool,
                                 const Ice::Current& c = Ice::emptyCurrent) override;
 
 
    private:
        void combineReflexes();
        Eigen::Vector3f quaternionToRPY(Eigen::Quaternionf q);
 
        // OpticalFlowListener interface
    public:
        void reportNewOpticalFlow(Ice::Float,
                                  Ice::Float,
                                  Ice::Float,
                                  Ice::Long,
                                  const Ice::Current&) override;
 
        // HeadIKUnitListener interface
    public:
        void reportHeadTargetChanged(const NameValueMap& targetJointAngles,
                                     const FramedPositionBasePtr& targetPosition,
                                     const Ice::Current& c = Ice::emptyCurrent) override;
 
        // InertialMeasurementUnitListener interface
    public:
        void reportSensorValues(const std::string& device,
                                const std::string& name,
                                const IMUData& values,
                                const TimestampBasePtr& timestamp,
                                const Ice::Current& c = Ice::emptyCurrent) override;
 
        // HeadIKUnitListener interface
    public:
        void
        reportHeadTargetChanged(const Ice::Current&)
        {
        }
 
        // TrackingErrorListener interface
    public:
        void reportNewTrackingError(Ice::Float pixelX,
                                    Ice::Float pixelY,
                                    Ice::Float angleX,
                                    Ice::Float angleY,
                                    const Ice::Current&) override;
    };
} // namespace armarx