Robot( const vctDynamicVector<double>& q ) : mtsTaskPeriodic( "Robot", 0.01, true ){ qout.Position() = q; mtsInterfaceProvided* input = AddInterfaceProvided( "Input" ); if( input ){ StateTable.AddData( tin, "TorquesInput" ); input->AddCommandWriteState( StateTable, tin, "SetTorqueJoint" ); } else{ CMN_LOG_RUN_ERROR << "Failed to create interface Input for " << GetName() << std::endl; } mtsInterfaceProvided* output = AddInterfaceProvided( "Output" ); if( output ){ StateTable.AddData( qout, "PositionsOutput" ); output->AddCommandReadState( StateTable, qout, "GetPositionJoint" ); } else{ CMN_LOG_RUN_ERROR << "Failed to create interface Output for " << GetName() << std::endl; } }
C2ServerTask::C2ServerTask(const std::string & taskName, double period): // base constructor, same task name and period. Set the length of // state table to 5000 mtsTaskPeriodic(taskName, period, false, 5000) { // add ServerData to the StateTable defined in mtsTask this->StateTable.AddData(ReadValue1, "ReadValue1"); this->StateTable.AddData(ReadValue2, "ReadValue2"); // add one interface, this will create an mtsInterfaceProvided mtsInterfaceProvided * provided = AddInterfaceProvided("p1"); if (provided) { provided->AddCommandVoid(&C2ServerTask::Void1, this, "Void"); provided->AddCommandWrite(&C2ServerTask::Write1, this, "Write"); provided->AddCommandReadState(this->StateTable, this->ReadValue1, "Read"); provided->AddCommandQualifiedRead(&C2ServerTask::QualifiedRead1, this, "QualifiedRead"); provided->AddEventVoid(this->EventVoid1, "EventVoid"); provided->AddEventWrite(this->EventWrite1, "EventWrite", mtsDouble(3.14)); } provided = AddInterfaceProvided("p2"); if (provided) { provided->AddCommandVoid(&C2ServerTask::Void2, this, "Void"); provided->AddCommandWrite(&C2ServerTask::Write2, this, "Write"); provided->AddCommandReadState(this->StateTable, this->ReadValue2, "Read"); provided->AddCommandQualifiedRead(&C2ServerTask::QualifiedRead2, this, "QualifiedRead"); provided->AddEventVoid(this->EventVoid2, "EventVoid"); provided->AddEventWrite(this->EventWrite2, "EventWrite", mtsDouble(3.14)); } }
// main constructor mtsOSGManipulatorTask::mtsOSGManipulatorTask( const std::string& name, double period, osaOSGManipulator* manipulator, osaCPUMask cpumask, int priority, InputType inputtype ) : mtsTaskPeriodic( name, period, true ), manipulator( manipulator ), inputtype( inputtype ), inputp( NULL ), inputr( NULL ), output( NULL ), ctl( NULL ), cpumask( cpumask ), priority( priority ){ if( inputtype == mtsOSGManipulatorTask::PROVIDE_INPUT ){ inputp = AddInterfaceProvided( "Input" ); if( inputp ){ StateTable.AddData( prmqin, "PositionJointInput" ); inputp->AddCommandWriteState( StateTable, prmqin, "SetPositionJoint" ); } else{ CMN_LOG_RUN_ERROR << "Failed to create interface Input for " << GetName() << std::endl; } } else{ inputr = AddInterfaceRequired( "Input" ); if( inputr ) { inputr->AddFunction( "GetPositionJoint", GetPositionJoint ); } else{ CMN_LOG_RUN_ERROR << "Failed to create interface Input for " << GetName() << std::endl; } } output = AddInterfaceProvided( "Output" ); if( output ){ StateTable.AddData( prmqout, "PositionJointOutput" ); StateTable.AddData( prmRtout, "PositionCartesianOutput" ); output->AddCommandReadState( StateTable, prmRtout, "GetPositionCartesian" ); output->AddCommandReadState( StateTable, prmqout, "GetPositionJoint" ); } else{ CMN_LOG_RUN_ERROR << "Failed to create interface Output for " << GetName() << std::endl; } }
void mtsMedtronicStealthlink::Init(void) { DataMapContainerInsertReturnValue dataMapInsertReturn; mtsInterfaceProvided * provided = 0; provided = AddInterfaceProvided("Controller"); if (provided){ //provided->AddCommandRead(&mtsMedtronicStealthlink::GetTool, this, "GetTool"); //provided->AddCommandRead(&mtsMedtronicStealthlink::GetFrame, this, "GetFrame"); } dataMapInsertReturn = myDataMap.insert(DataMapContainerItem(new MNavStealthLink::SurgicalPlan(),new mtsMedtronicStealthlink::SurgicalPlan())); if (dataMapInsertReturn.second){ provided = AddInterfaceProvided("SurgicalPlan"); dataMapInsertReturn.first->second->ConfigureInterfaceProvided(provided); }else{ CMN_LOG_CLASS_INIT_ERROR << "Init: Failed to add SurgicalPlan data to the DataMap " << std::endl; } dataMapInsertReturn = myDataMap.insert(DataMapContainerItem(new MNavStealthLink::Registration(),new mtsMedtronicStealthlink::Registration())); // Add interface for registration, ideally we should standardize such interface commands/payloads // maybe we should create a separate state table for registration? Would only advance if changed. if (dataMapInsertReturn.second){ provided = AddInterfaceProvided("Registration"); if(provided){ provided->AddCommandRead(&mtsMedtronicStealthlink::GetRegistrationValid, this, "GetValid"); } dataMapInsertReturn.first->second->ConfigureInterfaceProvided(provided); }else{ CMN_LOG_CLASS_INIT_ERROR << "Init: Failed to add Registration data to the DataMap " << std::endl; } dataMapInsertReturn = myDataMap.insert(DataMapContainerItem(new MNavStealthLink::Exam(),new mtsMedtronicStealthlink::ExamInformation())); if (dataMapInsertReturn.second){ provided = AddInterfaceProvided("ExamInformation"); if(provided){ provided->AddCommandVoid(&mtsMedtronicStealthlink::RequestExamInformation, this, "RequestExamInformation"); } dataMapInsertReturn.first->second->ConfigureInterfaceProvided(provided); } else { CMN_LOG_CLASS_INIT_ERROR << "Init: Failed to add Exam data to the DataMap " << std::endl; } }
guiTask::guiTask(const char *name) : mtsTaskContinuous(name){ mtsInterfaceRequired * required = AddInterfaceRequired("guiRequiresNDI"); if (required) { required->AddFunction("Beep", NDI.Beep); required->AddFunction("PortHandlesInitialize", NDI.Initialize); required->AddFunction("PortHandlesQuery", NDI.Query); required->AddFunction("PortHandlesEnable", NDI.Enable); required->AddFunction("CalibratePivot", NDI.CalibratePivot); required->AddFunction("ToggleTracking", NDI.Track); required->AddFunction("ReportStrayMarkers", NDI.ReportStrayMarkers); } mtsInterfaceRequired * requiredRobotBase = AddInterfaceRequired("guiRequiresRobotBase"); if (requiredRobotBase) { requiredRobotBase->AddFunction("GetPosition", RobotBase.GetPosition); } mtsInterfaceRequired * requiredRobotTip = AddInterfaceRequired("guiRequiresRobotTip"); if (requiredRobotTip) { requiredRobotTip->AddFunction("GetPosition", RobotTip.GetPosition); } mtsInterfaceProvided * provided = AddInterfaceProvided("guiProvidesClarity"); if (provided) { provided->AddCommandRead(&guiTask::GetRobotBasePosition, this, "GetRobotBasePosition", vct7()); provided->AddCommandRead(&guiTask::GetRobotTipPosition, this, "GetRobotTipPosition", vct7()); } trackToggle = true; robotBasePosition.SetAll(0.0); robotTipPosition.SetAll(0.0); }
void svlFilterSplitter::CreateInterfaces() { // Add NON-QUEUED provided interface for configuration management mtsInterfaceProvided* provided = AddInterfaceProvided("Settings", MTS_COMMANDS_SHOULD_NOT_BE_QUEUED); if (provided) { provided->AddCommandWrite(&svlFilterSplitter::AddOutputCommand, this, "AddOutput"); } }
bool mtsIntuitiveResearchKitConsole::AddFootpedalInterfaces(void) { // Footpedal events, receive mtsInterfaceRequired * clutchRequired = AddInterfaceRequired("Clutch"); if (clutchRequired) { clutchRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::ClutchEventHandler, this, "Button"); } else { return false; } mtsInterfaceRequired * cameraRequired = AddInterfaceRequired("Camera"); if (cameraRequired) { cameraRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::CameraEventHandler, this, "Button"); } else { return false; } mtsInterfaceRequired * headRequired = AddInterfaceRequired("OperatorPresent"); if (headRequired) { headRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::OperatorPresentEventHandler, this, "Button"); } else { return false; } // Console events, send mtsInterfaceProvided * interfaceProvided = AddInterfaceProvided("Clutch"); if (interfaceProvided) { interfaceProvided->AddEventWrite(ConsoleEvents.Clutch, "Button", prmEventButton()); } else { return false; } interfaceProvided = AddInterfaceProvided("Camera"); if (interfaceProvided) { interfaceProvided->AddEventWrite(ConsoleEvents.Camera, "Button", prmEventButton()); } else { return false; } interfaceProvided = AddInterfaceProvided("OperatorPresent"); if (interfaceProvided) { interfaceProvided->AddEventWrite(ConsoleEvents.OperatorPresent, "Button", prmEventButton()); } else { return false; } return true; }
CameraMotion() : mtsTaskPeriodic( "CameraMotion", 0.01, true ){ Rt.Position().Translation()[2] = 1; StateTable.AddData( Rt, "PositionOrientation" ); // provide the camera position mtsInterfaceProvided* output = AddInterfaceProvided( "Output" ); output->AddCommandReadState( StateTable, Rt, "GetPositionCartesian" ); }
void mtsSartoriusSerial::SetupInterface(void) { // add weight to state table StateTable.AddData(this->Weight, "Weight"); // add one interface, this will create an mtsTaskInterface mtsInterfaceProvided * providedInterface = AddInterfaceProvided("Scale"); if (providedInterface) { // add command to access state table values to the interface providedInterface->AddCommandReadState(this->StateTable, this->Weight, "GetWeight"); } }
void NDISimulator::Construct(void){ mtsInterfaceProvided* provided = AddInterfaceProvided("providesPositionCartesian"); StateTable.AddData(MarkerPosition,"MarkerPosition"); if(provided){ provided->AddCommandReadState(StateTable, MarkerPosition, "GetPositionCartesian"); } theta = 0; }
// main constructor mtsODEManipulatorTask::mtsODEManipulatorTask( const std::string& name, double period, osaOSGManipulator* manipulator, osaCPUMask cpumask, int priority ) : mtsTaskPeriodic( name, period, true ), manipulator( manipulator ), input( NULL ), output( NULL ), ctl( NULL ), cpumask( cpumask ), priority( priority ){ input = AddInterfaceProvided( "Input" ); if( input ){ StateTable.AddData( qin, "PositionsInput" ); input->AddCommandWriteState( StateTable, qin, "SetPositionJoint" ); } else{ CMN_LOG_RUN_ERROR << "Failed to create interface Input for " << GetName() << std::endl; } output = AddInterfaceProvided( "Output" ); if( output ){ StateTable.AddData( qout, "PositionsOutput" ); output->AddCommandReadState( StateTable, qout, "GetPositionJoint" ); } else{ CMN_LOG_RUN_ERROR << "Failed to create interface Output for " << GetName() << std::endl; } }
mtsIntuitiveResearchKitConsole::mtsIntuitiveResearchKitConsole(const std::string & componentName): mtsTaskFromSignal(componentName, 100) { mtsInterfaceProvided * interfaceProvided = AddInterfaceProvided("Main"); if (interfaceProvided) { interfaceProvided->AddCommandWrite(&mtsIntuitiveResearchKitConsole::SetRobotsControlState, this, "SetRobotsControlState", std::string("")); interfaceProvided->AddCommandWrite(&mtsIntuitiveResearchKitConsole::TeleopEnable, this, "TeleopEnable", false); interfaceProvided->AddEventWrite(MessageEvents.Error, "Error", std::string("")); interfaceProvided->AddEventWrite(MessageEvents.Warning, "Warning", std::string("")); interfaceProvided->AddEventWrite(MessageEvents.Status, "Status", std::string("")); } }
void mtsMicroScribeDigitizer::InitComponent(void) { mtsInterfaceProvided * provided = AddInterfaceProvided(DigitizerInterfaceName); if (provided) { StateTable.AddData(DeviceStatus, "DeviceStatus"); StateTable.AddData(TipPosition, "TipPosition"); StateTable.AddData(TipOrientation, "TipOrientation"); StateTable.AddData(TipOrientationUnitVector, "TipOrientationUnitVector"); StateTable.AddData(ButtonState, "ButtonState"); StateTable.AddData(JointReadings, "JointReadings"); provided->AddCommandReadState(StateTable, DeviceStatus, DigitizerCommandNames::GetDeviceStatus); provided->AddCommandReadState(StateTable, TipPosition, DigitizerCommandNames::GetTipPosition); provided->AddCommandReadState(StateTable, TipOrientation, DigitizerCommandNames::GetTipOrientation); provided->AddCommandReadState(StateTable, TipOrientationUnitVector, DigitizerCommandNames::GetTipOrientationUnitVector); provided->AddCommandReadState(StateTable, ButtonState, DigitizerCommandNames::GetButtonState); provided->AddCommandReadState(StateTable, JointReadings, DigitizerCommandNames::GetJointReadings); provided->AddCommandRead(&mtsMicroScribeDigitizer::GetDigitizerInfo, this, DigitizerCommandNames::GetDigitizerInfo); provided->AddEventVoid(EventButton1Up, DigitizerEventNames::EventButton1Up); provided->AddEventVoid(EventButton1Down, DigitizerEventNames::EventButton1Down); provided->AddEventVoid(EventButton2Up, DigitizerEventNames::EventButton2Up); provided->AddEventVoid(EventButton2Down, DigitizerEventNames::EventButton2Down); provided->AddEventVoid(EventDigitizerConnected, DigitizerEventNames::EventDigitizerConnected); provided->AddEventVoid(EventDigitizerDisconnected, DigitizerEventNames::EventDigitizerDisconnected); } // Device initialization if (ARM_SUCCESS != ArmStart(NULL)) { // Retry after calling ArmEnd() ArmEnd(); if (ARM_SUCCESS != ArmStart(NULL)) { cmnThrow("mtsMicroScribeDigitizer: Startup: Failed to initialize MicroScribe Digitizer"); } } // Custom error handler registration if (ARM_SUCCESS != ArmSetErrorHandlerFunction(NO_HCI_HANDLER, ErrorHandler_NoHCI)) { cmnThrow("mtsMicroScribeDigitizer: Startup: Failed to register error handler: NO_HCI_HANDLER"); } if (ARM_SUCCESS != ArmSetErrorHandlerFunction(BAD_PORT_HANDLER, ErrorHandler_BadPort)) { cmnThrow("mtsMicroScribeDigitizer: Startup: Failed to register error handler: BAD_PORT_HANDLER"); } if (ARM_SUCCESS != ArmSetErrorHandlerFunction(CANT_OPEN_HANDLER, ErrorHandler_CantOpen)) { cmnThrow("mtsMicroScribeDigitizer: Startup: Failed to register error handler: CANT_OPEN_HANDLER"); } if (ARM_SUCCESS != ArmSetErrorHandlerFunction(CANT_BEGIN_HANDLER, ErrorHandler_CantBegin)) { cmnThrow("mtsMicroScribeDigitizer: Startup: Failed to register error handler: CANT_BEGIN_HANDLER"); } }
void mtsAtracsysFusionTrack::Construct(void) { Internals = new mtsAtracsysFusionTrackInternals(); StateTable.AddData(NumberOfThreeDFiducials, "NumberOfThreeDFiducials"); StateTable.AddData(ThreeDFiducialPosition, "ThreeDFiducialPosition"); mtsInterfaceProvided * provided = AddInterfaceProvided("Controller"); if (provided) { // provided->AddCommandReadState(StateTable, IsTracking, "IsTracking"); provided->AddCommandReadState(StateTable, NumberOfThreeDFiducials, "GetNumberOfThreeDFiducials"); provided->AddCommandReadState(StateTable, ThreeDFiducialPosition, "GetThreeDFiducialPosition"); } }
mtsIntuitiveResearchKitConsole::mtsIntuitiveResearchKitConsole(const std::string & componentName): mtsTaskFromSignal(componentName, 100), mConfigured(false), mSUJECMInterfaceRequired(0), mECMBaseFrameInterfaceProvided(0) { mtsInterfaceProvided * interfaceProvided = AddInterfaceProvided("Main"); if (interfaceProvided) { interfaceProvided->AddCommandWrite(&mtsIntuitiveResearchKitConsole::SetRobotsControlState, this, "SetRobotsControlState", std::string("")); interfaceProvided->AddCommandWrite(&mtsIntuitiveResearchKitConsole::TeleopEnable, this, "TeleopEnable", false); interfaceProvided->AddEventWrite(MessageEvents.Error, "Error", std::string("")); interfaceProvided->AddEventWrite(MessageEvents.Warning, "Warning", std::string("")); interfaceProvided->AddEventWrite(MessageEvents.Status, "Status", std::string("")); } mIOComponentName = "io"; mOperatorPresentComponent = mIOComponentName; mOperatorPresentInterface = "COAG"; }
mtsManagerComponentServer::mtsManagerComponentServer(mtsManagerGlobal * gcm) : mtsManagerComponentBase(mtsManagerComponentBase::GetNameOfManagerComponentServer()), GCM(gcm), InterfaceGCMFunctionMap("InterfaceGCMFunctionMap") { // Prevent this component from being created more than once // MJ: singleton can be implemented instead. static int instanceCount = 0; if (instanceCount != 0) { cmnThrow(std::runtime_error("Error in creating manager component server: it's already created")); } gcm->SetMCS(this); InterfaceGCMFunctionMap.SetOwner(*this); // For system-wide thread-safe logging mtsInterfaceProvided * provided = AddInterfaceProvided( mtsManagerComponentBase::InterfaceNames::InterfaceSystemLoggerProvided); if (provided) { provided->AddEventWrite(this->EventPrintLog, mtsManagerComponentBase::EventNames::PrintLog, mtsLogMessage()); } }
serverQtComponent::serverQtComponent(const std::string & componentName): mtsComponent(componentName), VoidCounter(0), ReadValue(0), QualifiedReadValue(0) { // create the cisstMultiTask interface with commands and events mtsInterfaceProvided * provided = AddInterfaceProvided("Provided"); if (provided) { provided->AddCommandVoid(&serverQtComponent::Void, this, "Void"); provided->AddCommandWrite(&serverQtComponent::Write, this, "Write"); provided->AddCommandRead(&serverQtComponent::Read, this, "Read"); provided->AddCommandQualifiedRead(&serverQtComponent::QualifiedRead, this, "QualifiedRead"); provided->AddEventVoid(EventVoid, "EventVoid"); provided->AddEventWrite(EventWrite, "EventWrite", mtsInt()); } // create the user interface ServerWidget.setupUi(&CentralWidget); MainWindow.setCentralWidget(&CentralWidget); MainWindow.setWindowTitle(QString::fromStdString(componentName)); MainWindow.show(); // connect Qt signals to slots QObject::connect(this, SIGNAL(VoidQSignal(int)), ServerWidget.VoidValue, SLOT(setNum(int))); QObject::connect(this, SIGNAL(WriteQSignal(int)), ServerWidget.WriteValue, SLOT(setNum(int))); QObject::connect(ServerWidget.ReadSlider, SIGNAL(valueChanged(int)), this, SLOT(ReadQSlot(int))); QObject::connect(ServerWidget.QualifiedReadSlider, SIGNAL(valueChanged(int)), this, SLOT(QualifiedReadQSlot(int))); QObject::connect(ServerWidget.EventVoidButton, SIGNAL(clicked()), this, SLOT(EventVoidQSlot())); QObject::connect(ServerWidget.EventWriteSlider, SIGNAL(sliderMoved(int)), this, SLOT(EventWriteQSlot(int))); }
void mtsIntuitiveResearchKitArm::Init(void) { mCounter = 0; IsGoalSet = false; SetState(mtsIntuitiveResearchKitArmTypes::DVRK_UNINITIALIZED); // initialize trajectory data JointGet.SetSize(NumberOfJoints()); JointVelocityGet.SetSize(NumberOfJoints()); JointSet.SetSize(NumberOfJoints()); JointSetParam.Goal().SetSize(NumberOfAxes()); JointTrajectory.Velocity.SetSize(NumberOfJoints()); JointTrajectory.Acceleration.SetSize(NumberOfJoints()); JointTrajectory.Start.SetSize(NumberOfJoints()); JointTrajectory.Goal.SetSize(NumberOfJoints()); JointTrajectory.GoalError.SetSize(NumberOfJoints()); JointTrajectory.GoalTolerance.SetSize(NumberOfJoints()); JointTrajectory.EndTime = 0.0; PotsToEncodersTolerance.SetSize(NumberOfAxes()); // initialize velocity CartesianVelocityGetParam.SetVelocityLinear(vct3(0.0)); CartesianVelocityGetParam.SetVelocityAngular(vct3(0.0)); CartesianVelocityGetParam.SetTimestamp(0.0); CartesianVelocityGetParam.SetValid(false); // base frame, mostly for cases where no base frame is set by user BaseFrame = vctFrm4x4::Identity(); BaseFrameValid = true; // cartesian position are timestamped using timestamps provided by PID CartesianGetParam.SetAutomaticTimestamp(false); CartesianGetDesiredParam.SetAutomaticTimestamp(false); this->StateTable.AddData(CartesianGetLocalParam, "CartesianPositionLocal"); this->StateTable.AddData(CartesianGetLocalDesiredParam, "CartesianPositionLocalDesired"); this->StateTable.AddData(CartesianGetParam, "CartesianPosition"); this->StateTable.AddData(CartesianGetDesiredParam, "CartesianPositionDesired"); this->StateTable.AddData(BaseFrame, "BaseFrame"); this->StateTable.AddData(JointGetParam, "JointPosition"); this->StateTable.AddData(JointGetDesired, "JointPositionDesired"); this->StateTable.AddData(CartesianVelocityGetParam, "CartesianVelocityGetParam"); this->StateTable.AddData(StateJointParam, "StateJoint"); this->StateTable.AddData(StateJointDesiredParam, "StateJointDesired"); // PID PIDInterface = AddInterfaceRequired("PID"); if (PIDInterface) { PIDInterface->AddFunction("Enable", PID.Enable); PIDInterface->AddFunction("GetPositionJoint", PID.GetPositionJoint); PIDInterface->AddFunction("GetPositionJointDesired", PID.GetPositionJointDesired); PIDInterface->AddFunction("GetStateJoint", PID.GetStateJoint); PIDInterface->AddFunction("GetStateJointDesired", PID.GetStateJointDesired); PIDInterface->AddFunction("SetPositionJoint", PID.SetPositionJoint); PIDInterface->AddFunction("SetCheckJointLimit", PID.SetCheckJointLimit); PIDInterface->AddFunction("GetVelocityJoint", PID.GetVelocityJoint); PIDInterface->AddFunction("EnableTorqueMode", PID.EnableTorqueMode); PIDInterface->AddFunction("SetTorqueJoint", PID.SetTorqueJoint); PIDInterface->AddFunction("SetTorqueOffset", PID.SetTorqueOffset); PIDInterface->AddFunction("EnableTrackingError", PID.EnableTrackingError); PIDInterface->AddFunction("SetTrackingErrorTolerances", PID.SetTrackingErrorTolerance); PIDInterface->AddEventHandlerWrite(&mtsIntuitiveResearchKitArm::JointLimitEventHandler, this, "JointLimit"); PIDInterface->AddEventHandlerWrite(&mtsIntuitiveResearchKitArm::ErrorEventHandler, this, "Error"); } // Robot IO IOInterface = AddInterfaceRequired("RobotIO"); if (IOInterface) { IOInterface->AddFunction("GetSerialNumber", RobotIO.GetSerialNumber); IOInterface->AddFunction("EnablePower", RobotIO.EnablePower); IOInterface->AddFunction("DisablePower", RobotIO.DisablePower); IOInterface->AddFunction("GetActuatorAmpStatus", RobotIO.GetActuatorAmpStatus); IOInterface->AddFunction("GetBrakeAmpStatus", RobotIO.GetBrakeAmpStatus); IOInterface->AddFunction("BiasEncoder", RobotIO.BiasEncoder); IOInterface->AddFunction("ResetSingleEncoder", RobotIO.ResetSingleEncoder); IOInterface->AddFunction("GetAnalogInputPosSI", RobotIO.GetAnalogInputPosSI); IOInterface->AddFunction("SetActuatorCurrent", RobotIO.SetActuatorCurrent); IOInterface->AddFunction("UsePotsForSafetyCheck", RobotIO.UsePotsForSafetyCheck); IOInterface->AddFunction("SetPotsToEncodersTolerance", RobotIO.SetPotsToEncodersTolerance); IOInterface->AddFunction("BrakeRelease", RobotIO.BrakeRelease); IOInterface->AddFunction("BrakeEngage", RobotIO.BrakeEngage); } // Setup Joints SUJInterface = AddInterfaceRequired("BaseFrame", MTS_OPTIONAL); if (SUJInterface) { SUJInterface->AddEventHandlerWrite(&mtsIntuitiveResearchKitArm::SetBaseFrame, this, "BaseFrameDesired"); SUJInterface->AddEventHandlerWrite(&mtsIntuitiveResearchKitArm::ErrorEventHandler, this, "Error"); } // Arm RobotInterface = AddInterfaceProvided("Robot"); if (RobotInterface) { // Get RobotInterface->AddCommandReadState(this->StateTable, JointGetParam, "GetPositionJoint"); RobotInterface->AddCommandReadState(this->StateTable, JointGetDesired, "GetPositionJointDesired"); RobotInterface->AddCommandReadState(this->StateTable, StateJointParam, "GetStateJoint"); RobotInterface->AddCommandReadState(this->StateTable, StateJointDesiredParam, "GetStateJointDesired"); RobotInterface->AddCommandReadState(this->StateTable, CartesianGetLocalParam, "GetPositionCartesianLocal"); RobotInterface->AddCommandReadState(this->StateTable, CartesianGetLocalDesiredParam, "GetPositionCartesianLocalDesired"); RobotInterface->AddCommandReadState(this->StateTable, CartesianGetParam, "GetPositionCartesian"); RobotInterface->AddCommandReadState(this->StateTable, CartesianGetDesiredParam, "GetPositionCartesianDesired"); RobotInterface->AddCommandReadState(this->StateTable, BaseFrame, "GetBaseFrame"); RobotInterface->AddCommandReadState(this->StateTable, CartesianVelocityGetParam, "GetVelocityCartesian"); // Set RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitArm::SetPositionJoint, this, "SetPositionJoint"); RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitArm::SetPositionGoalJoint, this, "SetPositionGoalJoint"); RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitArm::SetPositionCartesian, this, "SetPositionCartesian"); RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitArm::SetPositionGoalCartesian, this, "SetPositionGoalCartesian"); // Trajectory events RobotInterface->AddEventWrite(JointTrajectory.GoalReachedEvent, "GoalReached", bool()); // Robot State RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitArm::SetRobotControlState, this, "SetRobotControlState", std::string("")); RobotInterface->AddCommandRead(&mtsIntuitiveResearchKitArm::GetRobotControlState, this, "GetRobotControlState", std::string("")); // Human readable messages RobotInterface->AddEventWrite(MessageEvents.Status, "Status", std::string("")); RobotInterface->AddEventWrite(MessageEvents.Warning, "Warning", std::string("")); RobotInterface->AddEventWrite(MessageEvents.Error, "Error", std::string("")); RobotInterface->AddEventWrite(MessageEvents.RobotState, "RobotState", std::string("")); // Stats RobotInterface->AddCommandReadState(StateTable, StateTable.PeriodStats, "GetPeriodStatistics"); } }
bool mtsManagerComponentServer::AddInterfaceGCM(void) { // InterfaceGCM's required interface is not created here but is created // when a manager component client connects to the manager component // server. // See mtsManagerComponentServer::AddNewClientProcess() // for the creation of required interfaces. // Add provided interface to which InterfaceLCM's required interface connects. const std::string interfaceName = mtsManagerComponentBase::GetNameOfInterfaceGCMProvided(); mtsInterfaceProvided * provided = AddInterfaceProvided(interfaceName); if (!provided) { CMN_LOG_CLASS_INIT_ERROR << "AddInterfaceGCM: failed to add \"GCM\" provided interface: " << interfaceName << std::endl; return false; } provided->AddCommandWriteReturn(&mtsManagerComponentServer::InterfaceGCMCommands_ComponentCreate, this, mtsManagerComponentBase::CommandNames::ComponentCreate); provided->AddCommandWrite(&mtsManagerComponentServer::InterfaceGCMCommands_ComponentConfigure, this, mtsManagerComponentBase::CommandNames::ComponentConfigure); provided->AddCommandWrite(&mtsManagerComponentServer::InterfaceGCMCommands_ComponentConnect, this, mtsManagerComponentBase::CommandNames::ComponentConnect); provided->AddCommandWrite(&mtsManagerComponentServer::InterfaceGCMCommands_ComponentDisconnect, this, mtsManagerComponentBase::CommandNames::ComponentDisconnect); provided->AddCommandWrite(&mtsManagerComponentServer::InterfaceGCMCommands_ComponentStart, this, mtsManagerComponentBase::CommandNames::ComponentStart); provided->AddCommandWrite(&mtsManagerComponentServer::InterfaceGCMCommands_ComponentStop, this, mtsManagerComponentBase::CommandNames::ComponentStop); provided->AddCommandWrite(&mtsManagerComponentServer::InterfaceGCMCommands_ComponentResume, this, mtsManagerComponentBase::CommandNames::ComponentResume); provided->AddCommandQualifiedRead(&mtsManagerComponentServer::InterfaceGCMCommands_ComponentGetState, this, mtsManagerComponentBase::CommandNames::ComponentGetState); provided->AddCommandRead(&mtsManagerComponentServer::InterfaceGCMCommands_GetNamesOfProcesses, this, mtsManagerComponentBase::CommandNames::GetNamesOfProcesses); provided->AddCommandQualifiedRead(&mtsManagerComponentServer::InterfaceGCMCommands_GetNamesOfComponents, this, mtsManagerComponentBase::CommandNames::GetNamesOfComponents); provided->AddCommandQualifiedRead(&mtsManagerComponentServer::InterfaceGCMCommands_GetNamesOfInterfaces, this, mtsManagerComponentBase::CommandNames::GetNamesOfInterfaces); provided->AddCommandRead(&mtsManagerComponentServer::InterfaceGCMCommands_GetListOfConnections, this, mtsManagerComponentBase::CommandNames::GetListOfConnections); provided->AddCommandQualifiedRead(&mtsManagerComponentServer::InterfaceGCMCommands_GetListOfComponentClasses, this, mtsManagerComponentBase::CommandNames::GetListOfComponentClasses); provided->AddCommandQualifiedRead(&mtsManagerComponentServer::InterfaceGCMCommands_GetInterfaceProvidedDescription, this, mtsManagerComponentBase::CommandNames::GetInterfaceProvidedDescription); provided->AddCommandQualifiedRead(&mtsManagerComponentServer::InterfaceGCMCommands_GetInterfaceRequiredDescription, this, mtsManagerComponentBase::CommandNames::GetInterfaceRequiredDescription); provided->AddCommandQualifiedRead(&mtsManagerComponentServer::InterfaceGCMCommands_LoadLibrary, this, mtsManagerComponentBase::CommandNames::LoadLibrary); provided->AddCommandWrite(&mtsManagerComponentServer::InterfaceGCMCommands_PrintLog, this, mtsManagerComponentBase::CommandNames::PrintLog, MTS_COMMAND_NOT_QUEUED); provided->AddCommandWrite(&mtsManagerComponentServer::InterfaceGCMCommands_EnableLogForwarding, this, mtsManagerComponentBase::CommandNames::EnableLogForwarding); provided->AddCommandWrite(&mtsManagerComponentServer::InterfaceGCMCommands_DisableLogForwarding, this, mtsManagerComponentBase::CommandNames::DisableLogForwarding); provided->AddCommandQualifiedRead(&mtsManagerComponentServer::InterfaceGCMCommands_GetLogForwardingStates, this, mtsManagerComponentBase::CommandNames::GetLogForwardingStates); provided->AddCommandQualifiedRead(&mtsManagerComponentServer::InterfaceGCMCommands_GetAbsoluteTimeDiffs, this, mtsManagerComponentBase::CommandNames::GetAbsoluteTimeDiffs); provided->AddEventWrite(this->InterfaceGCMEvents_AddComponent, mtsManagerComponentBase::EventNames::AddComponent, mtsDescriptionComponent()); // MJ TODO: We may need RemoveComponent command as well //provided->AddEventWrite(this->InterfaceGCMEvents_AddComponent, // mtsManagerComponentBase::EventNames::AddComponent, mtsDescriptionComponent()); provided->AddEventWrite(this->InterfaceGCMEvents_AddConnection, mtsManagerComponentBase::EventNames::AddConnection, mtsDescriptionConnection()); provided->AddEventWrite(this->InterfaceGCMEvents_RemoveConnection, mtsManagerComponentBase::EventNames::RemoveConnection, mtsDescriptionConnection()); provided->AddEventWrite(this->InterfaceGCMEvents_ChangeState, mtsManagerComponentBase::EventNames::ChangeState, mtsComponentStateChange()); provided->AddEventVoid(this->InterfaceGCMEvents_MCSReady, mtsManagerComponentBase::EventNames::MCSReady); CMN_LOG_CLASS_INIT_VERBOSE << "AddInterfaceGCM: successfully added \"GCM\" interfaces" << std::endl; return true; }
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 mtsPIDQtWidget::Init() { PID.PositionJointGetParam.Position().SetSize(NumberOfAxis); PID.PositionJointGetDesired.SetSize(NumberOfAxis); PID.VelocityJointGetParam.Velocity().SetSize(NumberOfAxis); PID.EffortJoint.SetSize(NumberOfAxis); DesiredPosition.SetSize(NumberOfAxis); DesiredPosition.SetAll(0.0); DesiredEffort.SetSize(NumberOfAxis); DesiredEffort.SetAll(0.0); UnitFactor.SetSize(NumberOfAxis); UnitFactor.SetAll(1.0); DirectControl = false; PlotIndex = 0; logsEnabled = false; logEntryIndex = 0; // Setup cisst interface mtsInterfaceRequired * interfaceRequired = AddInterfaceRequired("Controller"); if (interfaceRequired) { interfaceRequired->AddFunction("ResetController", PID.ResetController); interfaceRequired->AddFunction("Enable", PID.Enable); interfaceRequired->AddFunction("EnableIO", PID.EnableIO); interfaceRequired->AddFunction("EnableLogs", PID.EnableLogs); interfaceRequired->AddFunction("EnableTorqueMode", PID.EnableTorqueMode); interfaceRequired->AddFunction("SetPositionJoint", PID.SetPositionJoint); interfaceRequired->AddFunction("SetTorqueJoint", PID.SetEffortJoint); interfaceRequired->AddFunction("GetPositionJoint", PID.GetPositionJoint); interfaceRequired->AddFunction("GetVelocityJoint", PID.GetVelocityJoint); interfaceRequired->AddFunction("GetPositionJointDesired", PID.GetPositionJointDesired); interfaceRequired->AddFunction("GetEffortJointDesired", PID.GetEffortJointDesired); interfaceRequired->AddFunction("GetPeriodStatistics", PID.GetPeriodStatistics); interfaceRequired->AddFunction("GetJointType", PID.GetJointType); interfaceRequired->AddFunction("GetPGain", PID.GetPGain); interfaceRequired->AddFunction("GetDGain", PID.GetDGain); interfaceRequired->AddFunction("GetIGain", PID.GetIGain); interfaceRequired->AddFunction("SetPGain", PID.SetPGain); interfaceRequired->AddFunction("SetDGain", PID.SetDGain); interfaceRequired->AddFunction("SetIGain", PID.SetIGain); // Events interfaceRequired->AddEventHandlerWrite(&mtsPIDQtWidget::JointLimitEventHandler, this, "JointLimit"); interfaceRequired->AddEventHandlerWrite(&mtsPIDQtWidget::ErrorEventHandler, this, "Error"); interfaceRequired->AddEventHandlerWrite(&mtsPIDQtWidget::EnableEventHandler, this, "Enabled"); } if(usingSimulink) { mtsInterfaceRequired * simulinkQtInterfaceRequired = AddInterfaceRequired("SimulinkQtInterfacePIDCommand"); if (simulinkQtInterfaceRequired) { simulinkQtInterfaceRequired->AddFunction("EnableSimulinkWidgetFromPID", SimulinkQtWidget.EnableWidget); simulinkQtInterfaceRequired->AddFunction("EnableSimulinkFromPID", SimulinkQtWidget.Enable); simulinkQtInterfaceRequired->AddFunction("EnableSimulinkLogsFromPID", SimulinkQtWidget.EnableLogs); //the below means COMMENCED, as in, is Simulink in control? simulinkQtInterfaceRequired->AddFunction("IsSimulinkEnabled", SimulinkQtWidget.IsEnabled); } mtsInterfaceProvided * simulinkQtInterfaceProvided = AddInterfaceProvided("SimulinkQtInterfaceSimulinkCommand"); if (simulinkQtInterfaceProvided) { simulinkQtInterfaceProvided->AddCommandWrite(&mtsPIDQtWidget::EnablePIDFromSimulinkQt , this, "EnablePIDFromSimulink", mtsBool()); simulinkQtInterfaceProvided->AddCommandWrite(&mtsPIDQtWidget::EnableLogsFromSimulinkQt , this, "EnablePIDLogsFromSimulink", mtsBool()); } } setupUi(); startTimer(TimerPeriodInMilliseconds); // ms }
mtsMedtronicStealthlink::Tool * mtsMedtronicStealthlink::AddTool(const std::string & stealthName, const std::string & interfaceName) { // First, check if tool has already been added Tool * tool = FindTool(stealthName); if (tool) { if (tool->GetInterfaceName() == interfaceName) { CMN_LOG_CLASS_RUN_WARNING << "AddTool: tool " << stealthName << " already exists with interface " << interfaceName << std::endl; return tool; } // We could support having the same tool in multiple interfaces, but we would need to maintain // an array of CurrentTools, or loop through the entire Tools list in the Run method (to assign the // MarkerPosition and TooltipPosition). CMN_LOG_CLASS_RUN_ERROR << "AddTool: tool " << stealthName << " already exists in interface " << tool->GetInterfaceName() << ", could not create new interface " << interfaceName << std::endl; return 0; } // Next, check if interface has already been added mtsInterfaceProvided * provided = GetInterfaceProvided(interfaceName); if (provided) { CMN_LOG_CLASS_RUN_ERROR << "AddTool: interface " << interfaceName << " already exists." << std::endl; return 0; } // Create the tool and add it to the list tool = new Tool (stealthName, interfaceName); Tools.push_back(tool); DataMapContainerInsertReturnValue dataMapInsertReturn; MNavStealthLink::DataItem * newItem = 0; myGenericObject * newObject = tool; if (strcmp(interfaceName.c_str(),"Frame") == 0){ MNavStealthLink::Frame * newFrame = new MNavStealthLink::Frame(); newFrame->name = stealthName; newItem = newFrame; } else if (strcmp(interfaceName.c_str(),"Tool") == 0 ){ MNavStealthLink::Instrument * newTool = new MNavStealthLink::Instrument(); newTool->name = stealthName; newItem = newTool; }else{ //warning not a Frame or Tool assuming Tool MNavStealthLink::Instrument * newTool = new MNavStealthLink::Instrument(); newTool->name = stealthName; newItem = newTool; } dataMapInsertReturn = myDataMap.insert(DataMapContainerItem(newItem,newObject)); if (dataMapInsertReturn.second){ CMN_LOG_CLASS_RUN_VERBOSE << "AddTool: adding " << stealthName << " to interface " << interfaceName << std::endl; provided = AddInterfaceProvided(interfaceName); dataMapInsertReturn.first->second->ConfigureInterfaceProvided(provided); } else { CMN_LOG_CLASS_INIT_ERROR << "AddTool: Failed to add tool to the DataMap" << std::endl; } return tool; }
bool mtsAtracsysFusionTrack::AddToolIni(const std::string & toolName, const std::string & fileName) { // check if this tool already exists mtsAtracsysFusionTrackTool * tool = Tools.GetItem(toolName); if (tool) { CMN_LOG_CLASS_INIT_ERROR << "AddToolIni: " << tool->Name << " already exists" << std::endl; return false; } // make sure we can find and load this tool ini file ftkError error; ftkGeometry geometry; switch (loadGeometry(Internals->Library, Internals->Device, fileName, geometry)) { case 1: CMN_LOG_CLASS_INIT_VERBOSE << "AddToolIni: loaded " << fileName << " from installation directory" << std::endl; case 0: error = ftkSetGeometry(Internals->Library, Internals->Device, &geometry); if (error != FTK_OK) { CMN_LOG_CLASS_INIT_ERROR << "AddToolIni: unable to set geometry for tool " << fileName << " (" << this->GetName() << ")" << std::endl; return false; } break; default: CMN_LOG_CLASS_INIT_ERROR << "AddToolIni: error, cannot load geometry file " << fileName << std::endl; return false; } // make sure there is no such geometry Id yet const mtsAtracsysFusionTrackInternals::GeometryIdToToolMap::const_iterator toolIterator = Internals->GeometryIdToTool.find(geometry.geometryId); if (toolIterator != Internals->GeometryIdToTool.end()) { CMN_LOG_CLASS_INIT_ERROR << "AddToolIni: error, found an existing tool with the same Id " << geometry.geometryId << " for " << fileName << std::endl; return false; } // finally create a cisst tool structure tool = new mtsAtracsysFusionTrackTool(toolName); // create an interface for tool tool->Interface = AddInterfaceProvided(toolName); if (!tool->Interface) { CMN_LOG_CLASS_INIT_ERROR << "AddToolIni: " << tool->Name << " already exists" << std::endl; delete tool; return false; } // regiter newly created tool this->Tools.AddItem(toolName, tool); Internals->GeometryIdToTool[geometry.geometryId] = tool; // add data for this tool and populate tool interface tool->StateTable.SetAutomaticAdvance(false); this->AddStateTable(&(tool->StateTable)); tool->StateTable.AddData(tool->Position, "Position"); tool->StateTable.AddData(tool->RegistrationError, "RegistrationError"); tool->Interface->AddCommandReadState(tool->StateTable, tool->Position, "GetPositionCartesian"); tool->Interface->AddCommandReadState(tool->StateTable, tool->RegistrationError, "GetRegistrationError"); tool->Interface->AddCommandReadState(tool->StateTable, tool->StateTable.PeriodStats, "GetPeriodStatistics"); return true; }
void mtsIntuitiveResearchKitMTM::Init(void) { logsEnabled = false; logEntryIndex = 0; SetState(mtsIntuitiveResearchKitMTMTypes::MTM_UNINITIALIZED); RobotType = MTM_NULL; SetMTMType(); // initialize gripper state GripperClosed = false; // initialize trajectory data JointGet.SetSize(NumberOfJoints); JointSet.SetSize(NumberOfJoints); JointSetParam.Goal().SetSize(NumberOfJoints + 1); // PID treats gripper as joint JointTrajectory.Velocity.SetSize(NumberOfJoints); JointTrajectory.Velocity.SetAll(720.0 * cmnPI_180); // degrees per second JointTrajectory.Acceleration.SetSize(NumberOfJoints); JointTrajectory.Acceleration.SetAll(720.0 * cmnPI_180); JointTrajectory.Start.SetSize(NumberOfJoints); JointTrajectory.Goal.SetSize(NumberOfJoints); JointTrajectory.GoalError.SetSize(NumberOfJoints); JointTrajectory.GoalTolerance.SetSize(NumberOfJoints); JointTrajectory.GoalTolerance.SetAll(3.0 * cmnPI / 180.0); // hard coded to 3 degrees JointTrajectory.EndTime = 0.0; this->StateTable.AddData(CartesianGetParam, "CartesianPosition"); this->StateTable.AddData(CartesianGetDesiredParam, "CartesianDesired"); this->StateTable.AddData(JointGetParam, "JointPosition"); this->StateTable.AddData(GripperPosition, "GripperAngle"); // setup cisst interfaces mtsInterfaceRequired * interfaceRequired; interfaceRequired = AddInterfaceRequired("PID"); if (interfaceRequired) { interfaceRequired->AddFunction("Enable", PID.Enable); interfaceRequired->AddFunction("EnableTorqueMode", PID.EnableTorqueMode); interfaceRequired->AddFunction("GetPositionJoint", PID.GetPositionJoint); interfaceRequired->AddFunction("GetPositionJointDesired", PID.GetPositionJointDesired); interfaceRequired->AddFunction("SetPositionJoint", PID.SetPositionJoint); interfaceRequired->AddFunction("SetTorqueJoint", PID.SetTorqueJoint); interfaceRequired->AddFunction("SetCheckJointLimit", PID.SetCheckJointLimit); interfaceRequired->AddFunction("SetTorqueOffset", PID.SetTorqueOffset); } if(usingSimulink) { mtsInterfaceRequired * simulinkInterfaceRequired = AddInterfaceRequired("SimulinkControlCommand"); if (simulinkInterfaceRequired) { simulinkInterfaceRequired->AddFunction("SignalTrajEvent", SimulinkController.SignalTrajectoryRunning); simulinkInterfaceRequired->AddFunction("SetPositionJoint", SimulinkController.SetDesiredJointPosition); simulinkInterfaceRequired->AddFunction("SetPositionCartDes", SimulinkController.SetDesiredCartesianPosition); simulinkInterfaceRequired->AddFunction("GetSimControllerType", SimulinkController.GetControllerTypeIsJoint); } } // Robot IO interfaceRequired = AddInterfaceRequired("RobotIO"); if (interfaceRequired) { interfaceRequired->AddFunction("EnablePower", RobotIO.EnablePower); interfaceRequired->AddFunction("DisablePower", RobotIO.DisablePower); interfaceRequired->AddFunction("GetActuatorAmpStatus", RobotIO.GetActuatorAmpStatus); interfaceRequired->AddFunction("BiasEncoder", RobotIO.BiasEncoder); interfaceRequired->AddFunction("SetActuatorCurrent", RobotIO.SetActuatorCurrent); interfaceRequired->AddFunction("UsePotsForSafetyCheck", RobotIO.UsePotsForSafetyCheck); interfaceRequired->AddFunction("SetPotsToEncodersTolerance", RobotIO.SetPotsToEncodersTolerance); interfaceRequired->AddFunction("ResetSingleEncoder", RobotIO.ResetSingleEncoder); interfaceRequired->AddFunction("GetAnalogInputPosSI", RobotIO.GetAnalogInputPosSI); } mtsInterfaceProvided * interfaceProvided = AddInterfaceProvided("Robot"); if (interfaceProvided) { // Cartesian interfaceProvided->AddCommandReadState(this->StateTable, JointGetParam, "GetPositionJoint"); interfaceProvided->AddCommandReadState(this->StateTable, CartesianGetParam, "GetPositionCartesian"); interfaceProvided->AddCommandReadState(this->StateTable, CartesianGetDesiredParam, "GetPositionCartesianDesired"); interfaceProvided->AddCommandWrite(&mtsIntuitiveResearchKitMTM::SetPositionCartesian, this, "SetPositionCartesian"); interfaceProvided->AddCommandWrite(&mtsIntuitiveResearchKitMTM::SetWrench, this, "SetWrench"); interfaceProvided->AddCommandRead(&mtsIntuitiveResearchKitMTM::GetRobotCartVelFromJacobian, this, "GetRobotCartVelFromJacobian"); interfaceProvided->AddCommandRead(&mtsIntuitiveResearchKitMTM::GetJointTorqueFromForceTorque, this, "GetJointTorqueFromForceTorque"); // Gripper interfaceProvided->AddCommandReadState(this->StateTable, GripperPosition, "GetGripperPosition"); interfaceProvided->AddEventVoid(EventTriggers.GripperPinch, "GripperPinchEvent"); interfaceProvided->AddEventWrite(EventTriggers.GripperClosed, "GripperClosedEvent", true); // Robot State interfaceProvided->AddCommandWrite(&mtsIntuitiveResearchKitMTM::SetRobotControlState, this, "SetRobotControlState", std::string("")); interfaceProvided->AddCommandRead(&mtsIntuitiveResearchKitMTM::GetRobotControlState, this, "GetRobotControlState", std::string("")); interfaceProvided->AddEventWrite(EventTriggers.RobotStatusMsg, "RobotStatusMsg", std::string("")); interfaceProvided->AddEventWrite(EventTriggers.RobotErrorMsg, "RobotErrorMsg", std::string("")); // Stats interfaceProvided->AddCommandReadState(StateTable, StateTable.PeriodStats, "GetPeriodStatistics"); } }
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); } }
SetPoints( const std::string& robotfilename, const vctFrame4x4<double>& Rtw0, const vctDynamicVector<double>& qinit ) : mtsTaskPeriodic( "Robot", 0.01, true ), manipulator( NULL ), q( qinit ), qready( qinit ), t( 0.0 ), ctl( NULL ), mtsEnabled( false ), online( false ), input( NULL ), output( NULL ){ manipulator = new robManipulator( robotfilename, Rtw0 ); Rt = manipulator->ForwardKinematics( q ); x = Rt[0][3]; qready[0] = 0.0; qready[1] = -cmnPI/2.0; qready[2] = 0.0; qready[3] = cmnPI/2.0; qready[4] = 0.0; qready[5] = -cmnPI/2.0; qready[6] = 0.0; // The control interface: to change the state of the component ctl = AddInterfaceProvided( "Control" ); if( ctl ){ StateTable.AddData( mtsEnabled, "Enabled" ); ctl->AddCommandWriteState( StateTable, mtsEnabled, "Enable" ); } else{ CMN_LOG_RUN_ERROR << "Failed to create interface Control for "<< GetName() << std::endl; } input = AddInterfaceProvided( "Input" ); if( input ){ StateTable.AddData( prmCommand, "Enabled" ); input->AddCommandWriteState(StateTable,prmCommand,"SetPositionCartesian"); } else{ CMN_LOG_RUN_ERROR << "Failed to create interface Input for " << GetName() << std::endl; } outputSE3 = AddInterfaceProvided( "OutputSE3" ); if( outputSE3 ){ StateTable.AddData( prmTelemetry, "Telemetry" ); outputSE3->AddCommandReadState( StateTable, prmTelemetry, "GetPositionCartesian" ); } output = AddInterfaceRequired( "Output" ); if( output ){ output->AddFunction( "SetPositionCartesian", SetPositionCartesian ); } else{ CMN_LOG_RUN_ERROR << "Failed to create interface Output for " << GetName() << std::endl; } }
void mtsIntuitiveResearchKitPSM::Init(void) { IsCartesianGoalSet = false; Counter = 0; SetState(PSM_UNINITIALIZED); DesiredOpenAngle = 0 * cmnPI_180; // initialize trajectory data JointCurrent.SetSize(NumberOfJoints); JointDesired.SetSize(NumberOfJoints); JointDesiredParam.Goal().SetSize(NumberOfJoints); JointTrajectory.Start.SetSize(NumberOfJoints); JointTrajectory.Velocity.SetSize(NumberOfJoints); JointTrajectory.Acceleration.SetSize(NumberOfJoints); JointTrajectory.Goal.SetSize(NumberOfJoints); JointTrajectory.GoalError.SetSize(NumberOfJoints); JointTrajectory.GoalTolerance.SetSize(NumberOfJoints); JointTrajectory.GoalTolerance.SetAll(3.0 * cmnPI / 180.0); // hard coded to 3 degrees JointTrajectory.Zero.SetSize(NumberOfJoints); EngagingJointSet.SetSize(NumberOfJoints); this->StateTable.AddData(CartesianCurrentParam, "CartesianPosition"); this->StateTable.AddData(JointCurrentParam, "JointPosition"); // setup CISST Interface mtsInterfaceRequired * interfaceRequired; interfaceRequired = AddInterfaceRequired("PID"); if (interfaceRequired) { interfaceRequired->AddFunction("Enable", PID.Enable); interfaceRequired->AddFunction("GetPositionJoint", PID.GetPositionJoint); interfaceRequired->AddFunction("GetPositionJointDesired", PID.GetPositionJointDesired); interfaceRequired->AddFunction("SetPositionJoint", PID.SetPositionJoint); interfaceRequired->AddFunction("SetCheckJointLimit", PID.SetCheckJointLimit); } // Robot IO interfaceRequired = AddInterfaceRequired("RobotIO"); if (interfaceRequired) { interfaceRequired->AddFunction("EnablePower", RobotIO.EnablePower); interfaceRequired->AddFunction("DisablePower", RobotIO.DisablePower); interfaceRequired->AddFunction("GetAmpStatus", RobotIO.GetAmpStatus); interfaceRequired->AddFunction("BiasEncoder", RobotIO.BiasEncoder); interfaceRequired->AddFunction("SetMotorCurrent", RobotIO.SetMotorCurrent); } // Event Adapter engage: digital input button event from PSM interfaceRequired = AddInterfaceRequired("Adapter"); if (interfaceRequired) { Adapter.IsPresent = false; interfaceRequired->AddFunction("GetButton", Adapter.GetButton); interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitPSM::EventHandlerAdapter, this, "Button"); } // Event Tool engage: digital input button event from PSM interfaceRequired = AddInterfaceRequired("Tool"); if (interfaceRequired) { Tool.IsPresent = false; interfaceRequired->AddFunction("GetButton", Tool.GetButton); interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitPSM::EventHandlerTool, this, "Button"); } // ManipClutch: digital input button event from PSM interfaceRequired = AddInterfaceRequired("ManipClutch"); if (interfaceRequired) { interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitPSM::EventHandlerManipClutch, this, "Button"); } // SUJClutch: digital input button event from PSM interfaceRequired = AddInterfaceRequired("SUJClutch"); if (interfaceRequired) { interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitPSM::EventHandlerSUJClutch, this, "Button"); } mtsInterfaceProvided * interfaceProvided = AddInterfaceProvided("Robot"); if (interfaceProvided) { interfaceProvided->AddCommandReadState(this->StateTable, JointCurrentParam, "GetPositionJoint"); interfaceProvided->AddCommandReadState(this->StateTable, CartesianCurrentParam, "GetPositionCartesian"); interfaceProvided->AddCommandWrite(&mtsIntuitiveResearchKitPSM::SetPositionCartesian, this, "SetPositionCartesian"); interfaceProvided->AddCommandWrite(&mtsIntuitiveResearchKitPSM::SetOpenAngle, this, "SetOpenAngle"); interfaceProvided->AddCommandWrite(&mtsIntuitiveResearchKitPSM::SetRobotControlState, this, "SetRobotControlState", std::string("")); interfaceProvided->AddEventWrite(EventTriggers.RobotStatusMsg, "RobotStatusMsg", std::string("")); interfaceProvided->AddEventWrite(EventTriggers.RobotErrorMsg, "RobotErrorMsg", std::string("")); interfaceProvided->AddEventWrite(EventTriggers.ManipClutch, "ManipClutchBtn", prmEventButton()); interfaceProvided->AddEventWrite(EventTriggers.SUJClutch, "SUJClutchBtn", prmEventButton()); // Stats interfaceProvided->AddCommandReadState(StateTable, StateTable.PeriodStats, "GetPeriodStatistics"); } }
void mtsIntuitiveResearchKitConsole::Configure(const std::string & filename) { mConfigured = false; std::ifstream jsonStream; jsonStream.open(filename.c_str()); Json::Value jsonConfig, jsonValue; Json::Reader jsonReader; if (!jsonReader.parse(jsonStream, jsonConfig)) { CMN_LOG_CLASS_INIT_ERROR << "Configure: failed to parse configuration\n" << jsonReader.getFormattedErrorMessages(); this->mConfigured = false; return; } // extract path of main json config file to search other files relative to it cmnPath configPath(cmnPath::GetWorkingDirectory()); std::string fullname = configPath.Find(filename); std::string configDir = fullname.substr(0, fullname.find_last_of('/')); configPath.Add(configDir); // IO default settings double periodIO = 0.5 * cmn_ms; int firewirePort = 0; // get user preferences jsonValue = jsonConfig["io"]; if (!jsonValue.empty()) { CMN_LOG_CLASS_INIT_VERBOSE << "Configure: looking for user provided io:period and io:port" << std::endl; jsonValue = jsonConfig["io"]["period"]; if (!jsonValue.empty()) { periodIO = jsonValue.asDouble(); } jsonValue = jsonConfig["io"]["port"]; if (!jsonValue.empty()) { firewirePort = jsonValue.asInt(); } } else { CMN_LOG_CLASS_INIT_VERBOSE << "Configure: using default io:period and io:port" << std::endl; } CMN_LOG_CLASS_INIT_VERBOSE << "Configure: period IO is " << periodIO << std::endl << "Configure: FireWire port is " << firewirePort << std::endl; // create IO mtsRobotIO1394 * io = new mtsRobotIO1394(mIOComponentName, periodIO, firewirePort); const Json::Value arms = jsonConfig["arms"]; for (unsigned int index = 0; index < arms.size(); ++index) { if (!ConfigureArmJSON(arms[index], io->GetName(), configPath)) { CMN_LOG_CLASS_INIT_ERROR << "Configure: failed to configure arms[" << index << "]" << std::endl; return; } } // loop over all arms to configure IO only const ArmList::iterator end = mArms.end(); ArmList::iterator iter; for (iter = mArms.begin(); iter != end; ++iter) { std::string ioConfig = iter->second->mIOConfigurationFile; if (ioConfig != "") { io->Configure(ioConfig); } } mtsComponentManager::GetInstance()->AddComponent(io); // now can configure PID and Arms for (iter = mArms.begin(); iter != end; ++iter) { const std::string pidConfig = iter->second->mPIDConfigurationFile; if (pidConfig != "") { iter->second->ConfigurePID(pidConfig); } const std::string armConfig = iter->second->mArmConfigurationFile; if (armConfig != "") { iter->second->ConfigureArm(iter->second->mType, armConfig); } } bool hasSUJ = false; bool hasECM = false; // now load all PSM teleops const Json::Value psmTeleops = jsonConfig["psm-teleops"]; for (unsigned int index = 0; index < psmTeleops.size(); ++index) { if (!ConfigurePSMTeleopJSON(psmTeleops[index])) { CMN_LOG_CLASS_INIT_ERROR << "Configure: failed to configure psm-teleops[" << index << "]" << std::endl; return; } } // see which event is used for operator present // find name of button event used to detect if operator is present mOperatorPresentComponent = jsonConfig["operator-present"]["component"].asString(); mOperatorPresentInterface = jsonConfig["operator-present"]["interface"].asString(); //set defaults if (mOperatorPresentComponent == "") { mOperatorPresentComponent = mIOComponentName; } if (mOperatorPresentInterface == "") { mOperatorPresentInterface = "COAG"; } // look for footpedals in json config jsonValue = jsonConfig["io"]["has-footpedals"]; if (jsonValue.empty()) { mHasFootpedals = false; } else { mHasFootpedals = jsonValue.asBool(); } // if we have any teleoperation component, we need to add the interfaces for the foot pedals if (mTeleops.size() > 0) { mHasFootpedals = true; } if (mHasFootpedals) { this->AddFootpedalInterfaces(); } // interface to ecm to get ECM frame and then push to PSM SUJs as base frame mtsInterfaceRequired * ecmArmInterface = 0; for (iter = mArms.begin(); iter != end; ++iter) { if (iter->second->mType == Arm::ARM_ECM) { hasECM = true; ecmArmInterface = iter->second->ArmInterfaceRequired; } else if (iter->second->mType == Arm::ARM_SUJ) { hasSUJ = true; } } // add required and provided interfaces to grab positions from ECM SUJ and ECM if (hasSUJ && hasECM) { mSUJECMInterfaceRequired = AddInterfaceRequired("BaseFrame"); if (mSUJECMInterfaceRequired) { mSUJECMInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::SUJECMBaseFrameHandler, this, "BaseFrameDesired"); } if (ecmArmInterface) { ecmArmInterface->AddFunction("GetPositionCartesianLocal", mGetPositionCartesianLocalFromECM); } else { CMN_LOG_CLASS_INIT_VERBOSE << "Configure: arm interface not yet added for ECM" << std::endl; } mECMBaseFrameInterfaceProvided = AddInterfaceProvided("ECMBaseFrame"); if (mECMBaseFrameInterfaceProvided) { mECMBaseFrameInterfaceProvided->AddEventWrite(mECMBaseFrameEvent, "BaseFrameDesired", prmPositionCartesianGet()); } } // connect arm SUJ clutch button to SUJ if (hasSUJ) { for (iter = mArms.begin(); iter != end; ++iter) { Arm * arm = iter->second; if ((arm->mType == Arm::ARM_ECM) || (arm->mType == Arm::ARM_PSM)) { arm->SUJInterfaceRequiredFromIO = this->AddInterfaceRequired("SUJ-" + arm->Name() + "-IO"); arm->SUJInterfaceRequiredFromIO->AddEventHandlerWrite(&Arm::SUJClutchEventHandlerFromIO, arm, "Button"); arm->SUJInterfaceRequiredToSUJ = this->AddInterfaceRequired("SUJ-" + arm->Name()); arm->SUJInterfaceRequiredToSUJ->AddFunction("Clutch", arm->SUJClutch); } else { arm->SUJInterfaceRequiredFromIO = 0; arm->SUJInterfaceRequiredToSUJ = 0; } } } mConfigured = true; }