jointik.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 "jointik.h"
 
#include <memory>
 
using namespace armarx;
 
void JointIK::calc()
{
 
    std::scoped_lock lock(dataMutex);
 
    //Check if all sensor values have at least been reported once to avoid segfaults
    if (!(reportedJointAnglesBool && reportedJointVelocitiesBool && globalPos))
    {
        return;
    }
 
 
    RemoteRobot::synchronizeLocalClone(robot, robotStateComponent);
 
    NameValueMap newValues;
    for (size_t i = 0; i < allRobotNodeSet.size(); i++)
    {
        std::string name = allRobotNodeSet[i]->getName();
        //Sort non movable joints out
        if (name == "VirtualCentralGaze" || name == "Hand Palm 1 L" || name == "Hand Palm 1 R")
        {
            continue;
        }
        newValues[name] = reportedJointAngles[name] + (((float) interval) / 1000.0) * reportedJointVelocities[name];
    }
 
    robot->setJointValues(newValues);
 
    //HeadIK part
    VirtualRobot::RobotNodeSetPtr kinematicChain = robot->getRobotNodeSet(headIKName);
 
    //ARMARX_LOG << "kinematic chain name" << kinematicChain->getName();
    VirtualRobot::RobotNodePrismaticPtr virtualJoint;
 
    for (size_t i = 0; i < kinematicChain->getSize(); i++)
    {
        //TODO VirtualCentralGaze in Armar4 nicht vorhanden
        if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") == 0)
        {
            virtualJoint = std::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(kinematicChain->getNode(i));
        }
    }
 
    VirtualRobot::GazeIK ikSolver(kinematicChain, virtualJoint);
    ikSolver.enableJointLimitAvoidance(true);
    //ikSolver.setup(10, 30, 20);
 
    Eigen::Vector3f targetPosition = globalPos->toEigen();
    bool solutionFound = ikSolver.solve(targetPosition);
 
    if (solutionFound)
    {
        NameValueMap targetJointAngles;
        for (size_t i = 0; i < kinematicChain->getSize(); i++)
        {
            std::string jointName = kinematicChain->getNode(i)->getName();
            if (jointName.compare("VirtualCentralGaze") != 0)
            {
                targetJointAngles[jointName] = kinematicChain->getNode(i)->getJointValue();
 
            }
        }
 
        std::scoped_lock lock(mutex);
 
        jointAngles.swap(targetJointAngles);
    }
    else
    {
        ARMARX_WARNING_S   << "HeadIK no solution found";
    }
 
 
}
 
void JointIK::setRobot(std::string nodeSetName, std::string headIKName, RobotStateComponentInterfacePrx robotStateComponent)
{
    std::scoped_lock lock(mutex);
 
    this->robot =  RemoteRobot::createLocalCloneFromFile(robotStateComponent, VirtualRobot::RobotIO::RobotDescription::eFull);
 
    VirtualRobot::RobotNodeSetPtr nodeSetPtr = this->robot->getRobotNodeSet(nodeSetName);
    this->allRobotNodeSet = nodeSetPtr->getAllRobotNodes();
    this->headIKName = headIKName;
    this->robotStateComponent = robotStateComponent;
}
 
void JointIK::setBools(bool armar4, bool velocityBased)
{
    std::scoped_lock lock(mutex);
 
    this->armar4 = armar4;
    this->velocityBased = velocityBased;
}
 
void JointIK::reportJointAngles(const NameValueMap& values, bool valueChanged, const Ice::Current& c)
{
    std::scoped_lock lock(dataMutex);
 
    reportedJointAnglesBool = true;
    this->reportedJointAngles = values;
 
}
 
void JointIK::reportJointVelocities(const NameValueMap& values, bool valueChanged, const Ice::Current& c)
{
    std::scoped_lock lock(dataMutex);
 
    reportedJointVelocitiesBool = true;
    this->reportedJointVelocities = values;
}
 
 
 
void JointIK::reportHeadTargetChanged(const NameValueMap& targetJointAngles, const FramedPositionBasePtr& targetPosition)
{
    std::scoped_lock lock(dataMutex);
 
    globalPos = FramedPositionPtr::dynamicCast(targetPosition);
}
 
void JointIK::onStop()
{
    std::scoped_lock lock(dataMutex);
 
    // globalPos = {};
    reportedJointVelocitiesBool = false;
    reportedJointAnglesBool = false;
 
 
}