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 "reflex.h"
#include "vor.h"
#include "jointik.h"
#include "feedforward.h"
#include "okr.h"
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/observers/DebugObserver.h>
 
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/IK/GazeIK.h>
 
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
#include <VisionX/interface/components/ImageSourceSelectionInterface.h>
 
#include <RobotComponents/interface/GazeStabilizationInterface.h>
#include <ArmarXCore/observers/filters/DerivationFilter.h>
#include <ArmarXCore/observers/filters/ButterworthFilter.h>
#include <ArmarXCore/observers/filters/MedianFilter.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;
 
 
    };
}