コード例 #1
0
void mtsIntuitiveResearchKitArm::SetBaseFrame(const prmPositionCartesianGet & newBaseFrame)
{
    if (newBaseFrame.Valid()) {
        this->BaseFrame.FromNormalized(newBaseFrame.Position());
        this->BaseFrameValid = true;
    } else {
        this->BaseFrameValid = false;
    }
}
void mtsIntuitiveResearchKitConsole::SUJECMBaseFrameHandler(const prmPositionCartesianGet & baseFrameParam)
{
    // get position from ECM and convert to useful type
    prmPositionCartesianGet positionECMLocalParam;
    mGetPositionCartesianLocalFromECM(positionECMLocalParam);
    vctFrm3 positionECM = baseFrameParam.Position() * positionECMLocalParam.Position();

    // compute and send new base frame for all SUJs (SUJ will handle ECM differently)
    prmPositionCartesianGet baseFrameSUJParam;
    baseFrameSUJParam.Position().From(positionECM.Inverse());
    baseFrameSUJParam.SetValid(baseFrameParam.Valid() && positionECMLocalParam.Valid());
    baseFrameSUJParam.SetTimestamp(positionECMLocalParam.Timestamp());
    mECMBaseFrameEvent(baseFrameSUJParam);
}