KITProstheticHandUnit.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    RobotAPI::ArmarXObjects::KITProstheticHandUnit
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "KITProstheticHandUnit.h"
 
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <algorithm>
#include <thread>
#include <regex>
 
namespace armarx
{
    armarx::PropertyDefinitionsPtr KITProstheticHandUnit::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(new KITProstheticHandUnitPropertyDefinitions(
                getConfigIdentifier()));
    }
 
    void KITProstheticHandUnit::onInitHandUnit()
    {
        _driver = std::make_unique<BLEProthesisInterface>(getProperty<std::string>("MAC"));
        //addShapeName("Open"); //is added by something else already
        addShapeName("Close");
        addShapeName("G0");
        addShapeName("G1");
        addShapeName("G2");
        addShapeName("G3");
        addShapeName("G4");
        addShapeName("G5");
        addShapeName("G6");
        addShapeName("G7");
        addShapeName("G8");
 
        offeringTopic(getProperty<std::string>("DebugObserverName"));
        if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
        {
            usingProxy(getProperty<std::string>("RemoteGuiName"));
        }
    }
 
    void KITProstheticHandUnit::onStartHandUnit()
    {
        _debugObserver = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverName"));
        if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
        {
            _remoteGuiPrx = getProxy<RemoteGuiInterfacePrx>(getProperty<std::string>("RemoteGuiName").getValue());
 
 
            RemoteGui::detail::VBoxLayoutBuilder rootLayoutBuilder = RemoteGui::makeVBoxLayout();
 
            auto addFinger = [&](std::string name, float min, float max, float val, int steps)
            {
                rootLayoutBuilder.addChild(
                    RemoteGui::makeHBoxLayout()
                    .addChild(RemoteGui::makeTextLabel(name))
                    .addChild(RemoteGui::makeTextLabel("min " + std::to_string(min)))
                    .addChild(RemoteGui::makeFloatSlider(name).min(min).max(max).value(val).steps(steps))
                    .addChild(RemoteGui::makeTextLabel("max " + std::to_string(max)))
                );
                rootLayoutBuilder.addChild(
                    RemoteGui::makeHBoxLayout()
                    .addChild(RemoteGui::makeTextLabel(name + " Pos "))
                    .addChild(RemoteGui::makeLabel(name + "_pos").value("0"))
                    .addChild(new RemoteGui::HSpacer())
                );
                rootLayoutBuilder.addChild(
                    RemoteGui::makeHBoxLayout()
                    .addChild(RemoteGui::makeTextLabel(name + " PWM "))
                    .addChild(RemoteGui::makeLabel(name + "_pwm").value("0"))
                    .addChild(new RemoteGui::HSpacer())
                );
            };
 
            addFinger("Thumb", 0, 1, _lastGuiValueThumb, _driver->getMaxPosThumb());
            addFinger("Fingers", 0, 1, _lastGuiValueFingers, _driver->getMaxPosFingers());
 
            rootLayoutBuilder.addChild(new RemoteGui::VSpacer());
 
            _guiTask = new SimplePeriodicTask<>([&]()
            {
                _guiTab.receiveUpdates();
                _driver->getMaxPosThumb();
                const float t = _guiTab.getValue<float>("Thumb").get();
                const float f = _guiTab.getValue<float>("Fingers").get();
 
                bool updateT = t != _lastGuiValueThumb;
                bool updateF = f != _lastGuiValueFingers;
                _lastGuiValueThumb = t;
                _lastGuiValueFingers = f;
 
                if (updateT && updateF)
                {
                    setJointAngles({{"Thumb", t}, {"Fingers", f}});
                }
                else if (updateT)
                {
                    setJointAngles({{"Thumb", t}});
                }
                else if (updateF)
                {
                    setJointAngles({{"Fingers", f}});
                }
 
                _guiTab.getValue<std::string>("Thumb_pos").set(std::to_string(_driver->getThumbPos()));
                _guiTab.getValue<std::string>("Thumb_pwm").set(std::to_string(_driver->getThumbPWM()));
                _guiTab.getValue<std::string>("Fingers_pos").set(std::to_string(_driver->getFingerPos()));
                _guiTab.getValue<std::string>("Fingers_pwm").set(std::to_string(_driver->getFingerPWM()));
                _guiTab.sendUpdates();
            }, 10);
 
            RemoteGui::WidgetPtr rootLayout = rootLayoutBuilder;
 
            _remoteGuiPrx->createTab("KITProstheticHandUnit", rootLayout);
            _guiTab = RemoteGui::TabProxy(_remoteGuiPrx, "KITProstheticHandUnit");
 
            _guiTask->start();
        }
    }
 
    void KITProstheticHandUnit::onExitHandUnit()
    {
        _driver.reset();
    }
 
    void KITProstheticHandUnit::setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current&)
    {
        ARMARX_CHECK_NOT_NULL(_driver);
 
        for (const auto& pair : targetJointAngles)
        {
            if (pair.first == "Fingers")
            {
                const std::uint64_t pos = std::clamp(
                                              static_cast<std::uint64_t>(pair.second * _driver->getMaxPosFingers()),
                                              0ul, _driver->getMaxPosFingers());
                ARMARX_INFO << "set fingers " << pos;
                _driver->sendFingerPWM(200, 2999, pos);
                // fix until hw driver is fixed to handle multiple commands at the same time
                std::this_thread::sleep_for(std::chrono::milliseconds(100));
            }
            else if (pair.first == "Thumb")
            {
                const std::uint64_t pos = std::clamp(
                                              static_cast<std::uint64_t>(pair.second * _driver->getMaxPosThumb()),
                                              0ul, _driver->getMaxPosThumb());
                ARMARX_INFO << "set thumb " << pos;
                _driver->sendThumbPWM(200, 2999, pos);
                // fix until hw driver is fixed to handle multiple commands at the same time
                std::this_thread::sleep_for(std::chrono::milliseconds(100));
            }
            else
            {
                ARMARX_WARNING << "Invalid HandJointName '" << pair.first << "', ignoring.";
            }
        }
    }
 
    NameValueMap KITProstheticHandUnit::getCurrentJointValues(const Ice::Current&)
    {
        NameValueMap jointValues;
        jointValues["Fingers"] = _driver->getFingerPos() * 1.f / _driver->getMaxPosFingers();
        jointValues["Thumb"] = _driver->getThumbPos() * 1.f / _driver->getMaxPosThumb();
        return jointValues;
    }
 
    void KITProstheticHandUnit::addShape(const std::string& name, const std::map<std::string, float>& shape)
    {
        _shapes[name] = shape;
        addShapeName(name);
    }
 
    void KITProstheticHandUnit::addShapeName(const std::string& name)
    {
        Variant currentPreshape;
        currentPreshape.setString(name);
        shapeNames->addVariant(currentPreshape);
    }
 
    void KITProstheticHandUnit::setShape(const std::string& shapeName, const Ice::Current&)
    {
        if (std::regex_match(shapeName, std::regex{"[gG](0|[1-9][0-9]*)"}))
        {
            _driver->sendGrasp(std::stoul(shapeName.substr(1)));
        }
        else if (shapeName == "Open")
        {
            _driver->sendGrasp(0);
        }
        else if (shapeName == "Close")
        {
            _driver->sendGrasp(1);
        }
        else if (!_shapes.count(shapeName))
        {
            ARMARX_WARNING << "Unknown shape name '" << shapeName
                           << "'\nKnown shapes: " << _shapes;
        }
        else
        {
            setJointAngles(_shapes.at(shapeName));
        }
    }
}