void mtsTeleOperationECM::Init(void)
{
// configure state machine
mTeleopState.AddState("SETTING_ARMS_STATE");
mTeleopState.AddState("ENABLED");
mTeleopState.AddAllowedDesiredState("DISABLED");
mTeleopState.AddAllowedDesiredState("ENABLED");
// state change, to convert to string events for users (Qt, ROS)
mTeleopState.SetStateChangedCallback(&mtsTeleOperationECM::StateChanged,
this);
// run for all states
mTeleopState.SetRunCallback(&mtsTeleOperationECM::RunAllStates,
this);
// disabled
mTeleopState.SetTransitionCallback("DISABLED",
&mtsTeleOperationECM::TransitionDisabled,
this);
// setting arms state
mTeleopState.SetEnterCallback("SETTING_ARMS_STATE",
&mtsTeleOperationECM::EnterSettingArmsState,
this);
mTeleopState.SetTransitionCallback("SETTING_ARMS_STATE",
&mtsTeleOperationECM::TransitionSettingArmsState,
this);
// enabled
mTeleopState.SetEnterCallback("ENABLED",
&mtsTeleOperationECM::EnterEnabled,
this);
mTeleopState.SetRunCallback("ENABLED",
&mtsTeleOperationECM::RunEnabled,
this);
mTeleopState.SetTransitionCallback("ENABLED",
&mtsTeleOperationECM::TransitionEnabled,
this);
mScale = 0.2;
mIsClutched = false;
StateTable.AddData(mMTML.PositionCartesianCurrent, "MTMLCartesianPosition");
StateTable.AddData(mMTMR.PositionCartesianCurrent, "MTMRCartesianPosition");
StateTable.AddData(mECM.PositionCartesianCurrent, "ECMCartesianPosition");
mConfigurationStateTable = new mtsStateTable(100, "Configuration");
mConfigurationStateTable->SetAutomaticAdvance(false);
AddStateTable(mConfigurationStateTable);
mConfigurationStateTable->AddData(mScale, "Scale");
mConfigurationStateTable->AddData(mRegistrationRotation, "RegistrationRotation");
mtsInterfaceRequired * interfaceRequired = AddInterfaceRequired("MTML");
if (interfaceRequired) {
interfaceRequired->AddFunction("GetPositionCartesian",
mMTML.GetPositionCartesian);
interfaceRequired->AddFunction("GetVelocityCartesian",
mMTML.GetVelocityCartesian);
interfaceRequired->AddFunction("GetCurrentState",
mMTML.GetCurrentState);
interfaceRequired->AddFunction("GetDesiredState",
mMTML.GetDesiredState);
interfaceRequired->AddFunction("SetDesiredState",
mMTML.SetDesiredState);
interfaceRequired->AddFunction("LockOrientation",
mMTML.LockOrientation);
interfaceRequired->AddFunction("SetWrenchBody",
mMTML.SetWrenchBody);
interfaceRequired->AddFunction("SetWrenchBodyOrientationAbsolute",
mMTML.SetWrenchBodyOrientationAbsolute);
interfaceRequired->AddEventHandlerWrite(&mtsTeleOperationECM::MTMLErrorEventHandler,
this, "Error");
}
interfaceRequired = AddInterfaceRequired("MTMR");
if (interfaceRequired) {
interfaceRequired->AddFunction("GetPositionCartesian",
mMTMR.GetPositionCartesian);
interfaceRequired->AddFunction("GetVelocityCartesian",
mMTMR.GetVelocityCartesian);
interfaceRequired->AddFunction("GetCurrentState",
mMTMR.GetCurrentState);
interfaceRequired->AddFunction("GetDesiredState",
mMTMR.GetDesiredState);
interfaceRequired->AddFunction("SetDesiredState",
mMTMR.SetDesiredState);
interfaceRequired->AddFunction("LockOrientation",
mMTMR.LockOrientation);
interfaceRequired->AddFunction("SetWrenchBody",
mMTMR.SetWrenchBody);
interfaceRequired->AddFunction("SetWrenchBodyOrientationAbsolute",
mMTMR.SetWrenchBodyOrientationAbsolute);
interfaceRequired->AddEventHandlerWrite(&mtsTeleOperationECM::MTMRErrorEventHandler,
this, "Error");
}
interfaceRequired = AddInterfaceRequired("ECM");
if (interfaceRequired) {
// ECM, use PID desired position to make sure there is no jump when engaging
interfaceRequired->AddFunction("GetPositionCartesianDesired",
mECM.GetPositionCartesian);
interfaceRequired->AddFunction("GetStateJointDesired",
mECM.GetStateJointDesired);
interfaceRequired->AddFunction("SetPositionJoint",
mECM.SetPositionJoint);
interfaceRequired->AddFunction("GetCurrentState",
mECM.GetCurrentState);
interfaceRequired->AddFunction("GetDesiredState",
mECM.GetDesiredState);
interfaceRequired->AddFunction("SetDesiredState",
mECM.SetDesiredState);
interfaceRequired->AddEventHandlerWrite(&mtsTeleOperationECM::ECMErrorEventHandler,
this, "Error");
}
// footpedal events
interfaceRequired = AddInterfaceRequired("Clutch");
if (interfaceRequired) {
interfaceRequired->AddEventHandlerWrite(&mtsTeleOperationECM::ClutchEventHandler, this, "Button");
}
mInterface = AddInterfaceProvided("Setting");
if (mInterface) {
mInterface->AddMessageEvents();
// commands
mInterface->AddCommandReadState(StateTable, StateTable.PeriodStats,
"GetPeriodStatistics"); // mtsIntervalStatistics
mInterface->AddCommandWrite(&mtsTeleOperationECM::SetDesiredState, this,
"SetDesiredState", std::string("DISABLED"));
mInterface->AddCommandWrite(&mtsTeleOperationECM::SetScale, this,
"SetScale", 0.5);
mInterface->AddCommandWrite(&mtsTeleOperationECM::SetRegistrationRotation, this,
"SetRegistrationRotation", vctMatRot3());
mInterface->AddCommandReadState(*mConfigurationStateTable,
mScale,
"GetScale");
mInterface->AddCommandReadState(*mConfigurationStateTable,
mRegistrationRotation,
"GetRegistrationRotation");
mInterface->AddCommandReadState(StateTable,
mMTML.PositionCartesianCurrent,
"GetPositionCartesianMTML");
mInterface->AddCommandReadState(StateTable,
mMTMR.PositionCartesianCurrent,
"GetPositionCartesianMTMR");
mInterface->AddCommandReadState(StateTable,
mECM.PositionCartesianCurrent,
"GetPositionCartesianECM");
// events
mInterface->AddEventWrite(MessageEvents.DesiredState,
"DesiredState", std::string(""));
mInterface->AddEventWrite(MessageEvents.CurrentState,
"CurrentState", std::string(""));
mInterface->AddEventWrite(MessageEvents.Following,
"Following", false);
// configuration
mInterface->AddEventWrite(ConfigurationEvents.Scale,
"Scale", 0.5);
}
}
void mtsTeleOperation::Init(void)
{
Counter = 0;
Scale = 0.2;
// Initialize states
this->IsClutched = false;
this->IsOperatorPresent = false;
this->IsEnabled = false;
Slave.IsManipClutched = false;
this->RotationLocked = false;
this->TranslationLocked = false;
this->StateTable.AddData(Master.PositionCartesianCurrent, "MasterCartesianPosition");
this->StateTable.AddData(Slave.PositionCartesianCurrent, "SlaveCartesianPosition");
this->ConfigurationStateTable = new mtsStateTable(100, "Configuration");
this->ConfigurationStateTable->SetAutomaticAdvance(false);
this->AddStateTable(this->ConfigurationStateTable);
this->ConfigurationStateTable->AddData(this->Scale, "Scale");
this->ConfigurationStateTable->AddData(this->RegistrationRotation, "RegistrationRotation");
this->ConfigurationStateTable->AddData(this->RotationLocked, "RotationLocked");
this->ConfigurationStateTable->AddData(this->TranslationLocked, "TranslationLocked");
// Setup CISST Interface
mtsInterfaceRequired * masterRequired = AddInterfaceRequired("Master");
if (masterRequired) {
masterRequired->AddFunction("GetPositionCartesian", Master.GetPositionCartesian);
masterRequired->AddFunction("SetPositionCartesian", Master.SetPositionCartesian);
masterRequired->AddFunction("SetPositionGoalCartesian", Master.SetPositionGoalCartesian);
masterRequired->AddFunction("GetGripperPosition", Master.GetGripperPosition);
masterRequired->AddFunction("SetRobotControlState", Master.SetRobotControlState);
masterRequired->AddEventHandlerWrite(&mtsTeleOperation::MasterErrorEventHandler, this, "Error");
}
mtsInterfaceRequired * slaveRequired = AddInterfaceRequired("Slave");
if (slaveRequired) {
slaveRequired->AddFunction("GetPositionCartesian", Slave.GetPositionCartesian);
slaveRequired->AddFunction("SetPositionCartesian", Slave.SetPositionCartesian);
slaveRequired->AddFunction("SetJawPosition", Slave.SetJawPosition);
slaveRequired->AddFunction("SetRobotControlState", Slave.SetRobotControlState);
slaveRequired->AddEventHandlerWrite(&mtsTeleOperation::SlaveErrorEventHandler, this, "Error");
slaveRequired->AddEventHandlerWrite(&mtsTeleOperation::SlaveClutchEventHandler, this, "ManipClutch");
}
// Footpedal events
mtsInterfaceRequired * clutchRequired = AddInterfaceRequired("Clutch");
if (clutchRequired) {
clutchRequired->AddEventHandlerWrite(&mtsTeleOperation::ClutchEventHandler, this, "Button");
}
mtsInterfaceRequired * headRequired = AddInterfaceRequired("OperatorPresent");
if (headRequired) {
headRequired->AddEventHandlerWrite(&mtsTeleOperation::OperatorPresentEventHandler, this, "Button");
}
mtsInterfaceProvided * providedSettings = AddInterfaceProvided("Setting");
if (providedSettings) {
// commands
providedSettings->AddCommandReadState(StateTable, StateTable.PeriodStats,
"GetPeriodStatistics"); // mtsIntervalStatistics
providedSettings->AddCommandWrite(&mtsTeleOperation::Enable, this, "Enable", false);
providedSettings->AddCommandWrite(&mtsTeleOperation::SetScale, this, "SetScale", 0.5);
providedSettings->AddCommandWrite(&mtsTeleOperation::SetRegistrationRotation, this,
"SetRegistrationRotation", vctMatRot3());
providedSettings->AddCommandWrite(&mtsTeleOperation::LockRotation, this, "LockRotation", false);
providedSettings->AddCommandWrite(&mtsTeleOperation::LockTranslation, this, "LockTranslation", false);
providedSettings->AddCommandWrite(&mtsTeleOperation::CameraClutchEventHandler, this, "CameraClutch", prmEventButton());
providedSettings->AddCommandReadState(*(this->ConfigurationStateTable), Scale, "GetScale");
providedSettings->AddCommandReadState(*(this->ConfigurationStateTable), RegistrationRotation, "GetRegistrationRotation");
providedSettings->AddCommandReadState(*(this->ConfigurationStateTable), RotationLocked, "GetRotationLocked");
providedSettings->AddCommandReadState(*(this->ConfigurationStateTable), TranslationLocked, "GetTranslationLocked");
providedSettings->AddCommandReadState(this->StateTable, Master.PositionCartesianCurrent, "GetPositionCartesianMaster");
providedSettings->AddCommandReadState(this->StateTable, Slave.PositionCartesianCurrent, "GetPositionCartesianSlave");
// events
providedSettings->AddEventWrite(MessageEvents.Status, "Status", std::string(""));
providedSettings->AddEventWrite(MessageEvents.Warning, "Warning", std::string(""));
providedSettings->AddEventWrite(MessageEvents.Error, "Error", std::string(""));
providedSettings->AddEventWrite(MessageEvents.Enabled, "Enabled", false);
// configuration
providedSettings->AddEventWrite(ConfigurationEvents.Scale, "Scale", 0.5);
providedSettings->AddEventWrite(ConfigurationEvents.RotationLocked, "RotationLocked", false);
providedSettings->AddEventWrite(ConfigurationEvents.TranslationLocked, "TranslationLocked", false);
}
}
