src/FastResearchInterfaceLibrary/GetRobotControlData.cpp

Go to the documentation of this file.

//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
//   For a convenient reading of this file's source code,
//   please use a tab width of four characters.
//  ----------------------------------------------------------


#include <FastResearchInterface.h>
#include <pthread.h>
#include <friComm.h>


// ****************************************************************
// GetMeasuredJointPositions()
//
void FastResearchInterface::GetMeasuredJointPositions(float *MeasuredJointPositions)
{
    unsigned int        i   =   0;

    pthread_mutex_lock(&(this->MutexForControlData));
    for (i = 0; i < LBR_MNJ; i++)
    {
        MeasuredJointPositions[i]   =   this->ReadData.data.msrJntPos[i];
    }
    pthread_mutex_unlock(&(this->MutexForControlData));

    return;
}


// ****************************************************************
// GetCommandedJointPositions()
//
void FastResearchInterface::GetCommandedJointPositions(float *CommandedJointPositions)
{
    unsigned int        i   =   0;

    pthread_mutex_lock(&(this->MutexForControlData));
    for (i = 0; i < LBR_MNJ; i++)
    {
        CommandedJointPositions[i]  =   this->ReadData.data.cmdJntPos[i];
    }
    pthread_mutex_unlock(&(this->MutexForControlData));

    return;
}


// ****************************************************************
// GetCommandedJointPositionOffsets()
//
void FastResearchInterface::GetCommandedJointPositionOffsets(float *CommandedJointPositionOffsets)
{
    unsigned int        i   =   0;

    pthread_mutex_lock(&(this->MutexForControlData));
    for (i = 0; i < LBR_MNJ; i++)
    {
        CommandedJointPositionOffsets[i]    =   this->ReadData.data.cmdCartPosFriOffset[i];
    }
    pthread_mutex_unlock(&(this->MutexForControlData));

    return;
}


// ****************************************************************
// GetMeasuredJointTorques()
//
void FastResearchInterface::GetMeasuredJointTorques(float *MeasuredJointTorques)
{
    unsigned int        i   =   0;

    pthread_mutex_lock(&(this->MutexForControlData));
    for (i = 0; i < LBR_MNJ; i++)
    {
        MeasuredJointTorques[i] =   this->ReadData.data.msrJntTrq[i];
    }
    pthread_mutex_unlock(&(this->MutexForControlData));

    return;
}


// ****************************************************************
// GetEstimatedExternalJointTorques()
//
void FastResearchInterface::GetEstimatedExternalJointTorques(float *EstimatedExternalJointTorques)
{
    unsigned int        i   =   0;

    pthread_mutex_lock(&(this->MutexForControlData));
    for (i = 0; i < LBR_MNJ; i++)
    {
        EstimatedExternalJointTorques[i]    =   this->ReadData.data.estExtJntTrq[i];
    }
    pthread_mutex_unlock(&(this->MutexForControlData));

    return;
}


// ****************************************************************
// GetMeasuredCartPose()
//
void FastResearchInterface::GetMeasuredCartPose(float *MeasuredCartPose)
{
    unsigned int        i   =   0;

    pthread_mutex_lock(&(this->MutexForControlData));
    for (i = 0; i < FRI_CART_FRM_DIM; i++)
    {
        MeasuredCartPose[i] =   this->ReadData.data.msrCartPos[i];
    }
    pthread_mutex_unlock(&(this->MutexForControlData));

    return;
}


// ****************************************************************
// GetCommandedCartPose()
//
void FastResearchInterface::GetCommandedCartPose(float *CommandedCartPose)
{
    unsigned int        i   =   0;

    pthread_mutex_lock(&(this->MutexForControlData));
    for (i = 0; i < FRI_CART_FRM_DIM; i++)
    {
        CommandedCartPose[i]    =   this->ReadData.data.cmdCartPos[i];
    }
    pthread_mutex_unlock(&(this->MutexForControlData));

    return;
}


// ****************************************************************
// GetCommandedCartPoseOffsets()
//
void FastResearchInterface::GetCommandedCartPoseOffsets(float *CommandedCartPoseOffsets)
{
    unsigned int        i   =   0;

    pthread_mutex_lock(&(this->MutexForControlData));
    for (i = 0; i < FRI_CART_FRM_DIM; i++)
    {
        CommandedCartPoseOffsets[i] =   this->ReadData.data.cmdCartPosFriOffset[i];
    }
    pthread_mutex_unlock(&(this->MutexForControlData));

    return;
}


// ****************************************************************
// GetEstimatedExternalCartForcesAndTorques()
//
void FastResearchInterface::GetEstimatedExternalCartForcesAndTorques(float *EstimatedExternalCartForcesAndTorques)
{
    unsigned int        i   =   0;

    pthread_mutex_lock(&(this->MutexForControlData));
    for (i = 0; i < FRI_CART_VEC; i++)
    {
        EstimatedExternalCartForcesAndTorques[i]    =   this->ReadData.data.estExtTcpFT[i];
    }
    pthread_mutex_unlock(&(this->MutexForControlData));

    return;
}