feedforward.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 "feedforward.h"
 
using namespace armarx;
 
void
FeedforwardReflex::calc()
{
 
    if (!update_input_fromArmarX(stabilizer->input))
    {
        return;
    }
 
    reportedJointVelocitiesBool = false;
    reportedJointAnglesBool = false;
 
    stabilizer->control_loop();
 
    optFlow_pred = stabilizer->getOptFlowPred();
    mean_optFl_pred = stabilizer->getMeanOptFlPred();
    gyroscopeRotation_pred = stabilizer->getIMUPred();
 
    update_output_toArmarX(stabilizer->output);
}
 
void
FeedforwardReflex::setRobot(std::string nodeSetName,
                            std::string headIKName,
                            RobotStateComponentInterfacePrx robotStateComponent)
{
    std::scoped_lock lock(mutex);
 
    this->robotStateComponent = robotStateComponent;
 
    globalPos = new FramedPosition();
 
    GazeStabOptions* gs_options;
 
    if (armar4)
    {
        gs_options = new GazeStabOptions(1);
    }
    else // armar3
    {
        gs_options = new GazeStabOptions(2);
    }
 
    stabilizer = new GazeStabilization(gs_options);
    stabilizer->init();
 
    forward_predictor = new ForwardPredictor();
 
    startTime = armarx::TimeUtil::GetTime();
 
    if (armar4)
    {
        headJointNames = {"Neck_1_joint",
                          "Neck_2_joint",
                          "Neck_3_joint",
                          "Head_1_joint",
                          "Head_2_joint",
                          "EyeR_1_joint",
                          "EyeR_2_joint"};
    }
    else // armar3
    {
        headJointNames = {"Neck_1_Pitch", "Neck_2_Roll", "Neck_3_Yaw", "Cameras", "Eye_Right"};
    }
}
 
bool
FeedforwardReflex::update_input_fromArmarX(GazeStabInput* gs_input)
{
 
    std::scoped_lock lock(dataMutex);
 
    //Check if all sensor values have at least been reported once to avoid segfaults
    if (!(reportedJointAnglesBool && reportedJointVelocitiesBool)) // TODO:: && globalPos))
    {
        ARMARX_WARNING_S << "Try to access gaze stab input that haven't been reported";
        return false;
    }
 
    //    globalPos->changeFrame("Neck_1_joint");
 
    if (armar4)
    {
        PoseBasePtr globalPose = robotStateComponent->getSynchronizedRobot()
                                     ->getRobotNode("Torso_Yaw_joint")
                                     ->getGlobalPose();
 
        Eigen::Quaternionf quaternion =
            QuaternionPtr::dynamicCast(globalPose->orientation)->toEigenQuaternion();
        Eigen::Vector3f rpy = quaternionToRPY(quaternion);
 
 
        gs_input->q_LB[0] =
            globalPose->position->x; // should be probably converted from mm to m (TBC)
        gs_input->q_LB[1] = globalPose->position->y;
        gs_input->q_LB[2] = globalPose->position->z;
 
        gs_input->q_LB[3] = rpy(0);
        gs_input->q_LB[4] = rpy(1);
        gs_input->q_LB[5] = rpy(2);
 
        gs_input->q_LB[6] = this->reportedJointAngles["Torso_Yaw_joint"];
        gs_input->q_LB[7] = -1 * this->reportedJointAngles["Torso_Pitch_joint"];
 
 
        // in this test we assume we don´t know the floaring base velocity
        gs_input->qd_LB_des[0] = 0.;
        gs_input->qd_LB_des[1] = 0.;
        gs_input->qd_LB_des[2] = 0.;
        gs_input->qd_LB_des[3] = 0.;
        gs_input->qd_LB_des[4] = 0.;
        gs_input->qd_LB_des[5] = 0.;
 
        gs_input->qd_LB_des[6] = -1 * this->reportedJointVelocities["Torso_Yaw_joint"];
        gs_input->qd_LB_des[7] = this->reportedJointVelocities["Torso_Pitch_joint"];
    }
    else // armar3
    {
        PoseBasePtr globalPose = robotStateComponent->getSynchronizedRobot()->getGlobalPose();
 
        Eigen::Quaternionf quaternion =
            QuaternionPtr::dynamicCast(globalPose->orientation)->toEigenQuaternion();
        Eigen::Vector3f rpy = quaternionToRPY(quaternion);
 
        gs_input->q_LB[0] = globalPose->position->x / 1000.;
        gs_input->q_LB[1] = globalPose->position->y / 1000.;
        gs_input->q_LB[2] = globalPose->position->z / 1000.;
 
        gs_input->q_LB[3] = rpy(0);
        gs_input->q_LB[4] = rpy(1);
        gs_input->q_LB[5] = rpy(2);
 
        gs_input->q_LB[6] = 0.; // yaw platform (unused here...)
        gs_input->q_LB[7] = this->reportedJointAngles["Hip Pitch"];
        gs_input->q_LB[8] = this->reportedJointAngles["Hip Roll"];
        gs_input->q_LB[9] = this->reportedJointAngles["Hip Yaw"];
 
        // in this test we assume we don´t know the floating base velocity
        //gs_inputt->qd_LB_des[0] = 0.;  //x
        //gs_input->qd_LB_des[1] = 0.;  //y
        gs_input->qd_LB_des[2] = 0.;
        gs_input->qd_LB_des[3] = 0.;
        gs_input->qd_LB_des[4] = 0.;
        //gs_input->qd_LB_des[5] = 0.; // theta
 
        gs_input->qd_LB_des[6] = 0.; // yaw platform (unused here...)
        gs_input->qd_LB_des[7] = -1 * this->reportedJointVelocities["Hip Pitch"];
        gs_input->qd_LB_des[8] = -1 * this->reportedJointVelocities["Hip Roll"];
        gs_input->qd_LB_des[9] = this->reportedJointVelocities["Hip Yaw"];
 
        // for now we don't make any diff between desired and measured velocity (to be corrected).
        for (int i = 0; i < 10; i++)
        {
            gs_input->qd_LB[i] = gs_input->qd_LB_des[i];
        }
    }
 
    for (size_t i = 0; i < headJointNames.size(); i++)
    {
        gs_input->q_UB[i] = this->reportedJointAngles[headJointNames[i]];
        gs_input->qd_UB[i] = this->reportedJointVelocities[headJointNames[i]];
    }
 
    if (armar4)
    {
        gs_input->q_UB[2] += M_PI; // shift between models
        gs_input->q_UB[0] *= -1; // sense of rotation between models
        gs_input->q_UB[1] *= -1;
        gs_input->q_UB[6] *= -1;
    }
    else
    {
        //        gs_input->q_UB[4] *= -1;
    }
 
    gs_input->tsim = (armarx::TimeUtil::GetTime() - startTime).toSecondsDouble();
 
    return true;
}
 
void
FeedforwardReflex::update_output_toArmarX(GazeStabOutput* gs_output)
{
 
    std::map<std::string, float> targetJointAngles;
 
    for (size_t i = 0; i < headJointNames.size(); i++)
    {
        targetJointAngles[headJointNames[i]] = gs_output->qd_UB_des[i];
    }
    if (armar4)
    {
        targetJointAngles[headJointNames[0]] *= -1; // sense of rotation between models
        targetJointAngles[headJointNames[1]] *= -1;
        targetJointAngles[headJointNames[6]] *= -1;
    }
    else // armar 3
    {
        targetJointAngles["Eye_Left"] = targetJointAngles["Eye_Right"];
    }
 
    std::scoped_lock lock(mutex);
 
    jointAngles.swap(targetJointAngles);
}
 
void
FeedforwardReflex::setBools(bool armar4, bool velocityBased)
{
    std::scoped_lock lock(mutex);
 
    this->armar4 = armar4;
    this->velocityBased = velocityBased;
}
 
void
FeedforwardReflex::reportJointAngles(const NameValueMap& values,
                                     bool valueChanged,
                                     const Ice::Current& c)
{
    std::scoped_lock lock(dataMutex);
 
    reportedJointAnglesBool = true;
    this->reportedJointAngles = values;
}
 
void
FeedforwardReflex::reportJointVelocities(const NameValueMap& values,
                                         bool valueChanged,
                                         const Ice::Current& c)
{
    std::scoped_lock lock(dataMutex);
 
    reportedJointVelocitiesBool = true;
    this->reportedJointVelocities = values;
}
 
void
FeedforwardReflex::reportPlatformVelocity(float x, float y, float a)
{
    stabilizer->input->qd_LB_des[0] = x / 1000.;
    stabilizer->input->qd_LB_des[1] = y / 1000.;
    stabilizer->input->qd_LB_des[5] = a;
}
 
void
FeedforwardReflex::reportHeadTargetChanged(const NameValueMap& targetJointAngles,
                                           const FramedPositionBasePtr& targetPosition)
{
    std::scoped_lock lock(dataMutex);
 
    globalPos = FramedPositionPtr::dynamicCast(targetPosition);
 
    Eigen::Vector3f x = globalPos->toEigen() / 1000.0f;
 
    stabilizer->input->pos_target = x.cast<double>();
}
 
void
FeedforwardReflex::onStop()
{
    std::scoped_lock lock(dataMutex);
 
    // globalPos = {};
    reportedJointVelocitiesBool = false;
    reportedJointAnglesBool = false;
}