void mtsIntuitiveResearchKitPSM::Init(void) { // main initialization from base type mtsIntuitiveResearchKitArm::Init(); // initialize trajectory data JointTrajectory.Velocity.SetAll(90.0 * cmnPI_180); // degrees per second JointTrajectory.Velocity.Element(2) = 0.2; // m per second JointTrajectory.Acceleration.SetAll(90.0 * cmnPI_180); JointTrajectory.Acceleration.Element(2) = 0.2; // m per second JointTrajectory.GoalTolerance.SetAll(3.0 * cmnPI / 180.0); // hard coded to 3 degrees // high values for engage adapter/tool until these use a proper trajectory generator PotsToEncodersTolerance.SetAll(100.0 * cmnPI_180); // 100 degrees for rotations PotsToEncodersTolerance.Element(2) = 20.0 * cmn_mm; // 20 mm // for tool/adapter engage procedure EngagingJointSet.SetSize(NumberOfJoints()); mtsInterfaceRequired * interfaceRequired; // Main interface should have been created by base class init CMN_ASSERT(RobotInterface); RobotInterface->AddEventWrite(ClutchEvents.ManipClutch, "ManipClutch", prmEventButton()); RobotInterface->AddEventWrite(ClutchEvents.SUJClutch, "SUJClutch", prmEventButton()); // 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"); } // Main interface should have been created by base class init CMN_ASSERT(RobotInterface); RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitPSM::SetJawPosition, this, "SetJawPosition"); // Initialize the optimizer Optimizer = new mtsIntuitiveResearchKitOptimizer(6); Optimizer->InitializeFollowVF(6, "FollowVFSlave", "CurrentSlaveKinematics", "DesiredSlaveKinematics"); }
void mtsIntuitiveResearchKitECM::Init(void) { // main initialization from base type mtsIntuitiveResearchKitArm::Init(); // initialize trajectory data JointTrajectory.Velocity.Assign(30.0 * cmnPI_180, // degrees per second 30.0 * cmnPI_180, 0.05, // m per second 20.0 * cmnPI_180); JointTrajectory.Acceleration.Assign(30.0 * cmnPI_180, 30.0 * cmnPI_180, 0.05, 30.0 * cmnPI_180); JointTrajectory.GoalTolerance.SetAll(3.0 * cmnPI / 180.0); // hard coded to 3 degrees PotsToEncodersTolerance.SetAll(10.0 * cmnPI_180); // 10 degrees for rotations PotsToEncodersTolerance.Element(2) = 20.0 * cmn_mm; // 20 mm mtsInterfaceRequired * interfaceRequired; // Main interface should have been created by base class init CMN_ASSERT(RobotInterface); RobotInterface->AddEventWrite(ClutchEvents.ManipClutch, "ManipClutch", prmEventButton()); // ManipClutch: digital input button event from ECM interfaceRequired = AddInterfaceRequired("ManipClutch"); if (interfaceRequired) { interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitECM::EventHandlerManipClutch, this, "Button"); } }
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; }
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 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); } }