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); }