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