okr.cpp

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
 */
#include "okr.h"
 
using namespace armarx;
 
void
OKR::calc()
{
    std::scoped_lock lock(dataMutex);
 
    if (!newFlow)
    {
        return;
    }
 
    //    if (velocityBased && !reportedJointVelocitiesBool)
    //    {
    //        return;
    //    }
    else if (!velocityBased && !reportedJointAnglesBool)
    {
        return;
    }
 
    newFlow = false;
 
    //    float comp = 0.0;
    //    if (velocityBased)
    //    {
    //        comp = 3;
    //    }
    //    else
    //    {
    //        comp = 1;
    //    }
 
    /*if (flowX < comp && flowX > -comp)
    {
        flowX = 0.0;
    }
    if (flowY < comp && flowY > -comp)
    {
        flowY = 0.0;
    }*/
 
    NameValueMap headJoints;
    NameControlModeMap jointModes;
 
    if (velocityBased)
    {
        reportedJointVelocitiesBool = false;
 
        removeSelfInducedOpticalFlow();
 
        //headJoints[eye_pitch_left] = (flowY) - kp * errorY;
        //headJoints[eye_yaw_right] = -1.0 * (flowX) + kp * errorX;
 
        // gain < 1 is to avoid unstability with the head
        headJoints[eye_pitch_left] = 0.0; //0.8 * (flowY) - kp * errorY;
        headJoints[eye_yaw_right] = -0.8 * (flowX); // + kp * errorX;
 
        headJoints[eye_yaw_left] = headJoints[eye_yaw_right];
 
        // uncomment for baseline
        //headJoints[eye_pitch_left] = headJoints[eye_yaw_left] = headJoints[eye_yaw_right] = 0.;
    }
    else
    {
        reportedJointAnglesBool = false;
 
        headJoints[eye_pitch_left] = (reportedJointAngles[eye_pitch_left] + flowY);
        jointModes[eye_pitch_left] = ePositionControl;
 
        if (armar4)
        {
            headJoints[eye_pitch_right] = (reportedJointAngles[eye_pitch_left] + flowY);
            jointModes[eye_pitch_right] = ePositionControl;
        }
 
        headJoints[eye_yaw_left] = (reportedJointAngles[eye_yaw_left] - flowX);
        jointModes[eye_yaw_left] = ePositionControl;
 
        headJoints[eye_yaw_right] = (reportedJointAngles[eye_yaw_left] - flowX);
        jointModes[eye_yaw_right] = ePositionControl;
    }
 
    // mutexNodeSet.unlock();
 
    std::scoped_lock lock2(mutex);
 
    jointAngles.swap(headJoints);
}
 
void
OKR::removeSelfInducedOpticalFlow()
{
    NameValueMap velocities;
 
    if (jointVelocities.size() == 0)
    {
        ARMARX_WARNING << deactivateSpam(1) << "No self-induced velocity found (0)";
        return;
    }
 
    // find the velocities closest to the timestamp of the 2nd image
    for (auto& kv : jointVelocities)
    {
        //std::scoped_lock lock(dataMutex);
        long timestamp = kv.first;
 
        if (timestamp <= flow_time + flowT)
        {
            velocities = kv.second;
            jointVelocities.erase(
                kv.first); // erase the values belonging to the previous optical flow
        }
        else
        {
            velocities = kv.second;
            break;
        }
    }
 
    if (velocities.find(eye_yaw_right) == velocities.end()) // no velocity found
    {
        velocities = jointVelocities.rbegin()->second;
        ARMARX_WARNING << deactivateSpam(1) << "No self-induced velocity found";
    }
 
    flowY = flowY + velocities[eye_pitch_left];
    flowX = flowX - velocities[eye_yaw_right];
}
 
void
OKR::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)
{
    std::scoped_lock lock(mutex);
    this->eye_pitch_left = eye_pitch_left;
    this->eye_pitch_right = eye_pitch_right;
    this->eye_yaw_left = eye_yaw_left;
    this->eye_yaw_right = eye_yaw_right;
    this->neck_roll = neck_roll;
}
 
void
OKR::setPIDValues(float kp, float ki, float kd)
{
    std::scoped_lock lock(mutex);
 
    this->kp = kp;
    this->ki = ki;
    this->kd = kd;
}
 
void
OKR::setBools(bool armar4, bool velocityBased)
{
    std::scoped_lock lock(mutex);
 
    this->armar4 = armar4;
    this->velocityBased = velocityBased;
}
 
std::vector<float>
OKR::pid(std::vector<float> error,
         std::vector<float> kp,
         std::vector<float> ki,
         std::vector<float> kd)
{
    if (error.size() != kp.size() || kp.size() != ki.size() || ki.size() != kd.size())
    {
        ARMARX_WARNING_S << "Different arraysizes in HeadStabilization::pid";
        return {0.0, 0.0, 0.0};
    }
 
    std::vector<float> y;
 
    for (size_t i = 0; i < error.size(); i++)
    {
        err_sum[i] += error[i];
 
        y.push_back(kp[i] * error[i] + (ki[i] * err_sum[i]) + (kd[i] * (error[i] - err_old[i])));
 
        err_old[i] = error[i];
    }
 
    return y;
}
 
void
OKR::reportJointAngles(const NameValueMap& values, bool valueChanged, const Ice::Current& c)
{
    std::scoped_lock lock(dataMutex);
 
    reportedJointAnglesBool = true;
    this->reportedJointAngles = values;
}
 
void
OKR::reportJointVelocities(const NameValueMap& values,
                           bool valueChanged,
                           long timestamp,
                           const Ice::Current& c)
{
    std::scoped_lock lock(dataMutex);
 
    reportedJointVelocitiesBool = true;
    this->reportedJointVelocities = values;
 
    NameValueMap test(values);
    jointVelocities[timestamp] = test;
}
 
void
OKR::reportNewOpticalFlow(float x, float y, float deltaT, long timestamp)
{
    std::scoped_lock lock(dataMutex);
 
    newFlow = true;
    this->flowX = x;
    this->flowY = y;
    this->flowT = deltaT;
 
    flow_time = timestamp;
}
 
void
OKR::reportNewTrackingError(Ice::Float pixelX,
                            Ice::Float pixelY,
                            Ice::Float angleX,
                            Ice::Float angleY)
{
    std::scoped_lock lock(dataMutex);
 
    this->errorX = angleX;
    this->errorY = angleY;
}
 
void
OKR::onStop()
{
    std::scoped_lock lock(dataMutex);
 
    newFlow = false;
    reportedJointVelocitiesBool = false;
    reportedJointAnglesBool = false;
}