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"); } }
void mtsIntuitiveResearchKitArm::RunHomingPower(void) { const double timeToPower = 3.0 * cmn_s; const double currentTime = this->StateTable.GetTic(); // first, request power to be turned on if (!HomingPowerRequested) { // in case we still have power but brakes are not engaged if (NumberOfBrakes() > 0) { RobotIO.BrakeEngage(); } // bias encoders based on pots RobotIO.BiasEncoder(); // use pots for redundancy if (UsePotsForSafetyCheck()) { RobotIO.SetPotsToEncodersTolerance(PotsToEncodersTolerance); RobotIO.UsePotsForSafetyCheck(true); } HomingTimer = currentTime; // make sure the PID is not sending currents PID.Enable(false); // pre-load the boards with zero current RobotIO.SetActuatorCurrent(vctDoubleVec(NumberOfAxes(), 0.0)); // enable power and set a flags to move to next step RobotIO.EnablePower(); HomingPowerRequested = true; MessageEvents.Status(this->GetName() + " power requested"); return; } // second, check status if (HomingPowerRequested && ((currentTime - HomingTimer) > timeToPower)) { // pre-load PID to make sure desired position has some reasonable values PID.GetPositionJoint(JointGetParam); // assign to a more convenient vctDoubleVec JointGet.Assign(JointGetParam.Position(), NumberOfJoints()); SetPositionJointLocal(JointGet); // check power status vctBoolVec actuatorAmplifiersStatus(NumberOfJoints()); RobotIO.GetActuatorAmpStatus(actuatorAmplifiersStatus); vctBoolVec brakeAmplifiersStatus(NumberOfBrakes()); if (NumberOfBrakes() > 0) { RobotIO.GetBrakeAmpStatus(brakeAmplifiersStatus); } if (actuatorAmplifiersStatus.All() && brakeAmplifiersStatus.All()) { MessageEvents.Status(this->GetName() + " power on"); if (NumberOfBrakes() > 0) { RobotIO.BrakeRelease(); } this->SetState(mtsIntuitiveResearchKitArmTypes::DVRK_HOMING_CALIBRATING_ARM); } else { // make sure the PID is not sending currents PID.Enable(false); RobotIO.SetActuatorCurrent(vctDoubleVec(NumberOfAxes(), 0.0)); RobotIO.DisablePower(); MessageEvents.Error(this->GetName() + " failed to enable power."); this->SetState(mtsIntuitiveResearchKitArmTypes::DVRK_UNINITIALIZED); } } }
void mtsIntuitiveResearchKitECM::SetState(const mtsIntuitiveResearchKitArmTypes::RobotStateType & newState) { CMN_LOG_CLASS_RUN_DEBUG << GetName() << ": SetState: new state " << mtsIntuitiveResearchKitArmTypes::RobotStateTypeToString(newState) << std::endl; switch (newState) { case mtsIntuitiveResearchKitArmTypes::DVRK_UNINITIALIZED: RobotIO.SetActuatorCurrent(vctDoubleVec(NumberOfAxes(), 0.0)); RobotIO.DisablePower(); PID.Enable(false); PID.SetCheckJointLimit(true); RobotState = newState; MessageEvents.Status(this->GetName() + " not initialized"); break; case mtsIntuitiveResearchKitArmTypes::DVRK_HOMING_BIAS_ENCODER: HomingBiasEncoderRequested = false; RobotState = newState; MessageEvents.Status(this->GetName() + " updating encoders based on potentiometers"); break; case mtsIntuitiveResearchKitArmTypes::DVRK_HOMING_POWERING: HomingTimer = 0.0; HomingPowerRequested = false; RobotState = newState; MessageEvents.Status(this->GetName() + " powering"); break; case mtsIntuitiveResearchKitArmTypes::DVRK_HOMING_CALIBRATING_ARM: HomingCalibrateArmStarted = false; RobotState = newState; this->MessageEvents.Status(this->GetName() + " calibrating arm"); break; case mtsIntuitiveResearchKitArmTypes::DVRK_READY: // when returning from manual mode, need to re-enable PID RobotState = newState; MessageEvents.Status(this->GetName() + " ready"); break; case mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_JOINT: case mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_GOAL_JOINT: if (this->RobotState < mtsIntuitiveResearchKitArmTypes::DVRK_READY) { MessageEvents.Error(this->GetName() + " is not ready"); return; } RobotState = newState; JointSet.Assign(JointGetDesired, this->NumberOfJoints()); if (newState == mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_JOINT) { IsGoalSet = false; MessageEvents.Status(this->GetName() + " position joint"); } else { JointTrajectory.EndTime = 0.0; MessageEvents.Status(this->GetName() + " position goal joint"); } break; case mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_CARTESIAN: case mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_GOAL_CARTESIAN: if (this->RobotState < mtsIntuitiveResearchKitArmTypes::DVRK_ARM_CALIBRATED) { MessageEvents.Error(this->GetName() + " is not calibrated"); return; } // check that the tool is inserted deep enough if (JointGet.Element(2) < 40.0 * cmn_mm) { MessageEvents.Error(this->GetName() + " can't start cartesian mode, make sure the endoscope is inserted past the cannula (joint 3 > 40 mm)"); } else { if (JointGet.Element(2) < 50.0 * cmn_mm) { MessageEvents.Warning(this->GetName() + " cartesian mode started close to RCM (joint 3 < 50 mm), joint 3 will be clamped at 40 mm to avoid moving inside cannula."); } RobotState = newState; if (newState == mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_CARTESIAN) { IsGoalSet = false; MessageEvents.Status(this->GetName() + " position cartesian"); } else { JointTrajectory.EndTime = 0.0; MessageEvents.Status(this->GetName() + " position goal cartesian"); } } break; case mtsIntuitiveResearchKitArmTypes::DVRK_CONSTRAINT_CONTROLLER_CARTESIAN: if (this->RobotState < mtsIntuitiveResearchKitArmTypes::DVRK_READY) { MessageEvents.Error(this->GetName() + " is not ready"); return; } // check that the tool is inserted deep enough if (JointGet.Element(2) < 80.0 * cmn_mm) { MessageEvents.Error(this->GetName() + " can't start constraint controller cartesian mode, make sure the tool is inserted past the cannula"); break; } RobotState = newState; IsGoalSet = false; MessageEvents.Status(this->GetName() + " constraint controller cartesian"); break; case mtsIntuitiveResearchKitArmTypes::DVRK_MANUAL: if (this->RobotState < mtsIntuitiveResearchKitArmTypes::DVRK_READY) { MessageEvents.Error(this->GetName() + " is not ready yet"); return; } // disable PID to allow manual move PID.Enable(false); RobotState = newState; MessageEvents.Status(this->GetName() + " in manual mode"); break; default: break; } // Emit event with current state MessageEvents.RobotState(mtsIntuitiveResearchKitArmTypes::RobotStateTypeToString(this->RobotState)); }
void mtsIntuitiveResearchKitPSM::SetState(const mtsIntuitiveResearchKitArmTypes::RobotStateType & newState) { CMN_LOG_CLASS_RUN_DEBUG << GetName() << ": SetState: new state " << mtsIntuitiveResearchKitArmTypes::RobotStateTypeToString(newState) << std::endl; switch (newState) { case mtsIntuitiveResearchKitArmTypes::DVRK_UNINITIALIZED: RobotIO.SetActuatorCurrent(vctDoubleVec(NumberOfAxes(), 0.0)); RobotIO.DisablePower(); PID.Enable(false); PID.SetCheckJointLimit(true); RobotState = newState; MessageEvents.Status(this->GetName() + " not initialized"); break; case mtsIntuitiveResearchKitArmTypes::DVRK_HOMING_POWERING: HomingTimer = 0.0; HomingPowerRequested = false; RobotState = newState; MessageEvents.Status(this->GetName() + " powering"); break; case mtsIntuitiveResearchKitArmTypes::DVRK_HOMING_CALIBRATING_ARM: HomingCalibrateArmStarted = false; RobotState = newState; this->MessageEvents.Status(this->GetName() + " calibrating arm"); break; case mtsIntuitiveResearchKitArmTypes::DVRK_ARM_CALIBRATED: RobotState = newState; this->MessageEvents.Status(this->GetName() + " arm calibrated"); // check if adpater is present and trigger new state Adapter.GetButton(Adapter.IsPresent); Adapter.IsPresent = !Adapter.IsPresent; if (Adapter.IsPresent) { SetState(mtsIntuitiveResearchKitArmTypes::DVRK_ENGAGING_ADAPTER); } break; case mtsIntuitiveResearchKitArmTypes::DVRK_ENGAGING_ADAPTER: EngagingAdapterStarted = false; if (this->RobotState < mtsIntuitiveResearchKitArmTypes::DVRK_ARM_CALIBRATED) { MessageEvents.Status(this->GetName() + " is not calibrated yet, will engage adapter later"); return; } // configure PID to fail in case of tracking error { PID.SetCheckJointLimit(false); vctDoubleVec tolerances(NumberOfJoints()); // first two rotations tolerances.Ref(2, 0).SetAll(10.0 * cmnPI_180); // 10 degrees // translation tolerances.Element(2) = 10.0 * cmn_mm; // 10 mm // tool/adapter gears tolerances.Ref(4, 3).SetAll(5.0 * cmnPI); // we request positions that can't be reached when the adapter/tool engage PID.SetTrackingErrorTolerance(tolerances); PID.EnableTrackingError(true); } // if the tool is present, the adapter is already engadged Tool.GetButton(Tool.IsPresent); Tool.IsPresent = !Tool.IsPresent; if (Tool.IsPresent) { SetState(mtsIntuitiveResearchKitArmTypes::DVRK_ADAPTER_ENGAGED); } else { RobotState = newState; this->MessageEvents.Status(this->GetName() + " engaging adapter"); } break; case mtsIntuitiveResearchKitArmTypes::DVRK_ADAPTER_ENGAGED: RobotState = newState; this->MessageEvents.Status(this->GetName() + " adapter engaged"); // check if tool is present and trigger new state Tool.GetButton(Tool.IsPresent); Tool.IsPresent = !Tool.IsPresent; if (Tool.IsPresent) { SetState(mtsIntuitiveResearchKitArmTypes::DVRK_ENGAGING_TOOL); } break; case mtsIntuitiveResearchKitArmTypes::DVRK_ENGAGING_TOOL: EngagingToolStarted = false; if (this->RobotState < mtsIntuitiveResearchKitArmTypes::DVRK_ADAPTER_ENGAGED) { MessageEvents.Status(this->GetName() + " adapter is not engaged yet, will engage tool later"); return; } RobotState = newState; this->MessageEvents.Status(this->GetName() + " engaging tool"); break; case mtsIntuitiveResearchKitArmTypes::DVRK_READY: { PID.SetCheckJointLimit(true); vctDoubleVec tolerances(NumberOfJoints()); // first two rotations tolerances.Ref(2, 0).SetAll(20.0 * cmnPI_180); // translation tolerances.Element(2) = 10.0 * cmn_mm; // 10 mm // shaft rotation tolerances.Element(3) = 120.0 * cmnPI_180; // tool orientation tolerances.Ref(2, 4).SetAll(35.0 * cmnPI_180); // gripper tolerances.Element(6) = 90.0 * cmnPI_180; // 90 degrees for gripper, until we change the master gripper matches tool angle PID.SetTrackingErrorTolerance(tolerances); PID.EnableTrackingError(true); // set tighter pots/encoder tolerances PotsToEncodersTolerance.SetAll(10.0 * cmnPI_180); // 10 degrees for rotations PotsToEncodersTolerance.Element(2) = 20.0 * cmn_mm; // 20 mm RobotIO.SetPotsToEncodersTolerance(PotsToEncodersTolerance); } RobotState = newState; MessageEvents.Status(this->GetName() + " ready"); break; case mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_JOINT: case mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_GOAL_JOINT: if (this->RobotState < mtsIntuitiveResearchKitArmTypes::DVRK_READY) { MessageEvents.Error(this->GetName() + " is not ready"); return; } RobotState = newState; JointSet.Assign(JointGetDesired, this->NumberOfJoints()); if (newState == mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_JOINT) { IsGoalSet = false; MessageEvents.Status(this->GetName() + " position joint"); } else { JointTrajectory.EndTime = 0.0; MessageEvents.Status(this->GetName() + " position goal joint"); } break; case mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_CARTESIAN: case mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_GOAL_CARTESIAN: if (this->RobotState < mtsIntuitiveResearchKitArmTypes::DVRK_ARM_CALIBRATED) { MessageEvents.Error(this->GetName() + " is not calibrated"); return; } // check that the tool is inserted deep enough if (JointGet.Element(2) < 80.0 * cmn_mm) { MessageEvents.Error(this->GetName() + " can't start cartesian mode, make sure the tool is inserted past the cannula"); break; } RobotState = newState; if (newState == mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_CARTESIAN) { IsGoalSet = false; MessageEvents.Status(this->GetName() + " position cartesian"); } else { JointTrajectory.EndTime = 0.0; MessageEvents.Status(this->GetName() + " position goal cartesian"); } break; case mtsIntuitiveResearchKitArmTypes::DVRK_CONSTRAINT_CONTROLLER_CARTESIAN: if (this->RobotState < mtsIntuitiveResearchKitArmTypes::DVRK_ARM_CALIBRATED) { MessageEvents.Error(this->GetName() + " is not calibrated"); return; } // check that the tool is inserted deep enough if (JointGet.Element(2) < 80.0 * cmn_mm) { MessageEvents.Error(this->GetName() + " can't start constraint controller cartesian mode, make sure the tool is inserted past the cannula"); break; } RobotState = newState; IsGoalSet = false; MessageEvents.Status(this->GetName() + " constraint controller cartesian"); break; case mtsIntuitiveResearchKitArmTypes::DVRK_MANUAL: if (this->RobotState < mtsIntuitiveResearchKitArmTypes::DVRK_ARM_CALIBRATED) { MessageEvents.Error(this->GetName() + " is not ready yet"); return; } // disable PID to allow manual move PID.Enable(false); RobotState = newState; MessageEvents.Status(this->GetName() + " in manual mode"); break; default: break; } // Emit event with current state MessageEvents.RobotState(mtsIntuitiveResearchKitArmTypes::RobotStateTypeToString(this->RobotState)); }
void mtsIntuitiveResearchKitMTM::SetState(const mtsIntuitiveResearchKitArmTypes::RobotStateType & newState) { CMN_LOG_CLASS_RUN_DEBUG << GetName() << ": SetState: new state " << mtsIntuitiveResearchKitArmTypes::RobotStateTypeToString(newState) << std::endl; vctBoolVec torqueMode(8); // first cleanup from previous state switch (RobotState) { case mtsIntuitiveResearchKitArmTypes::DVRK_GRAVITY_COMPENSATION: case mtsIntuitiveResearchKitArmTypes::DVRK_CLUTCH: // Disable torque mode for all joints torqueMode.SetAll(false); PID.EnableTorqueMode(torqueMode); PID.SetTorqueOffset(vctDoubleVec(8, 0.0)); SetPositionJointLocal(JointGetDesired); break; default: break; } // setup transition switch (newState) { case mtsIntuitiveResearchKitArmTypes::DVRK_UNINITIALIZED: RobotIO.SetActuatorCurrent(vctDoubleVec(NumberOfAxes(), 0.0)); RobotIO.DisablePower(); PID.Enable(false); PID.SetCheckJointLimit(true); RobotState = newState; MessageEvents.Status(this->GetName() + " not initialized"); break; case mtsIntuitiveResearchKitArmTypes::DVRK_HOMING_POWERING: HomingTimer = 0.0; HomingPowerRequested = false; RobotState = newState; MessageEvents.Status(this->GetName() + " powering"); break; case mtsIntuitiveResearchKitArmTypes::DVRK_HOMING_CALIBRATING_ARM: HomingCalibrateArmStarted = false; RobotState = newState; this->MessageEvents.Status(this->GetName() + " calibrating arm"); break; case mtsIntuitiveResearchKitArmTypes::DVRK_HOMING_CALIBRATING_ROLL: HomingCalibrateRollSeekLower = false; HomingCalibrateRollSeekUpper = false; HomingCalibrateRollSeekCenter = false; HomingCalibrateRollLower = cmnTypeTraits<double>::MaxPositiveValue(); HomingCalibrateRollUpper = cmnTypeTraits<double>::MinNegativeValue(); RobotState = newState; this->MessageEvents.Status(this->GetName() + " calibrating roll"); break; case mtsIntuitiveResearchKitArmTypes::DVRK_READY: RobotState = newState; MessageEvents.Status(this->GetName() + " ready"); break; case mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_JOINT: case mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_GOAL_JOINT: if (this->RobotState < mtsIntuitiveResearchKitArmTypes::DVRK_READY) { MessageEvents.Error(this->GetName() + " is not ready"); return; } RobotState = newState; JointSet.Assign(JointGetDesired, this->NumberOfJoints()); if (newState == mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_JOINT) { IsGoalSet = false; MessageEvents.Status(this->GetName() + " position joint"); } else { JointTrajectory.EndTime = 0.0; MessageEvents.Status(this->GetName() + " position goal joint"); } break; case mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_CARTESIAN: case mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_GOAL_CARTESIAN: if (this->RobotState < mtsIntuitiveResearchKitArmTypes::DVRK_ARM_CALIBRATED) { MessageEvents.Error(this->GetName() + " is not calibrated"); return; } RobotState = newState; if (newState == mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_CARTESIAN) { IsGoalSet = false; MessageEvents.Status(this->GetName() + " position cartesian"); } else { JointTrajectory.EndTime = 0.0; MessageEvents.Status(this->GetName() + " position goal cartesian"); } break; case mtsIntuitiveResearchKitArmTypes::DVRK_GRAVITY_COMPENSATION: if (this->RobotState < mtsIntuitiveResearchKitArmTypes::DVRK_READY) { MessageEvents.Error(this->GetName() + " is not homed"); return; } RobotState = newState; MessageEvents.Status(this->GetName() + " gravity compensation"); torqueMode.SetAll(true); PID.EnableTorqueMode(torqueMode); PID.SetTorqueOffset(vctDoubleVec(8, 0.0)); CMN_LOG_CLASS_RUN_DEBUG << GetName() << ": SetState: set gravity compensation" << std::endl; break; case mtsIntuitiveResearchKitArmTypes::DVRK_CLUTCH: // check if MTM is ready if (this->RobotState < mtsIntuitiveResearchKitArmTypes::DVRK_READY) { MessageEvents.Error(this->GetName() + " is not homed"); return; } RobotState = newState; MessageEvents.Status(this->GetName() + " clutch mode"); // save current cartesian position to CartesianCluted CartesianClutched.Assign(CartesianGet); // set J1-J3 to torque mode (GC) and J4-J7 to PID mode torqueMode.SetAll(false); std::fill(torqueMode.begin(), torqueMode.begin() + 3, true); PID.EnableTorqueMode(torqueMode); break; default: break; } // Emit event with current state MessageEvents.RobotState(mtsIntuitiveResearchKitArmTypes::RobotStateTypeToString(this->RobotState)); }