HeadIKUnit.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 "HeadIKUnit.h"
 
#include <memory>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/IK/GazeIK.h>
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
namespace armarx
{
 
    HeadIKUnit::HeadIKUnit() : requested(false), cycleTime(30), newTargetSet(false)
    {
        targetPosition = new FramedPosition();
    }
 
    void
    HeadIKUnit::onInitComponent()
    {
        std::unique_lock lock(accessMutex);
 
        usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
        usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
 
 
        cycleTime = getProperty<int>("CycleTime").getValue();
        offeringTopic("DebugDrawerUpdates");
        offeringTopic(getProperty<std::string>("HeadIKUnitTopicName").getValue());
    }
 
    void
    HeadIKUnit::onConnectComponent()
    {
        std::unique_lock lock(accessMutex);
 
        drawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
 
        kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(
            getProperty<std::string>("KinematicUnitName").getValue());
        robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(
            getProperty<std::string>("RobotStateComponentName").getValue());
 
 
        //remoteRobotPrx = robotStateComponentPrx->getSynchronizedRobot();
        localRobot = RemoteRobot::createLocalCloneFromFile(
            robotStateComponentPrx, VirtualRobot::RobotIO::RobotDescription::eStructure);
        //        localRobot = RemoteRobot::createLocalClone(robotStateComponentPrx);
 
 
        headIKUnitListener = getTopic<armarx::HeadIKUnitListenerPrx>(
            getProperty<std::string>("HeadIKUnitTopicName").getValue());
 
        //std::string robotModelFile;
        //ArmarXDataPath::getAbsolutePath("Armar3/robotmodel/ArmarIII.xml", robotModelFile);
        //localRobot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str(), VirtualRobot::RobotIO::eStructure);
        //VirtualRobot::RobotPtr robotSnapshot(new RemoteRobot(remoteRobotPrx));
        //localRobot->setConfig(robotSnapshot->getConfig());
 
        requested = false;
 
        if (execTask)
        {
            execTask->stop();
        }
 
        execTask = new PeriodicTask<HeadIKUnit>(
            this, &HeadIKUnit::periodicExec, cycleTime, false, "HeadIKCalculator");
        execTask->setDelayWarningTolerance(300);
        execTask->start();
    }
 
    void
    HeadIKUnit::onDisconnectComponent()
    {
        release();
 
        //std::unique_lock lock(accessMutex);
        if (drawer)
        {
            drawer->removeSphereDebugLayerVisu("HeadViewTarget");
            drawer->removeSphereDebugLayerVisu("HeadViewTargetSolution");
        }
 
 
        if (execTask)
        {
            execTask->stop();
        }
    }
 
    void
    HeadIKUnit::onExitComponent()
    {
    }
 
    void
    HeadIKUnit::setCycleTime(Ice::Int milliseconds, const Ice::Current& c)
    {
        std::unique_lock lock(accessMutex);
 
        cycleTime = milliseconds;
 
        if (execTask)
        {
            execTask->changeInterval(cycleTime);
        }
    }
 
    void
    HeadIKUnit::setHeadTarget(const std::string& robotNodeSetName,
                              const FramedPositionBasePtr& targetPosition,
                              const Ice::Current& c)
    {
        std::unique_lock lock(accessMutex);
 
        this->robotNodeSetNames = armarx::Split(robotNodeSetName, ",");
        for (auto& setName : robotNodeSetNames)
        {
            simox::alg::trim(setName);
        }
        this->targetPosition->x = targetPosition->x;
        this->targetPosition->y = targetPosition->y;
        this->targetPosition->z = targetPosition->z;
        this->targetPosition->frame = targetPosition->frame;
 
        FramedPositionPtr globalTarget =
            FramedPositionPtr::dynamicCast(targetPosition)
                ->toGlobal(robotStateComponentPrx->getSynchronizedRobot());
 
        if (drawer && getProperty<bool>("VisualizeIKTarget").getValue())
        {
            drawer->setSphereDebugLayerVisu(
                "HeadViewTarget", globalTarget, DrawColor{1, 0, 0, 0.7}, 15);
        }
 
        ARMARX_DEBUG << "new Head target set: " << *globalTarget << " for " << robotNodeSetName;
 
        newTargetSet = true;
    }
 
    void
    HeadIKUnit::init(const Ice::Current& c)
    {
    }
 
    void
    HeadIKUnit::start(const Ice::Current& c)
    {
    }
 
    void
    HeadIKUnit::stop(const Ice::Current& c)
    {
    }
 
    UnitExecutionState
    HeadIKUnit::getExecutionState(const Ice::Current& c)
    {
        switch (getState())
        {
            case eManagedIceObjectStarted:
                return eUnitStarted;
 
            case eManagedIceObjectInitialized:
            case eManagedIceObjectStarting:
                return eUnitInitialized;
 
            case eManagedIceObjectExiting:
            case eManagedIceObjectExited:
                return eUnitStopped;
 
            default:
                return eUnitConstructed;
        }
    }
 
    void
    HeadIKUnit::request(const Ice::Current& c)
    {
        std::unique_lock lock(accessMutex);
 
        requested = true;
        ARMARX_IMPORTANT << "Requesting HeadIKUnit";
 
        if (execTask)
        {
            execTask->stop();
        }
 
        execTask = new PeriodicTask<HeadIKUnit>(
            this, &HeadIKUnit::periodicExec, cycleTime, false, "TCPVelocityCalculator");
        execTask->start();
        ARMARX_IMPORTANT << "Requested HeadIKUnit";
    }
 
    void
    HeadIKUnit::release(const Ice::Current& c)
    {
        std::unique_lock lock(accessMutex);
 
        ARMARX_INFO << "Releasing HeadIKUnit";
        requested = false;
 
        if (drawer)
        {
            drawer->removeSphereDebugLayerVisu("HeadViewTarget");
            drawer->removeSphereDebugLayerVisu("HeadViewTargetSolution");
        }
 
        // why do we stop the kin unit?
        /*try
        {
            if (kinematicUnitPrx)
                kinematicUnitPrx->stop();
        } catch (...)
        {
            ARMARX_IMPORTANT << "Released HeadIKUnit failed";
        }*/
 
        ARMARX_INFO << "Released HeadIKUnit";
    }
 
    void
    HeadIKUnit::periodicExec()
    {
        bool doCalculation = false;
 
        {
            std::unique_lock lock(accessMutex, std::defer_lock);
 
            if (lock.try_lock())
            {
                doCalculation = requested && newTargetSet;
                newTargetSet = false;
            }
            else
            {
                return;
            }
        }
 
 
        if (doCalculation)
        {
            std::unique_lock lock(accessMutex);
 
            VirtualRobot::RobotNodeSetPtr kinematicChain;
            bool foundSolution = false;
            NameValueMap targetJointAngles;
            NameControlModeMap controlModes;
            std::set<std::string> possiblyInvolvedJointNames;
            //            // set all involved joints initially to zero
            for (auto robotNodeSetName : robotNodeSetNames)
            {
                kinematicChain = localRobot->getRobotNodeSet(robotNodeSetName);
                for (unsigned int i = 0; i < kinematicChain->getSize(); i++)
                {
                    possiblyInvolvedJointNames.insert(kinematicChain->getNode(i)->getName());
                    //                                kinematicChain->getNode(i)->setJointValue(0.0f);
                    //                                targetJointAngles[kinematicChain->getNode(i)->getName()] = 0.0f;
                    //                                controlModes[kinematicChain->getNode(i)->getName()] = ePositionControl;
                }
            }
            FramedPositionPtr globalPos;
            float error = -1;
            //VirtualRobot::RobotPtr robotSnapshot(new RemoteRobot(remoteRobotPrx));
            //localRobot->setConfig(robotSnapshot->getConfig());
            std::string selectedRobotNodeSetName;
            for (auto robotNodeSetName : robotNodeSetNames)
            {
                RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponentPrx);
 
                kinematicChain = localRobot->getRobotNodeSet(robotNodeSetName);
                auto tcpNode = kinematicChain->getTCP();
                VirtualRobot::RobotNodePrismaticPtr virtualPrismaticJoint;
 
                virtualPrismaticJoint =
                    std::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(tcpNode);
                if (!virtualPrismaticJoint)
                {
                    ARMARX_WARNING << deactivateSpam(10, robotNodeSetName)
                                   << "Head IK Kinematic Chain TCP is not a prismatic joint! "
                                      "skipping joint set";
                    continue;
                }
 
                // set other not-used joints to 0
                for (auto& nodeName : possiblyInvolvedJointNames)
                {
                    if (!kinematicChain->hasRobotNode(nodeName))
                    {
                        localRobot->getRobotNode(nodeName)->setJointValue(0.0f);
                        targetJointAngles[nodeName] = 0.0f;
                        controlModes[nodeName] = ePositionControl;
                    }
                }
 
                ARMARX_DEBUG << deactivateSpam(5) << VAROUT(virtualPrismaticJoint->getName()) << " "
                             << VAROUT(kinematicChain->getName());
                VirtualRobot::GazeIK ikSolver(kinematicChain, virtualPrismaticJoint);
                ikSolver.enableJointLimitAvoidance(true);
                ikSolver.setup(10, 30, 20);
                //ikSolver.setVerbose(true);
 
                globalPos = targetPosition->toGlobal(localRobot);
                ARMARX_DEBUG << "Calculating IK for target position " << globalPos->output();
                auto start = IceUtil::Time::now();
                bool solutionFound = ikSolver.solve(globalPos->toEigen());
                auto duration = (IceUtil::Time::now() - start);
 
                if (duration.toMilliSeconds() > 500)
                {
                    ARMARX_INFO << "Calculating Gaze IK took " << duration.toMilliSeconds()
                                << " ms";
                }
                Eigen::Vector3f position =
                    globalPos->toEigen() -
                    kinematicChain->getTCP()->getGlobalPose().block(0, 3, 3, 1);
                error = position.norm();
                if (!solutionFound)
                {
 
                    if (error < 150)
                    {
                        foundSolution = true;
                        selectedRobotNodeSetName = robotNodeSetName;
                        ARMARX_INFO << "IKSolver found no solution! applying best solution with "
                                    << error << "mm error";
                        break;
                    }
                }
                else
                {
                    foundSolution = true;
                    selectedRobotNodeSetName = robotNodeSetName;
                    break;
                }
            }
            if (!foundSolution)
            {
                ARMARX_WARNING << "IKSolver found no solution!  " << error << "mm error";
                return;
            }
            ARMARX_DEBUG << "Solution found with " << selectedRobotNodeSetName
                         << " - remaining error: " << error << " mm";
 
            if (drawer && localRobot->hasRobotNode("Cameras") &&
                getProperty<bool>("VisualizeIKTarget").getValue())
            {
                Vector3Ptr startPos =
                    new Vector3(localRobot->getRobotNode("Cameras")->getGlobalPose());
                drawer->setSphereDebugLayerVisu(
                    "HeadViewTargetSolution", startPos, DrawColor{0, 1, 1, 0.2}, 17);
            }
            auto tcpNode = kinematicChain->getTCP();
            for (int i = 0; i < (signed int)kinematicChain->getSize(); i++)
            {
                if (kinematicChain->getNode(i) != tcpNode)
                {
                    targetJointAngles[kinematicChain->getNode(i)->getName()] =
                        kinematicChain->getNode(i)->getJointValue();
                    controlModes[kinematicChain->getNode(i)->getName()] = ePositionControl;
                }
 
                ARMARX_DEBUG << kinematicChain->getNode(i)->getName() << ": "
                             << kinematicChain->getNode(i)->getJointValue();
            }
 
 
            if (headIKUnitListener)
            {
                headIKUnitListener->reportHeadTargetChanged(targetJointAngles, globalPos);
            }
 
            try
            {
                kinematicUnitPrx->switchControlMode(controlModes);
                ARMARX_DEBUG << "setting new joint angles" << targetJointAngles;
                kinematicUnitPrx->setJointAngles(targetJointAngles);
            }
            catch (...)
            {
                ARMARX_IMPORTANT << "setJointAngles failed";
            }
        }
    }
 
    PropertyDefinitionsPtr
    HeadIKUnit::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(new HeadIKUnitPropertyDefinitions(getConfigIdentifier()));
    }
 
 
} // namespace armarx