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