void mtsPIDQtWidget::SlotEnableTorqueMode(bool toggle)
{
vctBoolVec torqueMode(NumberOfAxis, toggle);
PID.EnableTorqueMode(torqueMode);
QVWDesiredEffortWidget->setEnabled(toggle);
if(!toggle)
{
QVWDesiredEffortWidget->setStyleSheet(QString::fromUtf8("QPushButton:disabled"
"{ color: gray }"
));
} else {
vctDoubleVec initTorque;
initTorque.SetSize(NumberOfAxis);
initTorque.SetAll(0.0);
QVWDesiredEffortWidget->SetValue(initTorque);
}
}
void mtsPIDQtWidget::SlotEnableTorqueMode(bool toggle)
{
vctBoolVec torqueMode(NumberOfAxis, toggle);
PID.EnableTorqueMode(torqueMode);
}
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));
}
void mtsIntuitiveResearchKitMTM::SetState(const mtsIntuitiveResearchKitMTMTypes::RobotStateType & newState)
{
CMN_LOG_CLASS_RUN_DEBUG << GetName() << ": SetState: new state "
<< mtsIntuitiveResearchKitMTMTypes::RobotStateTypeToString(newState) << std::endl;
vctBoolVec torqueMode(8, true);
switch (newState) {
case mtsIntuitiveResearchKitMTMTypes::MTM_UNINITIALIZED:
RobotState = newState;
EventTriggers.RobotStatusMsg(this->GetName() + " not initialized");
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_HOMING_POWERING:
HomingTimer = 0.0;
HomingPowerRequested = false;
HomingPowerCurrentBiasRequested = false;
RobotState = newState;
EventTriggers.RobotStatusMsg(this->GetName() + " powering");
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_HOMING_CALIBRATING_ARM:
HomingCalibrateArmStarted = false;
RobotState = newState;
this->EventTriggers.RobotStatusMsg(this->GetName() + " calibrating arm");
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_HOMING_CALIBRATING_ROLL:
HomingCalibrateRollSeekLower = false;
HomingCalibrateRollSeekUpper = false;
HomingCalibrateRollSeekCenter = false;
HomingCalibrateRollLower = cmnTypeTraits<double>::MaxPositiveValue();
HomingCalibrateRollUpper = cmnTypeTraits<double>::MinNegativeValue();
RobotState = newState;
this->EventTriggers.RobotStatusMsg(this->GetName() + " calibrating roll");
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_READY:
RobotState = newState;
EventTriggers.RobotStatusMsg(this->GetName() + " ready");
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_POSITION_CARTESIAN:
if (this->RobotState < mtsIntuitiveResearchKitMTMTypes::MTM_READY) {
EventTriggers.RobotErrorMsg(this->GetName() + " is not homed");
return;
}
RobotState = newState;
EventTriggers.RobotStatusMsg(this->GetName() + " position cartesian");
// Disable torque mode for all joints
torqueMode.SetAll(false);
PID.EnableTorqueMode(torqueMode);
PID.SetTorqueOffset(vctDoubleVec(8, 0.0));
SetPositionJointLocal(JointGet);
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_GRAVITY_COMPENSATION:
if (this->RobotState < mtsIntuitiveResearchKitMTMTypes::MTM_READY) {
EventTriggers.RobotErrorMsg(this->GetName() + " is not homed");
return;
}
RobotState = newState;
EventTriggers.RobotStatusMsg(this->GetName() + " gravity compensation");
PID.EnableTorqueMode(torqueMode);
PID.SetTorqueOffset(vctDoubleVec(8, 0.0));
CMN_LOG_CLASS_RUN_DEBUG << GetName() << ": SetState: set gravity compensation" << std::endl;
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_CLUTCH:
// check if MTM is ready
if (this->RobotState < mtsIntuitiveResearchKitMTMTypes::MTM_READY) {
EventTriggers.RobotErrorMsg(this->GetName() + " is not homed");
return;
}
RobotState = newState;
EventTriggers.RobotStatusMsg(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;
}
}
