src/FastResearchInterfaceLibrary/SetRobotControlData.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>


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

    pthread_mutex_lock(&(this->MutexForControlData));
    for (i = 0; i < LBR_MNJ; i++)
    {
        this->CommandData.cmd.jntPos[i] =   CommandedJointPositions[i];
    }
    pthread_mutex_unlock(&(this->MutexForControlData));

    return;
}


// ****************************************************************
// SetCommandedJointTorques()
//
void FastResearchInterface::SetCommandedJointTorques(const float *CommandedJointTorques)
{
    unsigned int        i   =   0;

    pthread_mutex_lock(&(this->MutexForControlData));
    for (i = 0; i < LBR_MNJ; i++)
    {
        this->CommandData.cmd.addJntTrq[i]  =   CommandedJointTorques[i];
    }
    pthread_mutex_unlock(&(this->MutexForControlData));

    return;
}


// ****************************************************************
// SetCommandedJointStiffness()
//
void FastResearchInterface::SetCommandedJointStiffness(const float *CommandedJointStiffness)
{
    unsigned int        i   =   0;

    pthread_mutex_lock(&(this->MutexForControlData));
    for (i = 0; i < LBR_MNJ; i++)
    {
        this->CommandData.cmd.jntStiffness[i]   =   CommandedJointStiffness[i];
    }
    pthread_mutex_unlock(&(this->MutexForControlData));

    return;
}


// ****************************************************************
// SetCommandedJointDamping()
//
void FastResearchInterface::SetCommandedJointDamping(const float *CommandedJointDamping)
{
    unsigned int        i   =   0;

    pthread_mutex_lock(&(this->MutexForControlData));
    for (i = 0; i < LBR_MNJ; i++)
    {
        this->CommandData.cmd.jntDamping[i] =   CommandedJointDamping[i];
    }
    pthread_mutex_unlock(&(this->MutexForControlData));

    return;
}


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

    pthread_mutex_lock(&(this->MutexForControlData));
    for (i = 0; i < FRI_CART_FRM_DIM; i++)
    {
        this->CommandData.cmd.cartPos[i]    =   CommandedCartPose[i];
    }
    pthread_mutex_unlock(&(this->MutexForControlData));

    return;
}


// ****************************************************************
// SetCommandedCartForcesAndTorques()
//
void FastResearchInterface::SetCommandedCartForcesAndTorques(const float *CartForcesAndTorques)
{
    unsigned int        i   =   0;

    pthread_mutex_lock(&(this->MutexForControlData));
    for (i = 0; i < FRI_CART_VEC; i++)
    {
        this->CommandData.cmd.addTcpFT[i]   =   CartForcesAndTorques[i];
    }
    pthread_mutex_unlock(&(this->MutexForControlData));

    return;
}


// ****************************************************************
// SetCommandedCartStiffness()
//
void FastResearchInterface::SetCommandedCartStiffness(const float *CommandedCartStiffness)
{
    unsigned int        i   =   0;

    pthread_mutex_lock(&(this->MutexForControlData));
    for (i = 0; i < FRI_CART_VEC; i++)
    {
        this->CommandData.cmd.cartStiffness[i]  =   CommandedCartStiffness[i];
    }
    pthread_mutex_unlock(&(this->MutexForControlData));

    return;
}


// ****************************************************************
// SetCommandedCartDamping()
//
void FastResearchInterface::SetCommandedCartDamping(const float *CommandedCartDamping)
{
    unsigned int        i   =   0;

    pthread_mutex_lock(&(this->MutexForControlData));
    for (i = 0; i < FRI_CART_VEC; i++)
    {
        this->CommandData.cmd.cartDamping[i]    =   CommandedCartDamping[i];
    }
    pthread_mutex_unlock(&(this->MutexForControlData));

    return;
}