HandModelV2.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 "HandModelV2.h"
 
#include <fstream>
#include <iostream>
 
#include <ArmarXCore/core/logging/Logging.h>
 
namespace visionx
{
 
    CHandModelV2::CFinger::CFinger(std::vector<float> aJointOffsets,
                                   std::vector<Vec3d> aFingerTipCorners)
    {
        for (size_t i = 0; i < aJointOffsets.size(); i++)
        {
            m_aJointOffsets.push_back(aJointOffsets.at(i));
            m_aJointAngles.push_back(0);
        }
        m_aFingerJointsInFingerBaseCS.resize(aJointOffsets.size() + 1);
        Math3d::SetVec(m_aFingerJointsInFingerBaseCS.at(0), 0, 0, 0);
 
        for (size_t i = 0; i < aFingerTipCorners.size(); i++)
        {
            m_aFingerTipCornersInLocalCS.push_back(aFingerTipCorners.at(i));
        }
        m_aFingerTipCornersInFingerBaseCS.resize(aFingerTipCorners.size());
 
        // initial calculation, all angles 0
        UpdateJointAngles(m_aJointAngles);
    }
 
    void
    CHandModelV2::CFinger::UpdateJointAngles(std::vector<float> aNewJointValues)
    {
        Mat3d mRotLocal, mRotGlobal = Math3d::unit_mat;
        Vec3d vTemp = Math3d::zero_vec;
 
        // joint angles and positions
        const size_t n = (m_aJointAngles.size() <= aNewJointValues.size()) ? m_aJointAngles.size()
                                                                           : aNewJointValues.size();
        for (size_t i = 0; i < n; i++)
        {
            m_aJointAngles.at(i) = aNewJointValues.at(i);
            Math3d::SetRotationMatY(mRotLocal, aNewJointValues.at(i));
            Math3d::MulMatMat(mRotGlobal, mRotLocal, mRotGlobal);
            vTemp.x = m_aJointOffsets.at(i);
            Math3d::MulMatVec(mRotGlobal,
                              vTemp,
                              m_aFingerJointsInFingerBaseCS.at(i),
                              m_aFingerJointsInFingerBaseCS.at(i + 1));
        }
 
        Vec3d vFingerTipPosition = m_aFingerJointsInFingerBaseCS.at(m_aJointAngles.size());
        for (size_t i = 0; i < m_aFingerTipCornersInLocalCS.size(); i++)
        {
            Math3d::MulMatVec(mRotGlobal,
                              m_aFingerTipCornersInLocalCS.at(i),
                              vFingerTipPosition,
                              m_aFingerTipCornersInFingerBaseCS.at(i));
        }
    }
 
    CHandModelV2::CHandModelV2(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;
        }
 
        setlocale(LC_NUMERIC, "C");
 
        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 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 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 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 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 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);
 
        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);
    }
 
    std::string
    CHandModelV2::GetNextNonCommentLine(std::ifstream& sFileStream)
    {
        std::string sLine;
 
        do
        {
            std::getline(sFileStream, sLine);
        } while (sLine.at(0) == '#');
 
        return sLine;
    }
 
    void
    CHandModelV2::UpdateHand(Vec3d vHandPosition,
                             Mat3d mHandOrientation,
                             float fPalmJointAngle,
                             std::vector<std::vector<float>>& aFingerJointAngles)
    {
        Math3d::SetVec(m_vHandPosition, vHandPosition);
        Math3d::SetMat(m_mHandOrientation, mHandOrientation);
 
        m_fPalmJointAngle = fPalmJointAngle;
 
        for (size_t i = 0; i < m_aFingers.size(); i++)
        {
            m_aFingers.at(i)->UpdateJointAngles(aFingerJointAngles.at(i));
        }
 
        Vec3d vOffsetToFingerBaseRotated, vTemp;
 
        // thumb
        Math3d::MulMatVec(
            m_mHandOrientation, m_aOffsetsToFingers.at(0), vOffsetToFingerBaseRotated);
        for (size_t i = 0; i < m_aFingers.at(0)->m_aFingerJointsInFingerBaseCS.size(); i++)
        {
            Math3d::MulMatVec(m_mHandOrientation,
                              m_aFingers.at(0)->m_aFingerJointsInFingerBaseCS.at(i),
                              vOffsetToFingerBaseRotated,
                              vTemp);
            Math3d::AddVecVec(vTemp, m_vHandPosition, m_aFingerJointsInWorldCS.at(0).at(i));
        }
        for (size_t i = 0; i < m_aFingers.at(0)->m_aFingerTipCornersInFingerBaseCS.size(); i++)
        {
            Math3d::MulMatVec(m_mHandOrientation,
                              m_aFingers.at(0)->m_aFingerTipCornersInFingerBaseCS.at(i),
                              vOffsetToFingerBaseRotated,
                              vTemp);
            Math3d::AddVecVec(vTemp, m_vHandPosition, m_aFingerTipCornersInWorldCS.at(0).at(i));
        }
 
        // other fingers
        Mat3d mPalmJointRotation, mAbsoluteRotationAfterPalmJoint;
        Math3d::SetRotationMatY(mPalmJointRotation, m_fPalmJointAngle);
        Math3d::MulMatMat(m_mHandOrientation, mPalmJointRotation, mAbsoluteRotationAfterPalmJoint);
 
        for (int j = 1; j <= 4; j++)
        {
            Math3d::MulMatVec(mAbsoluteRotationAfterPalmJoint,
                              m_aOffsetsToFingers.at(j),
                              vOffsetToFingerBaseRotated);
            for (size_t i = 0; i < m_aFingers.at(j)->m_aFingerJointsInFingerBaseCS.size(); i++)
            {
                Math3d::MulMatVec(mAbsoluteRotationAfterPalmJoint,
                                  m_aFingers.at(j)->m_aFingerJointsInFingerBaseCS.at(i),
                                  vOffsetToFingerBaseRotated,
                                  vTemp);
                Math3d::AddVecVec(vTemp, m_vHandPosition, m_aFingerJointsInWorldCS.at(j).at(i));
            }
 
            for (size_t i = 0; i < m_aFingers.at(j)->m_aFingerTipCornersInFingerBaseCS.size(); i++)
            {
                Math3d::MulMatVec(mAbsoluteRotationAfterPalmJoint,
                                  m_aFingers.at(j)->m_aFingerTipCornersInFingerBaseCS.at(i),
                                  vOffsetToFingerBaseRotated,
                                  vTemp);
                Math3d::AddVecVec(vTemp, m_vHandPosition, m_aFingerTipCornersInWorldCS.at(j).at(i));
            }
        }
 
 
        // project the fingertips and calculate convex polygons
        CalcProjectedPolygons();
 
 
        // tracking ball
        Vec3d vTrackingBallPositionInWorldCS;
        Math3d::MulMatVec(m_mHandOrientation,
                          m_vTrackingBallOffset,
                          m_vHandPosition,
                          vTrackingBallPositionInWorldCS);
 
        m_pStereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(
            vTrackingBallPositionInWorldCS, m_vTrackingBallPosLeftCam, false);
        float fFocalLengthLeft =
            0.5f *
            (m_pStereoCalibration->GetLeftCalibration()->GetCameraParameters().focalLength.x +
             m_pStereoCalibration->GetLeftCalibration()->GetCameraParameters().focalLength.y);
        m_fTrackingBallRadiusLeftCam =
            fFocalLengthLeft * m_fTrackingBallRadius / vTrackingBallPositionInWorldCS.z;
 
        m_pStereoCalibration->GetRightCalibration()->WorldToImageCoordinates(
            vTrackingBallPositionInWorldCS, m_vTrackingBallPosRightCam, false);
        float fFocalLengthRight =
            0.5f *
            (m_pStereoCalibration->GetRightCalibration()->GetCameraParameters().focalLength.x +
             m_pStereoCalibration->GetRightCalibration()->GetCameraParameters().focalLength.y);
        m_fTrackingBallRadiusRightCam =
            fFocalLengthRight * m_fTrackingBallRadius / vTrackingBallPositionInWorldCS.z;
    }
 
    void
    CHandModelV2::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(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(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]);
 
        UpdateHand(vHandPosition, mHandOrientation, fPalmJointAngle, aFingerJointAngles);
    }
 
    void
    CHandModelV2::CalcProjectedPolygons()
    {
        Vec2d vPos2D;
 
        Vec3d* pBuffer1 = new Vec3d[DSHT_MAX_POLYGON_CORNERS + 1];
        Vec3d* pBuffer2 = new Vec3d[DSHT_MAX_POLYGON_CORNERS + 1];
 
        for (int i = 0; i < DSHT_NUM_FINGERS; i++)
        {
            Math3d::SetVec(m_aFingerTipPolygonsLeftCam.at(i).center3d, Math3d::zero_vec);
            Math3d::SetVec(m_aFingerTipPolygonsRightCam.at(i).center3d, Math3d::zero_vec);
 
            for (size_t j = 0; j < m_aFingerTipCornersInWorldCS.at(i).size(); j++)
            {
                Vec3d& vPoint = m_aFingerTipCornersInWorldCS.at(i).at(j);
                m_pStereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(
                    vPoint, vPos2D, false);
                m_aFingerTipPolygonsLeftCam.at(i).hull[j].x = vPos2D.x;
                m_aFingerTipPolygonsLeftCam.at(i).hull[j].y = vPos2D.y;
                Math3d::AddToVec(vPoint, m_aFingerTipPolygonsLeftCam.at(i).center3d);
                m_pStereoCalibration->GetRightCalibration()->WorldToImageCoordinates(
                    vPoint, vPos2D, false);
                m_aFingerTipPolygonsRightCam.at(i).hull[j].x = vPos2D.x;
                m_aFingerTipPolygonsRightCam.at(i).hull[j].y = vPos2D.y;
                Math3d::AddToVec(vPoint, m_aFingerTipPolygonsRightCam.at(i).center3d);
            }
 
            Math3d::MulVecScalar(m_aFingerTipPolygonsLeftCam.at(i).center3d,
                                 1.0f / m_aFingerTipCornersInWorldCS.at(i).size(),
                                 m_aFingerTipPolygonsLeftCam.at(i).center3d);
            Math3d::MulVecScalar(m_aFingerTipPolygonsRightCam.at(i).center3d,
                                 1.0f / m_aFingerTipCornersInWorldCS.at(i).size(),
                                 m_aFingerTipPolygonsRightCam.at(i).center3d);
 
            m_aFingerTipPolygonsLeftCam.at(i).nCorners = m_aFingerTipCornersInWorldCS.at(i).size();
            ConvexPolygonCalculations::CalcConvexHull(
                &m_aFingerTipPolygonsLeftCam.at(i), pBuffer1, pBuffer2);
            m_aFingerTipPolygonsRightCam.at(i).nCorners = m_aFingerTipCornersInWorldCS.at(i).size();
            ConvexPolygonCalculations::CalcConvexHull(
                &m_aFingerTipPolygonsRightCam.at(i), pBuffer1, pBuffer2);
        }
 
        delete[] pBuffer1;
        delete[] pBuffer2;
    }
} // namespace visionx