okr.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#pragma once
 
#include "ReflexCombination.h"
#include "reflex.h"
 
namespace armarx
{
    class ReflexCombination;
 
    class OKR : virtual public Reflex
    {
    public:
        OKR(int interval) : Reflex(interval)
        {
            flow_time = 0.;
        }
 
        ~OKR() override
        {
        }
 
        std::string
        getName() const override
        {
            return "OKR";
        }
 
        // Reflex interface
    private:
        void calc() override;
 
        void onStop() override;
        std::vector<float> pid(std::vector<float> error,
                               std::vector<float> kp,
                               std::vector<float> ki,
                               std::vector<float> kd);
        void removeSelfInducedOpticalFlow();
 
    private:
        std::mutex dataMutex;
 
        bool reportedSensorValues, reportedJointAnglesBool, reportedJointVelocitiesBool;
        bool armar4, velocityBased, newFlow;
        float kp, ki, kd;
        float flowX, flowY, flowT;
        float errorX, errorY;
        std::vector<float> err_old = std::vector<float>(3);
        std::vector<float> err_sum = std::vector<float>(3);
        NameValueMap reportedJointAngles, reportedJointVelocities;
        std::string eye_pitch_left, eye_pitch_right, eye_yaw_left, eye_yaw_right, neck_roll;
 
        long flow_time;
        std::map<long, NameValueMap> jointVelocities;
 
    public:
        void
        reportJointAngles(const NameValueMap& values, bool valueChanged, const Ice::Current& c);
        void reportJointVelocities(const NameValueMap& values,
                                   bool valueChanged,
                                   long timestamp,
                                   const Ice::Current& c);
 
        void setJointNames(std::string eye_pitch_left,
                           std::string eye_pitch_right,
                           std::string eye_yaw_left,
                           std::string eye_yaw_right,
                           std::string neck_roll);
        void setPIDValues(float kp, float ki, float kd);
        void setBools(bool armar4, bool velocityBased);
        void reportNewOpticalFlow(float x, float y, float deltaT, long timestamp);
        void reportNewTrackingError(Ice::Float pixelX,
                                    Ice::Float pixelY,
                                    Ice::Float angleX,
                                    Ice::Float angleY);
    };
} // namespace armarx