ReflexCombination.cpp

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
 */
 
#include "ReflexCombination.h"
 
#include <ArmarXCore/observers/ObserverObjectFactories.h>
 
 
using namespace armarx;
 
void
ReflexCombination::onInitComponent()
{
    usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
    usingProxy(getProperty<std::string>("RobotStateName").getValue());
    offeringTopic(getProperty<std::string>("DebugObserverName").getValue());
    usingProxy(getProperty<std::string>("ImageSourceSelectionName").getValue());
 
    offeringTopic("DebugDrawerUpdates");
 
 
    usingTopic(getProperty<std::string>("RobotNodeSetName").getValue() + "State");
    usingTopic("IMUValues");
    usingTopic("PlatformState");
    usingTopic("HeadIKUnitTopic");
    usingTopic("OpticalFlowTopic");
    usingTopic("OpticalFlowOKRTopic");
    //usingTopic("TrackingErrorTopic");
 
    armar4 = !(getProperty<std::string>("KinematicUnitName").getValue().find("Armar3") == 0);
 
    headIKName = getProperty<std::string>("HeadIKName").getValue();
 
    if (armar4)
    {
        eye_pitch_left = "EyeL_1_joint";
        eye_pitch_right = "EyeR_1_joint";
        eye_yaw_left = "EyeL_2_joint";
        eye_yaw_right = "EyeR_2_joint";
        neck_roll = "Neck_2_joint";
    }
    else
    {
        eye_pitch_left = "Cameras";
        eye_yaw_left = "Eye_Left";
        eye_yaw_right = "Eye_Right";
        neck_roll = "Neck_2_Roll";
 
        headJointNames = {
            "Neck_1_Pitch", neck_roll, "Neck_3_Yaw", eye_pitch_left, eye_yaw_right, eye_yaw_left};
    }
 
    velocityBased = getProperty<bool>("VelocityBasedControl").getValue();
    neckPerturbation = getProperty<bool>("NeckPerturbation").getValue();
 
    if (getProperty<bool>("reafferenceCombination").getValue())
    {
        combinationMethod = Reafference;
    }
    else
    {
        combinationMethod = WeightedSum;
    }
 
    ARMARX_LOG << "Velocity based: " << velocityBased;
 
    vorWeight = getProperty<float>("VOR").getValue();
    okrWeight = getProperty<float>("OKR").getValue();
    jointIKWeight = getProperty<float>("JointIK").getValue();
 
 
    kp = getProperty<float>("kp").getValue();
    ki = getProperty<float>("ki").getValue();
    kd = getProperty<float>("kd").getValue();
 
    vor = new VOR(5);
    vor->setJointNames(eye_pitch_left, eye_pitch_right, eye_yaw_left, eye_yaw_right, neck_roll);
    vor->setBools(armar4, velocityBased);
    vor->setPIDValues(kp, ki, kd);
 
    jointIK = new FeedforwardReflex(7);
    jointIK->setBools(armar4, velocityBased);
 
    okr = new OKR(10);
    okr->setJointNames(eye_pitch_left, eye_pitch_right, eye_yaw_left, eye_yaw_right, neck_roll);
    okr->setBools(armar4, velocityBased);
    okr->setPIDValues(kp, ki, kd);
 
 
    execCombineReflexesTask = new PeriodicTask<ReflexCombination>(
        this, &ReflexCombination::combineReflexes, 5, false, "ReflexCombinationTask");
    execCombineReflexesTask->setDelayWarningTolerance(2);
 
 
    // Filters initialization
 
    int sampling_size = 20;
    IMU_GyroFilters["X"] = new armarx::filters::MedianFilter(sampling_size);
    IMU_GyroFilters["X"]->update(0, new Variant((float)0.0));
    IMU_GyroFilters["Y"] = new armarx::filters::MedianFilter(sampling_size);
    IMU_GyroFilters["Y"]->update(0, new Variant((float)0.0));
    IMU_GyroFilters["Z"] = new armarx::filters::MedianFilter(sampling_size);
    IMU_GyroFilters["Z"]->update(0, new Variant((float)0.0));
 
    double frequency = 10.; // cut off frequency
    int sampleRate = 100;
    double resonance = 1.0; // should be between 0.1 and sqrt(2)
 
    std::map<std::string, float> filterProperties{{"minSampleTimeDelta", 100 * 1000.0}};
 
    for (auto& joint_name : headJointNames)
    {
 
        VelocityFilters[joint_name] = new armarx::filters::DerivationFilter();
        VelocityFilters[joint_name]->setProperties(filterProperties);
        VelocityFilters[joint_name]->update(0, new Variant((double)0.0));
 
        PreVelocityFilters[joint_name] =
            new armarx::filters::ButterworthFilter(frequency, sampleRate, Lowpass, resonance);
        PreVelocityFilters[joint_name]->setInitialValue(0.);
        PreVelocityFilters[joint_name]->update(0, new Variant((double)0.0));
    }
 
    frequency = 5.0;
    sampleRate = 100;
    resonance = 1.0;
    FlowFilters["X"] =
        new armarx::filters::ButterworthFilter(frequency, sampleRate, Lowpass, resonance);
    FlowFilters["X"]->setInitialValue(0.);
    FlowFilters["X"]->update(0, new Variant((float)0.0));
    FlowFilters["Y"] =
        new armarx::filters::ButterworthFilter(frequency, sampleRate, Lowpass, resonance);
    FlowFilters["Y"]->setInitialValue(0.);
    FlowFilters["Y"]->update(0, new Variant((float)0.0));
 
 
    t_init = armarx::TimeUtil::GetTime().toMilliSecondsDouble() / 1000.;
}
 
void
ReflexCombination::onConnectComponent()
{
    kinUnitPrx = getProxy<KinematicUnitInterfacePrx>(
        getProperty<std::string>("KinematicUnitName").getValue());
    robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(
        getProperty<std::string>("RobotStateName").getValue());
 
    debugObserver = getTopic<DebugObserverInterfacePrx>(
        getProperty<std::string>("DebugObserverName").getValue());
    imageSourceSelection = getProxy<ImageSourceSelectionInterfacePrx>(
        getProperty<std::string>("ImageSourceSelectionName").getValue());
 
    drawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
 
 
    std::string nodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
 
    jointIK->setRobot(nodeSetName, headIKName, robotStateComponent);
 
 
    FramedPosePtr dummy = new FramedPose();
 
    ARMARX_LOG << "Armar4: " << armar4;
 
    updateWeights(vorWeight, okrWeight, jointIKWeight);
 
    if ((combinationMethod == Reafference) && !jointIKEnabled)
    {
        ARMARX_WARNING << "Reafference combination cannot work without IK method enabled. Setting "
                          "to weighted sum imnstead";
        combinationMethod = WeightedSum;
    }
 
    execCombineReflexesTask->start();
}
 
void
ReflexCombination::onDisconnectComponent()
{
    execCombineReflexesTask->stop();
}
 
void
ReflexCombination::onExitComponent()
{
    delete vor;
    delete jointIK;
    delete okr;
}
 
armarx::PropertyDefinitionsPtr
ReflexCombination::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new ReflexCombinationPropertyDefinitions(getConfigIdentifier()));
}
 
void
ReflexCombination::updateWeights(float vorWeight,
                                 float okrWeight,
                                 float jointIKWeight,
                                 const Ice::Current& c)
{
 
 
    ARMARX_LOG << "new reflex weights vor:" << vorWeight << " okr:" << okrWeight
               << " jointik:" << jointIKWeight;
 
 
    StringVariantBaseMap debugValues;
    debugValues["reflexes_vor"] = new Variant(vorWeight);
    debugValues["reflexes_okr"] = new Variant(okrWeight);
    debugValues["reflexes_ik"] = new Variant(jointIKWeight);
 
    debugObserver->setDebugChannel("ReflexCombination", debugValues);
 
 
    std::scoped_lock lock(mutex);
 
    this->vorWeight = vorWeight;
    this->okrWeight = okrWeight;
    this->jointIKWeight = jointIKWeight;
 
    vorEnabled = (vorWeight > 0);
    jointIKEnabled = (jointIKWeight > 0);
    okrEnabled = (okrWeight > 0);
 
    ARMARX_LOG << "enabled reflexes vor:" << vorEnabled << " okr:" << okrEnabled
               << " jointik:" << jointIKEnabled;
 
    vor->setEnabled(vorEnabled);
    okr->setEnabled(okrEnabled);
    jointIK->setEnabled(jointIKEnabled);
 
    ARMARX_LOG << "reflexes enabled.";
}
 
void
ReflexCombination::setReafferenceMethod(bool isReafference, const Ice::Current& c)
{
    if (isReafference)
    {
        this->combinationMethod = Reafference;
    }
    else
    {
        this->combinationMethod = WeightedSum;
    }
}
 
void
ReflexCombination::combineReflexes()
{
    std::scoped_lock lock(mutex);
 
    if (!(vorEnabled || jointIKEnabled || okrEnabled))
    {
        ARMARX_LOG << deactivateSpam(1)
                   << "at least one reflex must be enabled to stabilize the gaze!";
        return;
    }
 
    if (newHeadTarget)
    {
        ARMARX_LOG << deactivateSpam(1) << "waiting for new head position";
        return;
    }
 
    //    lock.unlock();
 
    NameValueMap resultJointValues;
    NameControlModeMap controlModes;
 
    std::map<std::string, float> weights;
    std::map<std::string, float> jointValuesIK = jointIK->getJoints();
    std::map<std::string, float> jointValuesVOR = vor->getJoints();
    std::map<std::string, float> jointValuesOKR = okr->getJoints();
 
 
    // logging to debug observed via single channel using the map
    StringVariantBaseMap mapValues;
    mapValues["optFlow_predX"] = new Variant(jointIK->optFlow_pred[0]);
    mapValues["optFlow_predY"] = new Variant(jointIK->optFlow_pred[1]);
    mapValues["meanOptFlow_pred"] = new Variant((float)jointIK->mean_optFl_pred);
    mapValues["imu_gyro_predX"] = new Variant(jointIK->gyroscopeRotation_pred[0]);
    mapValues["imu_gyro_predY"] = new Variant(jointIK->gyroscopeRotation_pred[1]);
    mapValues["imu_gyro_predZ"] = new Variant(jointIK->gyroscopeRotation_pred[2]);
    mapValues["vel_filtered"] = new Variant(VelocityFilters[eye_yaw_left]->getValue()->getDouble());
    mapValues["flow_filteredX"] = new Variant(FlowFilters["X"]->getValue()->getDouble());
    mapValues["flow_filteredY"] = new Variant(FlowFilters["Y"]->getValue()->getDouble());
    mapValues["imu_filteredX"] = new Variant(IMU_GyroFilters["X"]->getValue()->getFloat());
    mapValues["imu_filteredY"] = new Variant(IMU_GyroFilters["Y"]->getValue()->getFloat());
    mapValues["imu_filteredZ"] = new Variant(IMU_GyroFilters["Z"]->getValue()->getFloat());
    debugObserver->setDebugChannel("ForwardPredictor", mapValues);
 
    /*
 
    for (auto & r : reflexes)
    {
    float w = r.second->getWeight();
 
    for (auto & kv : r.second->getJoints())
    {
    if (!resultJointValues.count(kv.first))
    {
    resultJointValues[kv.first] = 0;
    weights[kv.first] = 0;
    }
    resultJointValues[kv.first] += w * kv.second;
    weights[kv.first] += w;
    }
    }
 
    */
 
 
    // Add up references of each reflexes
 
    for (auto& kv : jointValuesIK)
    {
        if (!resultJointValues.count(kv.first))
        {
            resultJointValues[kv.first] = 0;
            weights[kv.first] = 0;
        }
        resultJointValues[kv.first] += jointIKWeight * kv.second;
        weights[kv.first] += jointIKWeight;
    }
 
    for (auto& kv : jointValuesVOR)
    {
        if (!resultJointValues.count(kv.first))
        {
            resultJointValues[kv.first] = 0;
            weights[kv.first] = 0;
        }
        resultJointValues[kv.first] += vorWeight * kv.second;
        weights[kv.first] += vorWeight;
    }
 
    for (auto& kv : jointValuesOKR)
    {
        if (!resultJointValues.count(kv.first))
        {
            resultJointValues[kv.first] = 0;
            weights[kv.first] = 0;
        }
        resultJointValues[kv.first] += okrWeight * kv.second;
        weights[kv.first] += okrWeight;
        //ARMARX_LOG << "okr vel " << kv.first << "= " <<  kv.second;
    }
 
 
    if (combinationMethod == WeightedSum)
    {
        // normalize joint ref
        for (auto& kv : weights)
        {
            resultJointValues[kv.first] = resultJointValues[kv.first] / kv.second;
        }
    }
 
    // send command to controller
    if (resultJointValues.empty())
    {
        return;
    }
 
 
    // set the unset velocities to 0
    if (velocityBased)
    {
        for (auto const& headJoint : headJointNames)
        {
            if (resultJointValues.find(headJoint) == resultJointValues.end())
            {
                resultJointValues[headJoint] = 0.;
            }
        }
    }
 
    // position tracking to keep eyes parrallel (necessary for stereo calibration)
    if (velocityBased)
    {
        try
        {
            double kp = 5.;
            resultJointValues["Eye_Left"] =
                resultJointValues["Eye_Right"] +
                kp * (PreVelocityFilters["Eye_Right"]->getValue()->getDouble() -
                      PreVelocityFilters["Eye_Left"]->getValue()->getDouble());
        }
        catch (...)
        {
        }
    }
 
    for (auto& kv : resultJointValues)
    {
        if (velocityBased)
        {
            controlModes[kv.first] = eVelocityControl;
        }
        else
        {
            controlModes[kv.first] = ePositionControl;
        }
    }
 
    if (neckPerturbation && velocityBased)
    {
        controlModes["Neck_3_Yaw"] = eVelocityControl;
        resultJointValues["Neck_3_Yaw"] =
            4. * cos((armarx::TimeUtil::GetTime().toMilliSecondsDouble() / 1000. - t_init));
    }
 
    try
    {
        //        ARMARX_LOG << deactivateSpam(1) << resultJointValues;
        //ARMARX_IMPORTANT_S << "joint vel reflex comb" << resultJointValues;
 
        StringVariantBaseMap debugValues;
 
        for (auto& kv : resultJointValues)
        {
            debugValues[kv.first] = new Variant(kv.second);
        }
 
        debugObserver->setDebugChannel("ReflexCombination", debugValues);
 
 
        StringVariantBaseMap debugValuesIK;
        for (auto& kv : jointValuesIK)
        {
            debugValuesIK[kv.first] = new Variant(kv.second);
        }
        debugObserver->setDebugChannel("ReflexCombinationIK", debugValuesIK);
 
        StringVariantBaseMap debugValuesOKR;
        for (auto& kv : jointValuesOKR)
        {
            debugValuesOKR[kv.first] = new Variant(kv.second);
        }
        debugObserver->setDebugChannel("ReflexCombinationOKR", debugValuesOKR);
 
        StringVariantBaseMap debugValuesVOR;
        for (auto& kv : jointValuesVOR)
        {
            debugValuesVOR[kv.first] = new Variant(kv.second);
        }
        debugObserver->setDebugChannel("ReflexCombinationVOR", debugValuesVOR);
 
        kinUnitPrx->switchControlMode(controlModes);
        if (velocityBased)
        {
            kinUnitPrx->setJointVelocities(resultJointValues);
        }
        else
        {
            kinUnitPrx->setJointAngles(resultJointValues);
        }
    }
    catch (...)
    {
        ARMARX_IMPORTANT << "Setting joint values failed!";
    }
}
 
void
ReflexCombination::reportPlatformVelocity(Ice::Float currentPlatformVelocityX,
                                          Ice::Float currentPlatformVelocityY,
                                          Ice::Float currentPlatformVelocityRotation,
                                          const Ice::Current& c)
{
    if (jointIKEnabled)
    {
        //jointIK->reportPlatformVelocity(currentPlatformVelocityX, currentPlatformVelocityY, currentPlatformVelocityRotation);
    }
}
 
void
armarx::ReflexCombination::reportControlModeChanged(const NameControlModeMap&,
                                                    Ice::Long timestamp,
                                                    bool,
                                                    const Ice::Current&)
{
}
 
void
armarx::ReflexCombination::reportJointAngles(const NameValueMap& values,
                                             Ice::Long timestamp,
                                             bool valueChanged,
                                             const Ice::Current& c)
{
    std::scoped_lock lock(mutex);
 
    NameValueMap tmp = values;
 
    if (newHeadTarget)
    {
        float offset = 0.1f;
        bool jointAnglesReached = true;
        for (std::pair<std::string, Ice::Float> joint : targetJointAngles)
        {
            jointAnglesReached &= (tmp[joint.first] >= (joint.second - offset));
            jointAnglesReached &= (tmp[joint.first] <= (joint.second + offset));
        }
 
        if (jointAnglesReached)
        {
            newHeadTarget = false;
 
            ARMARX_LOG << deactivateSpam(1) << "new head target reached";
 
            if (vorEnabled)
            {
                vor->start();
            }
 
 
            if (jointIKEnabled)
            {
                jointIK->start();
            }
            if (okrEnabled)
            {
                okr->start();
            }
        }
        else
        {
            return;
        }
    }
 
    if (vorEnabled)
    {
        vor->reportJointAngles(values, valueChanged, c);
    }
    if (jointIKEnabled)
    {
        jointIK->reportJointAngles(values, valueChanged, c);
    }
    if (okrEnabled)
    {
        okr->reportJointAngles(values, valueChanged, c);
    }
 
    // velocity estimation from position (velocity sensing not working in mca)
    for (auto& joint_name : headJointNames)
    {
        PreVelocityFilters[joint_name]->update(timestamp, new Variant(tmp[joint_name]));
        VelocityFilters[joint_name]->update(
            timestamp, new Variant(PreVelocityFilters[joint_name]->getValue()->getDouble()));
    }
}
 
void
armarx::ReflexCombination::reportJointVelocities(const NameValueMap& velocities,
                                                 Ice::Long timestamp,
                                                 bool valueChanged,
                                                 const Ice::Current& c)
{
 
    std::scoped_lock lock(mutex);
    NameValueMap velocities_filtered(velocities);
    for (auto& joint_name : headJointNames)
    {
        if (velocities_filtered.count(joint_name))
        {
            velocities_filtered[joint_name] = VelocityFilters[joint_name]->getValue()->getDouble();
        }
    }
 
 
    if (vorEnabled)
    {
        vor->reportJointVelocities(velocities_filtered, valueChanged, c);
    }
    if (jointIKEnabled)
    {
        jointIK->reportJointVelocities(velocities_filtered, valueChanged, c);
    }
    if (okrEnabled)
    {
        if (combinationMethod == Reafference && jointIKEnabled)
        {
            velocities_filtered[eye_yaw_left] =
                0.; // no need of removing self induced velocities as already removed by the prediction
            velocities_filtered[eye_yaw_right] = 0.;
            velocities_filtered[eye_pitch_left] = 0.;
        }
        okr->reportJointVelocities(velocities_filtered, valueChanged, timestamp, c);
    }
}
 
void
armarx::ReflexCombination::reportJointTorques(const NameValueMap&,
                                              Ice::Long timestamp,
                                              bool,
                                              const Ice::Current&)
{
}
 
void
armarx::ReflexCombination::reportJointAccelerations(const NameValueMap&,
                                                    Ice::Long timestamp,
                                                    bool,
                                                    const Ice::Current&)
{
}
 
void
armarx::ReflexCombination::reportJointCurrents(const NameValueMap&,
                                               Ice::Long timestamp,
                                               bool,
                                               const Ice::Current&)
{
}
 
void
armarx::ReflexCombination::reportJointMotorTemperatures(const NameValueMap&,
                                                        Ice::Long timestamp,
                                                        bool,
                                                        const Ice::Current&)
{
}
 
void
armarx::ReflexCombination::reportJointStatuses(const NameStatusMap&,
                                               Ice::Long timestamp,
                                               bool,
                                               const Ice::Current&)
{
}
 
void
armarx::ReflexCombination::reportNewOpticalFlow(Ice::Float x,
                                                Ice::Float y,
                                                Ice::Float deltaT,
                                                Ice::Long timestamp,
                                                const Ice::Current& c)
{
 
    std::scoped_lock lock(mutex);
 
    FlowFilters["X"]->update(timestamp, new Variant(x));
    FlowFilters["Y"]->update(timestamp, new Variant(y));
 
    double x_flow, y_flow;
 
    x_flow = FlowFilters["X"]->getValue()->getDouble();
    y_flow = FlowFilters["Y"]->getValue()->getDouble();
 
    // no filtering
    //x_flow = x;
    //y_flow = y;
 
    if (combinationMethod == Reafference && jointIKEnabled)
    {
        x_flow -= jointIK->optFlow_pred[0];
        y_flow -= jointIK->optFlow_pred[1];
 
        if (fabs(x_flow) < 0.04)
        {
            x_flow = 0.;
        }
    }
 
    if (okrEnabled)
    {
        okr->reportNewOpticalFlow(x_flow, y_flow, deltaT, timestamp);
    }
}
 
void
armarx::ReflexCombination::reportHeadTargetChanged(const NameValueMap& targetJointAngles,
                                                   const FramedPositionBasePtr& targetPosition,
                                                   const Ice::Current&)
{
 
    std::scoped_lock lock(mutex);
 
    newHeadTarget = true;
    this->targetJointAngles = targetJointAngles;
 
    ARMARX_INFO << " new head target " << targetPosition->output();
 
    if (jointIKEnabled)
    {
        jointIK->stop();
    }
    jointIK->reportHeadTargetChanged(targetJointAngles, targetPosition);
 
    if (vorEnabled)
    {
        vor->stop();
    }
    if (okrEnabled)
    {
        okr->stop();
    }
}
 
void
armarx::ReflexCombination::reportSensorValues(const std::string& device,
                                              const std::string& name,
                                              const IMUData& values,
                                              const TimestampBasePtr& timestamp,
                                              const Ice::Current& c)
{
 
 
    std::scoped_lock lock(mutex);
 
    /*
    Eigen::Quaternionf qTemp = Eigen::Quaternionf(values.orientationQuaternion[0], values.orientationQuaternion[1], values.orientationQuaternion[2], values.orientationQuaternion[3]);
    Eigen::Vector3f rpy = quaternionToRPY(qTemp);
    ARMARX_LOG << deactivateSpam(1) << "RPY: " << rpy[0] << "   " << rpy[1] << "   " << rpy[2];
    ARMARX_LOG << deactivateSpam(1) << "Quat: " << qTemp.w() << "   " << qTemp.x() << "   " << qTemp.y() << "   " << qTemp.z();
    */
    if (IMU_GyroFilters["X"] && IMU_GyroFilters["Y"] && IMU_GyroFilters["Z"])
    {
        IMU_GyroFilters["X"]->update(timestamp->timestamp,
                                     new Variant((float)values.gyroscopeRotation[0]));
        IMU_GyroFilters["Y"]->update(timestamp->timestamp,
                                     new Variant((float)values.gyroscopeRotation[1]));
        IMU_GyroFilters["Z"]->update(timestamp->timestamp,
                                     new Variant((float)values.gyroscopeRotation[2]));
    }
 
    IMUData values2 = values;
 
    if (IMU_GyroFilters["X"] && IMU_GyroFilters["Y"] && IMU_GyroFilters["Z"])
    {
        values2.gyroscopeRotation[0] = IMU_GyroFilters["X"]->getValue()->getFloat();
        values2.gyroscopeRotation[1] = IMU_GyroFilters["Y"]->getValue()->getFloat();
        values2.gyroscopeRotation[2] = IMU_GyroFilters["Z"]->getValue()->getFloat();
    }
 
    if (combinationMethod == Reafference && jointIKEnabled)
    {
        values2.gyroscopeRotation[0] -= jointIK->gyroscopeRotation_pred[0];
        values2.gyroscopeRotation[1] -= jointIK->gyroscopeRotation_pred[1];
        values2.gyroscopeRotation[2] -= jointIK->gyroscopeRotation_pred[2];
 
        if (fabs(values2.gyroscopeRotation[2]) < 0.04)
        {
            values2.gyroscopeRotation[2] = 0.;
        }
    }
 
    if (vorEnabled)
    {
        vor->reportSensorValues(values2);
    }
}
 
void
armarx::ReflexCombination::setImageQuality(float quality)
{
    Ice::StringSeq imageSources = getProperty<Ice::StringSeq>("ImageProviders");
    ARMARX_CHECK_GREATER_EQUAL(imageSources.size(), 2) << imageSources;
    if (quality > 0.7)
    {
        imageSourceSelection->setImageSource(imageSources[1], 100000000);
    }
    else
    {
        imageSourceSelection->setImageSource(imageSources[0], 100000000);
    }
}
 
void
armarx::ReflexCombination::reportNewTrackingError(Ice::Float pixelX,
                                                  Ice::Float pixelY,
                                                  Ice::Float angleX,
                                                  Ice::Float angleY,
                                                  const Ice::Current&)
{
    okr->reportNewTrackingError(pixelX, pixelY, angleX, angleY);
}
 
void
armarx::ReflexCombination::reportPlatformOdometryPose(Ice::Float,
                                                      Ice::Float,
                                                      Ice::Float,
                                                      const Ice::Current&)
{
}