CyberGloveProsthesisControl.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::ProsthesisKinestheticTeachIn
 * @author     Julia Starke ( julia dot starke at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CyberGloveProsthesisControl.h"
 
#include <RobotAPI/components/KITHandUnit/KITHandUnit.h>
 
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
 
#include <VirtualRobot/math/Helpers.h>
#include <RobotAPI/components/DebugDrawer/DebugDrawerHelper.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
 
#include <chrono>
#include <iostream>
#include <fstream>
#include <filesystem>
 
using namespace armarx;
using namespace ProsthesisKinestheticTeachIn;
 
// DO NOT EDIT NEXT LINE
CyberGloveProsthesisControl::SubClassRegistry CyberGloveProsthesisControl::Registry(CyberGloveProsthesisControl::GetName(), &CyberGloveProsthesisControl::CreateInstance);
 
 
 
void CyberGloveProsthesisControl::onEnter()
{
    // put your user code for the enter-point here
    // execution time should be short (<100ms)
    ARMARX_IMPORTANT << "onEnter";
}
 
void CyberGloveProsthesisControl::shapeHand(float fingers, float thumb)
{
    getHandUnit()->setJointAngles({{"Fingers", fingers}, {"Thumb", thumb}});
}
 
// possibly add debug observer code here
//class ProsthesisPositionHelper;
//typedef boost::shared_ptr<ProsthesisPositionHelper> ProsthesisPositionHelperPtr;
//class ProsthesisPositionHelper
//{
//public:
 
//    ProsthesisPositionHelper(ObserverInterfacePrx robotUnitObserver)
//        : robotUnitObserver(robotUnitObserver)
//    {
//        thumbDF = DatafieldRefPtr::dynamicCast(robotUnitObserver->getDatafieldRefByName("SensorDevices", "RightHandThumb_position"));
//        fingersDF = DatafieldRefPtr::dynamicCast(robotUnitObserver->getDatafieldRefByName("SensorDevices", "RightHandFingers_position"));
//    }
 
//    ~ProsthesisPositionHelper()
//    {
//    }
 
//    float getThumbPosition()
//    {
//        return thumbDF->getDataField()->getFloat();
//    }
 
//    float getFingersPosition()
//    {
//        return fingersDF->getDataField()->getFloat();
//    }
 
//    ObserverInterfacePrx robotUnitObserver;
//    DatafieldRefPtr thumbDF;
//    DatafieldRefPtr fingersDF;
//};
 
 
void CyberGloveProsthesisControl::run()
{
    // put your user code for the execution-phase here
    // runs in seperate thread, thus can do complex operations
    // should check constantly whether isRunningTaskStopped() returns true
 
    // get a private kinematic instance for this state of the robot (tick "Robot State Component" proxy checkbox in statechart group)
    //    VirtualRobot::RobotPtr robot = getLocalRobot();
 
    //    DebugDrawerHelper debugDrawerHelper(getDebugDrawerTopic(), "RemoteTeachIn", robot);
    //    ProsthesisPositionHelperPtr prosthesisPositionHelper;
 
    RemoteGui::detail::VBoxLayoutBuilder rootLayoutBuilder = RemoteGui::makeVBoxLayout();
    rootLayoutBuilder
    .addChild(RemoteGui::makeLabel("message"))
    .addChild(RemoteGui::makeLabel("status").value("status"))
    .addChild(RemoteGui::makeLabel("instruction"))
    .addChild(RemoteGui::makeHBoxLayout()
              .addChild(RemoteGui::makeButton("calibrate_cyber_glove").label("Calibrate Cyber Glove"))
              .addChild(RemoteGui::makeButton("start_recording").label("Start Recording"))
              .addChild(RemoteGui::makeButton("stop_recording").label("Stop Recording"))
              .addChild(RemoteGui::makeButton("set_latest_timestamp").label("Set Latest Timestamp")))
    .addChild(RemoteGui::makeLineEdit("file_name").value("Set file name"))
    //.addChild(RemoteGui::makeLabel("longMessage").value("test"))
    .addChild(new RemoteGui::VSpacer());
 
    getRemoteGui()->createTab("CyberGloveProsthesisControl", rootLayoutBuilder);
    RemoteGui::TabProxy guiTab = RemoteGui::TabProxy(getRemoteGui(), "CyberGloveProsthesisControl");
 
    //std::string packageName = "ProsthesisKinestheticTeachIn";
    //armarx::CMakePackageFinder finder(packageName);
    //std::string dataDir = finder.getDataDir() + "/" + packageName + "/recordings/";
    std::filesystem::path myPath = std::filesystem::current_path();
    std::string dataDir = myPath.string() + "/prosthesisKinestheticTeaching/recordings/";
    ARMARX_IMPORTANT << "Data recording directory: " << dataDir;
    //SimpleJsonLoggerPtr logger;
    std::ofstream logger;
    std::string timestamp = "";
    std::string latest_timestamp = "";
 
    PhaseType currentPhase = PhaseType::CalibOpen;
 
    int lastClickCountCalibrateCyberGlove = guiTab.getValue<int>("calibrate_cyber_glove").get();
    int lastClickCountRecordOn = guiTab.getValue<int>("start_recording").get();
    int lastClickCountRecordOff = guiTab.getValue<int>("stop_recording").get();
    int lastClickCountLatestTimestamp = guiTab.getValue<int>("set_latest_timestamp").get();
 
    bool record = false;
 
    CyberGloveValues openValues;
    CyberGloveValues closedValues;
    bool storeCalibrationValuesForCyberGlove = false;
 
    std::chrono::time_point<std::chrono::system_clock> start, end;
 
 
    // uncomment this if you need a continous run function. Make sure to use sleep or use blocking wait to reduce cpu load.
    while (!isRunningTaskStopped()) // stop run function if returning true
    {
        // do your calculations
        // synchronize robot clone to most recent state
        //RemoteRobot::synchronizeLocalClone(robot, getRobotStateComponent());
        PhaseType nextPhase = currentPhase;
 
        guiTab.receiveUpdates();
        int calibrateCyberGloveClickCount = guiTab.getValue<int>("calibrate_cyber_glove").get();
        bool calibrateCyberGloveButtonWasClicked = calibrateCyberGloveClickCount > lastClickCountCalibrateCyberGlove;
        lastClickCountCalibrateCyberGlove = calibrateCyberGloveClickCount;
 
        int startRecordingClickCount = guiTab.getValue<int>("start_recording").get();
        bool startRecordingButtonClicked = startRecordingClickCount > lastClickCountRecordOn;
        lastClickCountRecordOn = startRecordingClickCount;
 
        int stopRecordingClickCount = guiTab.getValue<int>("stop_recording").get();
        bool stopRecordingButtonClicked = stopRecordingClickCount > lastClickCountRecordOff;
        lastClickCountRecordOff = stopRecordingClickCount;
 
        int setLatestTimestampClickCount = guiTab.getValue<int>("set_latest_timestamp").get();
        bool setLatestTimeStampButtonClicked = setLatestTimestampClickCount > lastClickCountLatestTimestamp;
        lastClickCountLatestTimestamp = setLatestTimestampClickCount;
        ARMARX_IMPORTANT << "Timestamp click count: " << setLatestTimestampClickCount;
 
        CyberGloveValues currentValues = getCyberGloveObserver()->getLatestValues();
 
        float thumbValue = 0;
        float indexValue = 0;
 
        auto calcThumbSum = [](const CyberGloveValues & v)
        {
            return v.thumbAbd + v.thumbCMC + v.thumbIP + v.thumbMCP;
        };
        auto calcIndexSum = [](const CyberGloveValues & v)
        {
            return v.indexDIP + v.indexMCP + v.indexPIP;
        };
 
        if (setLatestTimeStampButtonClicked)
        {
            if (!latest_timestamp.empty())
            {
                timestamp = latest_timestamp;
                //guiTab.getValue<std::string>("longMessage").set("Resume latest timestamp. Latest timestamp is " + latest_timestamp);
                getMessageDisplay()->setMessage("Resume latest timestamp", "Latest timestamp is " + latest_timestamp);
            }
            else
            {
                //guiTab.getValue<std::string>("longMessage").set("No timestamp stored.");
                getMessageDisplay()->setMessage("No timestamp stored.", "Take a point cloud to generate a timestamp.");
            }
        }
 
        if (startRecordingButtonClicked)
        {
            if (timestamp.empty())
            {
                IceUtil::Time now = IceUtil::Time::now();
                time_t timer = now.toSeconds();
                struct tm* ts;
                char buffer[80];
                ts = localtime(&timer);
                strftime(buffer, 80, "%Y-%m-%d-%H-%M-%S", ts);
                std::string str(buffer);
                timestamp = str;
                latest_timestamp = timestamp;
            }
            std::string fileName = guiTab.getValue<std::string>("file_name").get();
            //logger.reset(new SimpleJsonLogger(dataDir + fileName + "-temp-" + timestamp + ".json", false));
            logger.open(dataDir + fileName + "-temp-" + timestamp + ".csv");
            start = std::chrono::system_clock::now();
            logger << "Timestamp, indexTarget, thumbTarget, indexActual, thumbActual" << std::endl;
            record = true;
            storeCalibrationValuesForCyberGlove = true;
            getMessageDisplay()->setMessage("Start Recording Trajectory", "");
        }
 
        if (stopRecordingButtonClicked)
        {
            timestamp = "";
            //logger->close();
            logger.close();
            record = false;
 
            getMessageDisplay()->setMessage("Stop Recording Trajectory", "");
        }
 
        if (currentPhase == PhaseType::CalibOpen)
        {
            getMessageDisplay()->setMessage("Calibrating open hand", "");
            guiTab.getValue<std::string>("message").set("Put your hand flat on the table!");
            if (calibrateCyberGloveButtonWasClicked)
            {
                openValues = currentValues;
                nextPhase = PhaseType::CalibClose;
            }
        }
        if (currentPhase == PhaseType::CalibClose)
        {
            getMessageDisplay()->setMessage("Calibrating closed hand", "");
            guiTab.getValue<std::string>("message").set("Make a fist!");
            if (calibrateCyberGloveButtonWasClicked)
            {
                closedValues = currentValues;
                nextPhase = PhaseType::TeachIn;
            }
        }
        if (currentPhase == PhaseType::TeachIn)
        {
            guiTab.getValue<std::string>("message").set("Teach-In!");
            thumbValue = math::Helpers::Clamp(0, 1, math::Helpers::ILerp(calcThumbSum(openValues), calcThumbSum(closedValues), calcThumbSum(currentValues)));
            indexValue = math::Helpers::Clamp(0, 1, math::Helpers::ILerp(calcIndexSum(openValues), calcIndexSum(closedValues), calcIndexSum(currentValues)));
 
            if (!std::isfinite(thumbValue))
            {
                thumbValue = 0;
            }
            if (!std::isfinite(indexValue))
            {
                indexValue = 0;
            }
 
            //SimpleJsonLoggerEntry e;
            shapeHand(indexValue, thumbValue);
 
            //            e.AddTimestamp();
            NameValueMap handMotorValues = getHandUnit()->getCurrentJointValues();
            //            e.Add("indexValueTarget", indexValue);
            //            e.Add("thumbValueTarget", thumbValue);
            //            e.Add("indexValueActual", handMotorValues["Fingers"]);
            //            e.Add("thumbValueActual", handMotorValues["Thumb"]);
 
            if (record)
            {
 
                end = std::chrono::system_clock::now();
                double elapsed_milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>
                                              (end - start).count();
                logger << elapsed_milliseconds << "," << indexValue << "," << thumbValue << "," << handMotorValues["Fingers"] << "," << handMotorValues["Thumb"] << std::endl;
 
                //                if (storeCalibrationValuesForCyberGlove)
                //                {
                //                    ARMARX_IMPORTANT << "Recording raw joint values for calibration.";
                //                    e = getRawJointValuesForCalibration(openValues, closedValues);
                //                    storeCalibrationValuesForCyberGlove = false;
                //                }
                //                else
                //                {
                //                    addCurrentRawJointValues(currentValues, e);
                //                }
                //                logger->log(e);
            }
        }
 
        {
            std::stringstream ss;
            ss << "index: " << currentValues.indexMCP << " " << currentValues.indexPIP << " " << currentValues.indexDIP << "\n";
            ss << "thumb: " << currentValues.thumbMCP << " " << currentValues.thumbIP << " " << currentValues.thumbCMC << " " << currentValues.thumbAbd << "\n";
            ss << std::floor(indexValue * 100) << "% " << std::floor(thumbValue * 100) << "%\n";
            guiTab.getValue<std::string>("status").set(ss.str());
        }
        //ARMARX_IMPORTANT << "index: " << getCyberGloveObserver()->getLatestValues().indexMCP << " thumb: " << getCyberGloveObserver()->getLatestValues().thumbMCP;
 
        guiTab.sendUpdates();
 
        currentPhase = nextPhase;
 
        TimeUtil::SleepMS(10);
    }
 
    shapeHand(0, 0);
    //logger->close();
    logger.close();
}
 
//void CyberGloveProsthesisControl::onBreak()
//{
//    // put your user code for the breaking point here
//    // execution time should be short (<100ms)
//}
 
void CyberGloveProsthesisControl::onExit()
{
    // put your user code for the exit point here
    // execution time should be short (<100ms)
}
 
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr CyberGloveProsthesisControl::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new CyberGloveProsthesisControl(stateData));
}
 
//void CyberGloveProsthesisControl::addCurrentRawJointValues(CyberGloveValues& currentValues, SimpleJsonLoggerEntry& e)
//{
//    std::map<std::string, float> currentRawJointValues;
//    currentRawJointValues["thumbAbd"] = currentValues.thumbAbd;
//    currentRawJointValues["thumbCMC"] = currentValues.thumbCMC;
//    currentRawJointValues["thumbIP"] = currentValues.thumbIP;
//    currentRawJointValues["thumbMCP"] = currentValues.thumbMCP;
 
//    currentRawJointValues["indexDIP"] = currentValues.indexDIP;
//    currentRawJointValues["indexMCP"] = currentValues.indexMCP;
//    currentRawJointValues["indexPIP"] = currentValues.indexPIP;
 
//    currentRawJointValues["middleAbd"] = currentValues.middleAbd;
//    currentRawJointValues["middleDIP"] = currentValues.middleDIP;
//    currentRawJointValues["middleMCP"] = currentValues.middleMCP;
//    currentRawJointValues["middlePIP"] = currentValues.middlePIP;
 
//    currentRawJointValues["ringAbd"] = currentValues.ringAbd;
//    currentRawJointValues["ringDIP"] = currentValues.ringDIP;
//    currentRawJointValues["ringMCP"] = currentValues.ringMCP;
//    currentRawJointValues["ringPIP"] = currentValues.ringPIP;
 
//    currentRawJointValues["littleAbd"] = currentValues.littleAbd;
//    currentRawJointValues["littleDIP"] = currentValues.littleDIP;
//    currentRawJointValues["littleMCP"] = currentValues.littleMCP;
//    currentRawJointValues["littlePIP"] = currentValues.littlePIP;
 
//    currentRawJointValues["palmArch"] = currentValues.palmArch;
 
//    currentRawJointValues["wristFlex"] = currentValues.wristFlex;
//    currentRawJointValues["wristAbd"] = currentValues.wristAbd;
 
//    e.Add("rawJointValues", currentRawJointValues);
//}
 
//SimpleJsonLoggerEntry CyberGloveProsthesisControl::getRawJointValuesForCalibration(CyberGloveValues& openValues, CyberGloveValues& closedValues)
//{
//    SimpleJsonLoggerEntry entriesForCalibration;
//    entriesForCalibration.AddTimestamp();
//    std::map<std::string, float> rawJointValuesOpen;
//    rawJointValuesOpen["thumbAbd"] = openValues.thumbAbd;
//    rawJointValuesOpen["thumbCMC"] = openValues.thumbCMC;
//    rawJointValuesOpen["thumbIP"] = openValues.thumbIP;
//    rawJointValuesOpen["thumbMCP"] = openValues.thumbMCP;
 
//    rawJointValuesOpen["indexDIP"] = openValues.indexDIP;
//    rawJointValuesOpen["indexMCP"] = openValues.indexMCP;
//    rawJointValuesOpen["indexPIP"] = openValues.indexPIP;
 
//    rawJointValuesOpen["middleAbd"] = openValues.middleAbd;
//    rawJointValuesOpen["middleDIP"] = openValues.middleDIP;
//    rawJointValuesOpen["middleMCP"] = openValues.middleMCP;
//    rawJointValuesOpen["middlePIP"] = openValues.middlePIP;
 
//    rawJointValuesOpen["ringAbd"] = openValues.ringAbd;
//    rawJointValuesOpen["ringDIP"] = openValues.ringDIP;
//    rawJointValuesOpen["ringMCP"] = openValues.ringMCP;
//    rawJointValuesOpen["ringPIP"] = openValues.ringPIP;
 
//    rawJointValuesOpen["littleAbd"] = openValues.littleAbd;
//    rawJointValuesOpen["littleDIP"] = openValues.littleDIP;
//    rawJointValuesOpen["littleMCP"] = openValues.littleMCP;
//    rawJointValuesOpen["littlePIP"] = openValues.littlePIP;
 
//    rawJointValuesOpen["palmArch"] = openValues.palmArch;
 
//    rawJointValuesOpen["wristFlex"] = openValues.wristFlex;
//    rawJointValuesOpen["wristAbd"] = openValues.wristAbd;
 
//    entriesForCalibration.Add("rawJointValuesForCalibrationOpen", rawJointValuesOpen);
 
//    std::map<std::string, float> rawJointValuesClosed;
//    rawJointValuesClosed["thumbAbd"] = closedValues.thumbAbd;
//    rawJointValuesClosed["thumbCMC"] = closedValues.thumbCMC;
//    rawJointValuesClosed["thumbIP"] = closedValues.thumbIP;
//    rawJointValuesClosed["thumbMCP"] = closedValues.thumbMCP;
 
//    rawJointValuesClosed["indexDIP"] = closedValues.indexDIP;
//    rawJointValuesClosed["indexMCP"] = closedValues.indexMCP;
//    rawJointValuesClosed["indexPIP"] = closedValues.indexPIP;
 
//    rawJointValuesClosed["middleAbd"] = closedValues.middleAbd;
//    rawJointValuesClosed["middleDIP"] = closedValues.middleDIP;
//    rawJointValuesClosed["middleMCP"] = closedValues.middleMCP;
//    rawJointValuesClosed["middlePIP"] = closedValues.middlePIP;
 
//    rawJointValuesClosed["ringAbd"] = closedValues.ringAbd;
//    rawJointValuesClosed["ringDIP"] = closedValues.ringDIP;
//    rawJointValuesClosed["ringMCP"] = closedValues.ringMCP;
//    rawJointValuesClosed["ringPIP"] = closedValues.ringPIP;
 
//    rawJointValuesClosed["littleAbd"] = closedValues.littleAbd;
//    rawJointValuesClosed["littleDIP"] = closedValues.littleDIP;
//    rawJointValuesClosed["littleMCP"] = closedValues.littleMCP;
//    rawJointValuesClosed["littlePIP"] = closedValues.littlePIP;
 
//    rawJointValuesClosed["palmArch"] = closedValues.palmArch;
 
//    rawJointValuesClosed["wristFlex"] = closedValues.wristFlex;
//    rawJointValuesClosed["wristAbd"] = closedValues.wristAbd;
 
//    entriesForCalibration.Add("rawJointValuesForCalibrationClosed", rawJointValuesClosed);
 
//    return entriesForCalibration;
//}