vor.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 "vor.h"
 
using namespace armarx;
 
void
VOR::calc()
{
    std::scoped_lock lock(dataMutex);
 
    //Check if all sensor values have at least been reported once to avoid segfaults
    if (!(reportedSensorValues && initialOrientation && reportedJointAnglesBool))
    {
        return;
    }
 
    if (velocityBased && !(reportedJointVelocitiesBool))
    {
        return;
    }
 
    Eigen::Quaternionf delta = initialQuaternion.inverse() * currentOrientation;
    Eigen::Vector3f rpy = quaternionToRPY(delta);
 
    NameValueMap reportedJointAngles = this->reportedJointAngles;
    NameValueMap reportedJointVelocities = this->reportedJointVelocities;
 
    reportedJointAnglesBool = false;
    reportedJointVelocitiesBool = false;
 
    //lock.unlock();
 
    std::map<std::string, float> headJoints;
 
    if (velocityBased)
    {
        std::vector<float> error = std::vector<float>(3);
        //        error[0] = rotationVelocity[0] + reportedJointVelocities[neck_roll];
        //        error[1] = rotationVelocity[1] + reportedJointVelocities[eye_pitch_left];
        //        error[2] = (-rotationVelocity[2]) + reportedJointVelocities[eye_yaw_left];
 
        //ARMARX_LOG_S << deactivateSpam(3) << "rotationVelocity: (" << rotationVelocity[0] << " / " << rotationVelocity[1] << " / " << rotationVelocity[2] << ")";
 
        //        error[0] = rpy[0];
        //        error[1] = (rpy[1] - reportedJointAngles[eye_pitch_left]);
        //        error[2] = (rpy[2] - reportedJointAngles[eye_yaw_left]);
 
        //ARMARX_LOG_S << "RPY: " << rpy[0] << "   " << rpy[1] << "   " << rpy[2];
 
        //std::vector<float> y = pid(error, {kp, kp, kp}, {ki, ki, ki}, {kd, kd, kd});
 
        //std::vector<float> y = pid(error, {kp, kp, kp}, {0, 0, 0}, {0, 0, 0});
 
        //ARMARX_LOG_S << "RPY PID: " << y[0] << "   " << y[1] << "   " << y[2];
 
        //        headJoints[eye_pitch_left] = -1.0 * (reportedJointVelocities[eye_pitch_left] + y[1]);
        //        if (armar4)
        //        {
        //            headJoints[eye_pitch_right] = (reportedJointVelocities[eye_pitch_left] + y[1]);
        //        }
        //        headJoints[eye_yaw_left] = -1 * (reportedJointVelocities[eye_yaw_left] + y[2]);
        //        headJoints[eye_yaw_right] = -1 * (reportedJointVelocities[eye_yaw_left] + y[2]);
        //headJoints[neck_roll] =  -1 * (reportedJointVelocities[neck_roll] + y[0]);
 
 
        //        headJoints[eye_pitch_left] = (reportedJointVelocities[eye_pitch_left] + y[1]);
        //        if (armar4)
        //        {
        //            headJoints[eye_pitch_right] = (reportedJointVelocities[eye_pitch_left] - y[1]);
        //        }
        //        headJoints[eye_yaw_left] = (reportedJointVelocities[eye_yaw_left] + y[2]);
        //        headJoints[eye_yaw_right] = (reportedJointVelocities[eye_yaw_left] + y[2]);
 
        headJoints[eye_pitch_left] =
            -1 * rotationVelocity[1] + kp * (rpy[1] - reportedJointAngles[eye_pitch_left]);
        headJoints[eye_pitch_left] = 0.;
 
        headJoints[eye_yaw_left] =
            rotationVelocity[2] + kp * (rpy[2] - reportedJointAngles[eye_yaw_left]);
        headJoints[eye_yaw_right] =
            rotationVelocity[2] + kp * (rpy[2] - reportedJointAngles[eye_yaw_right]);
        //headJoints[neck_roll] = (reportedJointVelocities[neck_roll] + y[0]);
    }
    else
    {
        // todo assume that the axis is parallel to the IMU.
        headJoints[eye_pitch_left] = -1.0 * rpy[1];
        if (armar4)
        {
            headJoints[eye_pitch_right] = rpy[1];
            rpy[0] = -rpy[0];
        }
        headJoints[eye_yaw_left] = 1 * rpy[2];
        headJoints[eye_yaw_right] = 1 * rpy[2];
        //headJoints[neck_roll] = 1 * (rpy[0] + reportedJointAngles[neck_roll]);
    }
 
    std::scoped_lock lock2(mutex);
 
    jointAngles.swap(headJoints);
}
 
void
VOR::reportSensorValues(const IMUData& values)
{
    std::scoped_lock lock(dataMutex);
 
    std::vector<float> q = values.orientationQuaternion;
 
    currentOrientation = Eigen::Quaternionf(q[0], q[1], q[2], q[3]);
 
    rotationVelocity.clear();
 
    rotationVelocity = values.gyroscopeRotation;
 
    reportedSensorValues = true;
 
    if (!initialOrientation)
    {
        initialOrientation = true;
 
        initialQuaternion = Eigen::Quaternionf(q[0], q[1], q[2], q[3]);
    }
}
 
void
VOR::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
VOR::setPIDValues(float kp, float ki, float kd)
{
    std::scoped_lock lock(mutex);
 
    this->kp = kp;
    this->ki = ki;
    this->kd = kd;
}
 
void
VOR::setBools(bool armar4, bool velocityBased)
{
    std::scoped_lock lock(mutex);
 
    this->armar4 = armar4;
    this->velocityBased = velocityBased;
}
 
std::vector<float>
VOR::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
armarx::VOR::onStop()
{
    std::scoped_lock lock(dataMutex);
 
    reportedSensorValues = false;
    initialOrientation = false;
 
    reportedJointVelocitiesBool = false;
    reportedJointAnglesBool = false;
 
 
    std::vector<float> err_old(3, 0.0);
    err_old.swap(this->err_old);
    std::vector<float> err_sum(3, 0.0);
    err_sum.swap(this->err_sum);
}
 
void
armarx::VOR::reportJointAngles(const NameValueMap& values, bool valueChanged, const Ice::Current& c)
{
    std::scoped_lock lock(dataMutex);
 
    reportedJointAnglesBool = true;
    this->reportedJointAngles = values;
}
 
void
armarx::VOR::reportJointVelocities(const NameValueMap& values,
                                   bool valueChanged,
                                   const Ice::Current& c)
{
    std::scoped_lock lock(dataMutex);
 
    reportedJointVelocitiesBool = true;
    this->reportedJointVelocities = values;
}