MoveMasterModel.cpp

Go to the documentation of this file.

// *****************************************************************
// Filename:    SearchModule.cpp
// Copyright:   Kai Welke, Chair Prof. Dillmann (IAIM),
//              Institute for Computer Science and Engineering (CSE),
//              University of Karlsruhe. All rights reserved.
// Author:      Kai Welke
// Date:        25.09.2008
// *****************************************************************
 
// *****************************************************************
// includes
// *****************************************************************
#include "MoveMasterModel.h"
 
 
namespace visionx
{
    // *****************************************************************
    // initialization of static members
    // *****************************************************************
    const std::string CMoveMasterModel::keypoint[NO_KEYPOINTS] = {"PalmCenter", "IndexTip", "MiddleTip", "ThumbTip"};
    const std::string CMoveMasterModel::jointname[NO_JOINTS] = {"Body", "Shoulder", "Elbow", "Wrist_pan", "Wrist_rotate",
                                                                "Palm", "Index_0", "Index_1", "Middle_0", "Middle_1", "Ring_0", "Ring_1",
                                                                "Pinky_0", "Pinky_1", "Thumb_0", "Thumb_1",
                                                                "Actuator_Thumb_1", "Actuator_Thumb_2", "Actuator_Index_1", "Actuator_Index_2",
                                                                "Actuator_Middle_1", "Actuator_Middle_2", "Actuator_Ring_1", "Actuator_Ring_2",
                                                                "Actuator_Pinky_1", "Actuator_Pinky_2"
                                                               };
 
    // *****************************************************************
    // implementation of CMoveMasterModel
    // *****************************************************************
    // *****************************************************************
    // construction / destruction
    // *****************************************************************
    CMoveMasterModel::CMoveMasterModel()
    {
        m_pOIFwdKinematicsInterface = NULL;
    }
 
    CMoveMasterModel::~CMoveMasterModel()
    {
        if (m_pOIFwdKinematicsInterface != NULL)
        {
            delete    m_pOIFwdKinematicsInterface;
        }
    }
 
    // *****************************************************************
    // init and update
    // *****************************************************************
    bool CMoveMasterModel::init(std::string sFilename, float fFocalLengthY)
    {
        if (m_pOIFwdKinematicsInterface != NULL)
        {
            printf("already initialized\n");
            return false;
        }
 
        m_pOIFwdKinematicsInterface = new OIFwdKinematicsInterface(sFilename.c_str());
        if (!m_pOIFwdKinematicsInterface->isInitialized())
        {
            m_pOIFwdKinematicsInterface->m_fFocalLengthY = fFocalLengthY;
            m_pOIFwdKinematicsInterface->initviewer();
        }
 
        // *** Extract keypoint paths initially for fast access later
        for (int i = 0; i < NO_KEYPOINTS; i++)
        {
            if (! m_pOIFwdKinematicsInterface->getPath(keypoint[i], keypath[i]))
            {
                printf("Keypoint node %s not found in path!\n", keypoint[i].c_str());
            }
        }
 
        // *** Extract joint paths initially for fast access later
        for (int i = 0; i < NO_JOINTS; i++)
        {
            if (! m_pOIFwdKinematicsInterface->getPath(jointname[i], jointpath[i]))
            {
                printf("joint node %s not found in path!\n", jointname[i].c_str());
            }
        }
 
 
        // hand (David)
        SoPath* temp_path;
        m_pOIFwdKinematicsInterface->getPath("HandPos", temp_path);
        hand_pos = static_cast<SoTranslation*>(temp_path->getNodeFromTail(0));
        m_pOIFwdKinematicsInterface->getPath("HandRot", temp_path);
        hand_rot = static_cast<SoTransform*>(temp_path->getNodeFromTail(0));
        // object cylinder (David)
        m_pOIFwdKinematicsInterface->getPath("ObjCylinderPos", temp_path);
        m_pObjCylPos = static_cast<SoTranslation*>(temp_path->getNodeFromTail(0));
        m_pOIFwdKinematicsInterface->getPath("ObjCylinderRot", temp_path);
        m_pObjCylRot = static_cast<SoTransform*>(temp_path->getNodeFromTail(0));
        m_pOIFwdKinematicsInterface->getPath("ObjCylinder", temp_path);
        m_pObjectCylinder = static_cast<SoCylinder*>(temp_path->getNodeFromTail(0));
        // test sphere (David)
        m_pOIFwdKinematicsInterface->getPath("TestSphere", temp_path);
        m_pTestSpherePos = static_cast<SoTranslation*>(temp_path->getNodeFromTail(0));
 
        return true;
    }
 
    void CMoveMasterModel::update()
    {
        if (!m_pOIFwdKinematicsInterface)
        {
            printf("call init first\n");
            return;
        }
 
        m_pOIFwdKinematicsInterface->update();
    }
 
    // *****************************************************************
    // kinematics
    // *****************************************************************
    void CMoveMasterModel::setJointAngle(int nJoint, float fAngleRad)
    {
        if (nJoint >= NO_JOINTS)
        {
            printf("Illegal joint: %d\n", nJoint);
            return;
        }
 
        if (!m_pOIFwdKinematicsInterface->setJointAngle(jointpath[nJoint], fAngleRad))
        {
            printf("Could not set joint angle for node %s!\n", jointname[nJoint].c_str());
        }
        //else
        //m_pOIFwdKinematicsInterface->update();
    }
 
    Vec3d CMoveMasterModel::getPositionOfKeypoint(int nKeyPoint)
    {
        Vec3d result;
 
        if (nKeyPoint >= NO_KEYPOINTS)
        {
            printf("Illegal keypoint: %d\n", nKeyPoint);
            return result;
        }
 
 
        if (m_pOIFwdKinematicsInterface->getTranslation(iposition[nKeyPoint], keypath[nKeyPoint]))
        {
            float x, y, z;
            iposition[nKeyPoint].getValue(x, y, z);
 
            // transform to robot coordinate system
            result.x = -x;
            result.y = y;
            result.z = -z;
        }
        else
        {
            printf("Could not retrieve position of keypath %s!\n", keypoint[nKeyPoint].c_str());
        }
 
        return result;
    }
 
 
 
 
 
    void CMoveMasterModel::setHandRotation(double alpha, double beta, double gamma)     //  <- added (David)
    {
        Mat3d tempmat;
        Vec3d axis;
        float angle;
        SbRotation rot;
        SbVec3f vec;
 
        Math3d::SetRotationMat(tempmat, alpha, beta, gamma);
        Math3d::GetAxisAndAngle(tempmat, axis, angle);
        if (Math3d::Length(axis) == 0)
        {
            axis.z = 1;
        }
        //vec.setValue(-axis.x, axis.y, -axis.z);// transform to robot coordinate system
        vec.setValue(axis.x, axis.y, axis.z);
 
        //*** Keep angle in interval [-2Pi,2Pi] (required for coin!)
        if (fabs(angle) > 2 * M_PI)
        {
            angle = angle - (floor(angle / (2 * M_PI)) * 2 * M_PI);
        }
        //*** Keep angle positive (required for coin!)
        if (angle <= 0.0f)               // This is mandatory: Coin uses quaternions internally and can thus not
        {
            angle = 2 * M_PI + angle;    // reflect angles of 0deg without change of axis. Therefore set to 2Pi instead.
        }
 
        hand_rot->rotation.setValue(vec, angle);
    }
 
 
    void CMoveMasterModel::setHandPosition(Vec3d pos)           //  <- added (David)
    {
        SbVec3f temp;
        temp.setValue(pos.x, pos.y, pos.z);     // transform to robot coordinate system?
        hand_pos->translation.setValue(temp);
    }
 
 
 
    void CMoveMasterModel::setObjCylConfig(Vec3d pos, double alpha, double beta, double gamma, float height, float radius) // (David)
    {
        SbVec3f temp;
        temp.setValue(pos.x, pos.y, pos.z);
        m_pObjCylPos->translation.setValue(temp);
 
 
        Mat3d tempmat;
        Vec3d axis;
        float angle;
        SbRotation rot;
        SbVec3f vec;
 
        Math3d::SetRotationMat(tempmat, alpha, beta, gamma);
        Math3d::GetAxisAndAngle(tempmat, axis, angle);
        if (Math3d::Length(axis) == 0)
        {
            axis.z = 1;
        }
        //vec.setValue(-axis.x, axis.y, -axis.z);// transform to robot coordinate system
        vec.setValue(axis.x, axis.y, axis.z);
 
        //*** Keep angle in interval [-2Pi,2Pi] (required for coin!)
        if (std::fabs(angle) > 2 * M_PI)
        {
            angle = angle - (std::floor(angle / (2 * M_PI)) * 2 * M_PI);
        }
        //*** Keep angle positive (required for coin!)
        if (angle <= 0.0f)               // This is mandatory: Coin uses quaternions internally and can thus not
        {
            angle = 2 * M_PI + angle;    // reflect angles of 0deg without change of axis. Therefore set to 2Pi instead.
        }
 
        m_pObjCylRot->rotation.setValue(vec, angle);
 
        m_pObjectCylinder->height.setValue(height);
        m_pObjectCylinder->radius.setValue(radius);
    }
 
 
    void CMoveMasterModel::setTestSpherePosition(Vec3d pos)
    {
        SbVec3f temp;
        temp.setValue(pos.x, pos.y, pos.z);     // transform to robot coordinate system?
        m_pTestSpherePos->translation.setValue(temp);
    }
}