HandModeliCub.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 "HandModeliCub.h"
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <iostream>
#include <fstream>
 
 
 
namespace visionx
{
    HandModeliCub::HandModeliCub(std::string sConfigFileName, CStereoCalibration* pStereoCalibration)
    {
        m_pStereoCalibration = pStereoCalibration;
 
        //****************************************
        // read hand config file
        //****************************************
 
        std::ifstream sFileStream(sConfigFileName.c_str(), std::ifstream::in);
        if (!sFileStream.good())
        {
            ARMARX_WARNING_S << "CHandModelV2 constructor: file " <<  sConfigFileName << " could not be opened";
            return;
        }
 
        std::string sLine;
        Vec3d vTemp;
        float fTemp;
        std::vector<std::vector<float> > aJointOffsets;
        aJointOffsets.resize(5);
 
        // offset to thumb base
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f %f %f", &vTemp.x, &vTemp.y, &vTemp.z);
        m_aOffsetsToFingers.push_back(vTemp);
 
        // distance from thumb base joint to second joint
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f", &fTemp);
        aJointOffsets.at(0).push_back(fTemp);
 
        // distance from second to third thumb joint
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f", &fTemp);
        aJointOffsets.at(0).push_back(fTemp);
 
        // distance from third to fourth thumb joint
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f", &fTemp);
        aJointOffsets.at(0).push_back(fTemp);
 
        // distance from fourth thumb joint to fingertip
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f", &fTemp);
        aJointOffsets.at(0).push_back(fTemp);
 
 
 
        // offset from palm to index base
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f %f %f", &vTemp.x, &vTemp.y, &vTemp.z);
        m_aOffsetsToFingers.push_back(vTemp);
 
        // distance from index base joint to second joint
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f", &fTemp);
        aJointOffsets.at(1).push_back(fTemp);
 
        // distance from second to third index joint
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f", &fTemp);
        aJointOffsets.at(1).push_back(fTemp);
 
        // distance from third index joint to fingertip
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f", &fTemp);
        aJointOffsets.at(1).push_back(fTemp);
 
 
        // offset from palm to middle base
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f %f %f", &vTemp.x, &vTemp.y, &vTemp.z);
        m_aOffsetsToFingers.push_back(vTemp);
 
        // distance from middle base joint to second joint
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f", &fTemp);
        aJointOffsets.at(2).push_back(fTemp);
 
        // distance from second to third middle joint
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f", &fTemp);
        aJointOffsets.at(2).push_back(fTemp);
 
        // distance from third middle joint to fingertip
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f", &fTemp);
        aJointOffsets.at(2).push_back(fTemp);
 
 
        // offset from palm to ring base
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f %f %f", &vTemp.x, &vTemp.y, &vTemp.z);
        m_aOffsetsToFingers.push_back(vTemp);
 
        // distance from ring base joint to second joint
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f", &fTemp);
        aJointOffsets.at(3).push_back(fTemp);
 
        // distance from second to third ring joint
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f", &fTemp);
        aJointOffsets.at(3).push_back(fTemp);
 
        // distance from third ring joint to fingertip
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f", &fTemp);
        aJointOffsets.at(3).push_back(fTemp);
 
 
        // offset from palm to pinky base
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f %f %f", &vTemp.x, &vTemp.y, &vTemp.z);
        m_aOffsetsToFingers.push_back(vTemp);
 
        // distance from pinky base joint to second joint
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f", &fTemp);
        aJointOffsets.at(4).push_back(fTemp);
 
        // distance from second to third pinky joint
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f", &fTemp);
        aJointOffsets.at(4).push_back(fTemp);
 
        // distance from third pinky joint to fingertip
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f", &fTemp);
        aJointOffsets.at(4).push_back(fTemp);
 
 
        // read fingertip points
 
        int nNumFingertipPoints;
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%d", &nNumFingertipPoints);
        ARMARX_VERBOSE_S << "Number of fingertip points: " << nNumFingertipPoints;
 
        float fFingerTipOffsetZ;
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f", &fFingerTipOffsetZ);
 
 
        std::vector<Vec3d> aFingertipPoints;
        sLine = GetNextNonCommentLine(sFileStream);
        for (int i = 0; i < nNumFingertipPoints; i++)
        {
            sscanf(sLine.c_str(), "%f %f %f", &vTemp.x, &vTemp.y, &vTemp.z);
            vTemp.z += fFingerTipOffsetZ;
            aFingertipPoints.push_back(vTemp);
            std::getline(sFileStream, sLine);
        }
 
 
        // tracking ball
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f %f %f", &m_vTrackingBallOffset.x, &m_vTrackingBallOffset.y, &m_vTrackingBallOffset.z);
        sLine = GetNextNonCommentLine(sFileStream);
        sscanf(sLine.c_str(), "%f", &m_fTrackingBallRadius);
 
 
 
        //****************************************
        // construct fingers
        //****************************************
 
        // thumb
        std::vector<Vec3d> aFingertipPointsThumb;
        aFingertipPointsThumb.resize(nNumFingertipPoints);
        for (int i = 0; i < nNumFingertipPoints; i++)
        {
            Math3d::SetVec(aFingertipPointsThumb.at(i), aFingertipPoints.at(i).x, aFingertipPoints.at(i).y, aFingertipPoints.at(i).z);
        }
        CFinger* pNewFinger = new CFinger(aJointOffsets.at(0), aFingertipPointsThumb);
        m_aFingers.push_back(pNewFinger);
 
        // other fingers
        for (int i = 1; i <= 4; i++)
        {
            pNewFinger = new CFinger(aJointOffsets.at(i), aFingertipPoints);
            m_aFingers.push_back(pNewFinger);
        }
 
 
 
        //****************************************
        // init variables
        //****************************************
 
        m_aFingerJointsInWorldCS.resize(5);
        for (int i = 0; i < 5; i++)
        {
            m_aFingerJointsInWorldCS.at(i).resize(m_aFingers.at(i)->m_aFingerJointsInFingerBaseCS.size());
            for (size_t j = 0; j < m_aFingerJointsInWorldCS.at(i).size(); j++)
            {
                Math3d::SetVec(m_aFingerJointsInWorldCS.at(i).at(j), Math3d::zero_vec);
            }
        }
 
        m_aFingerTipCornersInWorldCS.resize(5);
        for (int i = 0; i < 5; i++)
        {
            m_aFingerTipCornersInWorldCS.at(i).resize(m_aFingers.at(i)->m_aFingerTipCornersInFingerBaseCS.size());
            for (size_t j = 0; j < m_aFingerTipCornersInWorldCS.at(i).size(); j++)
            {
                Math3d::SetVec(m_aFingerTipCornersInWorldCS.at(i).at(j), Math3d::zero_vec);
            }
        }
 
        m_aFingerTipPolygonsLeftCam.resize(5);
        m_aFingerTipPolygonsRightCam.resize(5);
 
        m_fPalmJointAngle = 0;
        Math3d::SetVec(m_vHandPosition, Math3d::zero_vec);
        Math3d::SetMat(m_mHandOrientation, Math3d::unit_mat);
    }
 
 
 
    void HandModeliCub::UpdateHand(double* pConfig)
    {
        Vec3d vHandPosition = {(float)pConfig[0], (float)pConfig[1], (float) pConfig[2]};
        Mat3d mHandOrientation;
        Math3d::SetRotationMat(mHandOrientation, pConfig[3], pConfig[4], pConfig[5]);
 
        float fPalmJointAngle = pConfig[6];
 
        std::vector<std::vector<float> > aFingerJointAngles;
        aFingerJointAngles.resize(5);
        aFingerJointAngles.at(0).push_back(pConfig[7]);
        aFingerJointAngles.at(0).push_back(pConfig[8]);
        aFingerJointAngles.at(0).push_back(pConfig[8]);
        aFingerJointAngles.at(0).push_back(pConfig[8]);
        aFingerJointAngles.at(1).push_back(-pConfig[9]);
        aFingerJointAngles.at(1).push_back(-pConfig[9]);
        aFingerJointAngles.at(1).push_back(-pConfig[9]);
        aFingerJointAngles.at(2).push_back(-pConfig[10]);
        aFingerJointAngles.at(2).push_back(-pConfig[10]);
        aFingerJointAngles.at(2).push_back(-pConfig[10]);
        aFingerJointAngles.at(3).push_back(-pConfig[11]);
        aFingerJointAngles.at(3).push_back(-pConfig[11]);
        aFingerJointAngles.at(3).push_back(-pConfig[11]);
        aFingerJointAngles.at(4).push_back(-pConfig[11]);
        aFingerJointAngles.at(4).push_back(-pConfig[11]);
        aFingerJointAngles.at(4).push_back(-pConfig[11]);
 
        CHandModelV2::UpdateHand(vHandPosition, mHandOrientation, fPalmJointAngle, aFingerJointAngles);
    }
}