void mtsIntuitiveResearchKitPSM::RunHomingCalibrateArm(void)
{
static const double extraTime = 5.0 * cmn_s;
const double currentTime = this->StateTable.GetTic();
// trigger motion
if (!HomingCalibrateArmStarted) {
// disable joint limits
PID.SetCheckJointLimit(false);
// enable PID and start from current position
JointSet.ForceAssign(JointGet);
SetPositionJointLocal(JointSet);
// finally enable PID
PID.Enable(true);
// compute joint goal position
JointTrajectory.Goal.SetSize(NumberOfJoints());
JointTrajectory.Goal.ForceAssign(JointGet);
// move to zero position only there is no tool present
Tool.GetButton(Tool.IsPresent);
Tool.IsPresent = !Tool.IsPresent;
if (!Tool.IsPresent) {
JointTrajectory.Goal.Element(0) = 0.0;
JointTrajectory.Goal.Element(1) = 0.0;
JointTrajectory.Goal.Element(2) = 0.0;
}
JointTrajectory.LSPB.Set(JointGet, JointTrajectory.Goal,
JointTrajectory.Velocity, JointTrajectory.Acceleration,
currentTime - this->GetPeriodicity(), robLSPB::LSPB_DURATION);
HomingTimer = currentTime + JointTrajectory.LSPB.Duration();
// set flag to indicate that homing has started
HomingCalibrateArmStarted = true;
}
// compute a new set point based on time
if (currentTime <= HomingTimer) {
JointTrajectory.LSPB.Evaluate(currentTime, JointSet);
SetPositionJointLocal(JointSet);
} else {
// request final position in case trajectory rounding prevent us to get there
SetPositionJointLocal(JointTrajectory.Goal);
// check position
JointTrajectory.GoalError.DifferenceOf(JointTrajectory.Goal, JointGet);
JointTrajectory.GoalError.AbsSelf();
bool isHomed = !JointTrajectory.GoalError.ElementwiseGreaterOrEqual(JointTrajectory.GoalTolerance).Any();
if (isHomed) {
PID.SetCheckJointLimit(true);
this->SetState(mtsIntuitiveResearchKitArmTypes::DVRK_ARM_CALIBRATED);
} else {
// time out
if (currentTime > HomingTimer + extraTime) {
CMN_LOG_CLASS_INIT_WARNING << GetName() << ": RunHomingCalibrateArm: unable to reach home position, error in degrees is "
<< JointTrajectory.GoalError * (180.0 / cmnPI) << std::endl;
MessageEvents.Error(this->GetName() + " unable to reach home position during calibration on pots.");
this->SetState(mtsIntuitiveResearchKitArmTypes::DVRK_UNINITIALIZED);
}
}
}
}
void mtsIntuitiveResearchKitPSM::RunEngagingAdapter(void)
{
if (!EngagingAdapterStarted) {
EngagingStopwatch.Reset();
EngagingStopwatch.Start();
EngagingJointSet.ForceAssign(JointGetDesired);
// disable joint limits
PID.SetCheckJointLimit(false);
EngagingAdapterStarted = true;
return;
}
// PSM tool last 4 actuator coupling matrix
// psm_m2jpos = [-1.5632 0.0000 0.0000 0.0000;
// 0.0000 1.0186 0.0000 0.0000;
// 0.0000 -0.8306 0.6089 0.6089;
// 0.0000 0.0000 -1.2177 1.2177];
// each actuator has -180 to 180 deg limits
// these joint limit is computed as
// joint_lim = psm_m2jpos * actuator_lim
if (EngagingStopwatch.GetElapsedTime() > (3500 * cmn_ms)) {
EngagingJointSet[3] = 0.0;
EngagingJointSet[4] = 0.0;
EngagingJointSet[5] = 0.0;
EngagingJointSet[6] = 0.0;
JointSet.ForceAssign(EngagingJointSet);
PID.SetCheckJointLimit(true);
SetPositionJointLocal(JointSet);
// Adapter engage done
EngagingStopwatch.Reset();
SetState(mtsIntuitiveResearchKitArmTypes::DVRK_ADAPTER_ENGAGED);
}
else if (EngagingStopwatch.GetElapsedTime() > (2500 * cmn_ms)) {
EngagingJointSet[3] = -300.0 * cmnPI / 180.0;
EngagingJointSet[4] = 170.0 * cmnPI / 180.0;
EngagingJointSet[5] = 65.0 * cmnPI / 180.0;
EngagingJointSet[6] = 0.0 * cmnPI / 180.0;
JointSet.ForceAssign(EngagingJointSet);
SetPositionJointLocal(JointSet);
}
else if (EngagingStopwatch.GetElapsedTime() > (1500 * cmn_ms)) {
EngagingJointSet[3] = 300.0 * cmnPI / 180.0;
EngagingJointSet[4] = -170.0 * cmnPI / 180.0;
EngagingJointSet[5] = -65.0 * cmnPI / 180.0;
EngagingJointSet[6] = 0.0 * cmnPI / 180.0;
JointSet.ForceAssign(EngagingJointSet);
SetPositionJointLocal(JointSet);
}
else if (EngagingStopwatch.GetElapsedTime() > (500 * cmn_ms)) {
EngagingJointSet[3] = -300.0 * cmnPI / 180.0;
EngagingJointSet[4] = 170.0 * cmnPI / 180.0;
EngagingJointSet[5] = 65.0 * cmnPI / 180.0;
EngagingJointSet[6] = 0.0 * cmnPI / 180.0;
JointSet.ForceAssign(EngagingJointSet);
SetPositionJointLocal(JointSet);
}
}
void mtsIntuitiveResearchKitPSM::RunEngagingTool(void)
{
if (!EngagingToolStarted) {
EngagingStopwatch.Reset();
EngagingStopwatch.Start();
EngagingJointSet.ForceAssign(JointGetDesired);
// disable joint limits
PID.SetCheckJointLimit(false);
EngagingToolStarted = true;
return;
}
if (EngagingStopwatch.GetElapsedTime() > (3000 * cmn_ms)) {
EngagingStopwatch.Reset();
// Adapter engage done
SetState(mtsIntuitiveResearchKitArmTypes::DVRK_READY);
}
else if (EngagingStopwatch.GetElapsedTime() > (2500 * cmn_ms)) {
// straight wrist open gripper
EngagingJointSet[3] = 0.0;
EngagingJointSet[4] = 0.0;
EngagingJointSet[5] = 0.0;
EngagingJointSet[6] = 10.0 * cmnPI / 180.0;
JointSet.Assign(EngagingJointSet);
SetPositionJointLocal(JointSet);
}
else if (EngagingStopwatch.GetElapsedTime() > (2000 * cmn_ms)) {
EngagingJointSet[3] = -280.0 * cmnPI / 180.0;
EngagingJointSet[4] = 10.0 * cmnPI / 180.0;
EngagingJointSet[5] = 10.0 * cmnPI / 180.0;
EngagingJointSet[6] = 10.0 * cmnPI / 180.0;
JointSet.Assign(EngagingJointSet);
SetPositionJointLocal(JointSet);
}
else if (EngagingStopwatch.GetElapsedTime() > (1500 * cmn_ms)) {
EngagingJointSet[3] = -280.0 * cmnPI / 180.0;
EngagingJointSet[4] = 10.0 * cmnPI / 180.0;
EngagingJointSet[5] = -10.0 * cmnPI / 180.0;
EngagingJointSet[6] = 10.0 * cmnPI / 180.0;
JointSet.Assign(EngagingJointSet);
SetPositionJointLocal(JointSet);
}
else if (EngagingStopwatch.GetElapsedTime() > (1000 * cmn_ms)) {
EngagingJointSet[3] = 280.0 * cmnPI / 180.0;
EngagingJointSet[4] = -10.0 * cmnPI / 180.0;
EngagingJointSet[5] = 10.0 * cmnPI / 180.0;
EngagingJointSet[6] = 10.0 * cmnPI / 180.0;
JointSet.Assign(EngagingJointSet);
SetPositionJointLocal(JointSet);
}
else if (EngagingStopwatch.GetElapsedTime() > (500 * cmn_ms)) {
EngagingJointSet[3] = -280.0 * cmnPI / 180.0;
EngagingJointSet[4] = -10.0 * cmnPI / 180.0;
EngagingJointSet[5] = -10.0 * cmnPI / 180.0;
EngagingJointSet[6] = 10.0 * cmnPI / 180.0;
JointSet.Assign(EngagingJointSet);
SetPositionJointLocal(JointSet);
}
}
void mtsIntuitiveResearchKitMTM::RunHomingCalibrateArm(void)
{
static const double extraTime = 2.0 * cmn_s;
const double currentTime = this->StateTable.GetTic();
// trigger motion
if (!HomingCalibrateArmStarted) {
// disable joint limits
PID.SetCheckJointLimit(false);
// enable PID and start from current position
JointSet.ForceAssign(JointGet);
SetPositionJointLocal(JointSet);
PID.Enable(true);
// compute joint goal position
JointTrajectory.Goal.SetSize(NumberOfJoints);
JointTrajectory.Goal.SetAll(0.0);
// last joint is calibrated later
JointTrajectory.Goal.Element(RollIndex) = JointGet.Element(RollIndex);
JointTrajectory.LSPB.Set(JointGet, JointTrajectory.Goal,
JointTrajectory.Velocity, JointTrajectory.Acceleration,
currentTime, robLSPB::LSPB_DURATION);
HomingTimer = currentTime + JointTrajectory.LSPB.Duration();
// set flag to indicate that homing has started
HomingCalibrateArmStarted = true;
}
// compute a new set point based on time
if (currentTime <= HomingTimer) {
JointTrajectory.LSPB.Evaluate(currentTime, JointSet);
SetPositionJointLocal(JointSet);
} else {
// request final position in case trajectory rounding prevent us to get there
SetPositionJointLocal(JointTrajectory.Goal);
// check position
JointTrajectory.GoalError.DifferenceOf(JointTrajectory.Goal, JointGet);
JointTrajectory.GoalError.AbsSelf();
bool isHomed = !JointTrajectory.GoalError.ElementwiseGreaterOrEqual(JointTrajectory.GoalTolerance).Any();
if (isHomed) {
PID.SetCheckJointLimit(true);
EventTriggers.RobotStatusMsg(this->GetName() + " arm calibrated");
this->SetState(mtsIntuitiveResearchKitMTMTypes::MTM_HOMING_CALIBRATING_ROLL);
} else {
// time out
if (currentTime > HomingTimer + extraTime) {
CMN_LOG_CLASS_INIT_WARNING << GetName() << ": RunHomingCalibrateArm: unable to reach home position, error in degrees is "
<< JointTrajectory.GoalError * (180.0 / cmnPI) << std::endl;
EventTriggers.RobotErrorMsg(this->GetName() + " unable to reach home position during calibration on pots.");
PID.Enable(false);
PID.SetCheckJointLimit(true);
this->SetState(mtsIntuitiveResearchKitMTMTypes::MTM_UNINITIALIZED);
}
}
}
}
void mtsIntuitiveResearchKitPSM::RunPositionCartesian(void)
{
//! \todo: should prevent user to go to close to RCM!
if (IsCartesianGoalSet == true) {
vctDoubleVec jointSet(6, 0.0);
jointSet.Assign(JointCurrent, 6);
// compute desired slave position
CartesianPositionFrm.From(CartesianGoalSet.Goal());
CartesianPositionFrm = CartesianPositionFrm * Frame6to7Inverse;
Manipulator.InverseKinematics(jointSet, CartesianPositionFrm);
jointSet.resize(7);
jointSet[6] = DesiredOpenAngle;
#if 1 // Anton
const double difference = JointCurrent[3] - jointSet[3];
const double differenceInTurns = nearbyint(difference / (2.0 * cmnPI));
jointSet[3] = jointSet[3] + differenceInTurns * 2.0 * cmnPI;
/*
if (differenceInTurns != 0.0) {
CMN_LOG_CLASS_RUN_DEBUG << GetName()
<< " diff = " << difference
<< " turns = " << difference / (2.0 * cmnPI)
<< " corr = " << differenceInTurns
<< " res = " << jointSet[3] << std::endl;
}
*/
#endif // Anton
SetPositionJointLocal(jointSet);
// reset flag
IsCartesianGoalSet = false;
}
}
void mtsIntuitiveResearchKitArm::RunPositionGoalJoint(void)
{
const double currentTime = this->StateTable.GetTic();
if (currentTime <= JointTrajectory.EndTime) {
JointTrajectory.LSPB.Evaluate(currentTime, JointSet);
SetPositionJointLocal(JointSet);
} else {
if (JointTrajectory.EndTime != 0.0) {
JointTrajectory.GoalReachedEvent(true);
JointTrajectory.EndTime = 0.0;
}
}
}
void mtsIntuitiveResearchKitMTM::RunPositionCartesian(void)
{
// sanity check
if (RobotState != mtsIntuitiveResearchKitMTMTypes::MTM_POSITION_CARTESIAN) {
CMN_LOG_CLASS_RUN_ERROR << GetName() << ": SetPositionCartesian: MTM not ready" << std::endl;
return;
}
const double currentTime = this->StateTable.GetTic();
if (currentTime <= JointTrajectory.EndTime) {
JointTrajectory.LSPB.Evaluate(currentTime, JointSet);
SetPositionJointLocal(JointSet);
}
}
void mtsIntuitiveResearchKitPSM::RunHomingPower(void)
{
const double timeToPower = 3.0 * cmn_s;
const double currentTime = this->StateTable.GetTic();
// first, request power to be turned on
if (!HomingPowerRequested) {
RobotIO.BiasEncoder();
HomingTimer = currentTime;
// make sure the PID is not sending currents
PID.Enable(false);
// pre-load the boards with zero current
RobotIO.SetMotorCurrent(vctDoubleVec(NumberOfJoints, 0.0));
// enable power and set a flags to move to next step
RobotIO.EnablePower();
HomingPowerRequested = true;
EventTriggers.RobotStatusMsg(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(JointCurrentParam);
// angles are radians but cisst uses mm. robManipulator uses SI, so we need meters
JointCurrentParam.Position().Element(2) /= 1000.0; // convert from mm to m
// assign to a more convenient vctDoubleVec
JointCurrent.Assign(JointCurrentParam.Position(), NumberOfJoints);
SetPositionJointLocal(JointCurrent);
// check power status
vctBoolVec amplifiersStatus(NumberOfJoints);
RobotIO.GetAmpStatus(amplifiersStatus);
if (amplifiersStatus.All()) {
EventTriggers.RobotStatusMsg(this->GetName() + " power on");
this->SetState(PSM_HOMING_CALIBRATING_ARM);
} else {
EventTriggers.RobotErrorMsg(this->GetName() + " failed to enable power.");
this->SetState(PSM_UNINITIALIZED);
}
}
}
void mtsIntuitiveResearchKitArm::RunPositionCartesian(void)
{
if (IsGoalSet) {
// copy current position
vctDoubleVec jointSet(JointGet.Ref(NumberOfJointsKinematics()));
// compute desired slave position
CartesianPositionFrm.From(CartesianSetParam.Goal());
if (this->InverseKinematics(jointSet, BaseFrame.Inverse() * CartesianPositionFrm) == robManipulator::ESUCCESS) {
// assign to joints used for kinematics
JointSet.Ref(NumberOfJointsKinematics()).Assign(jointSet);
// finally send new joint values
SetPositionJointLocal(JointSet);
} else {
MessageEvents.Error(this->GetName() + " unable to solve inverse kinematics.");
}
// reset flag
IsGoalSet = false;
}
}
void mtsIntuitiveResearchKitPSM::EventHandlerManipClutch(const prmEventButton & button)
{
// Pass events
ClutchEvents.ManipClutch(button);
// Start manual mode but save the previous state
if (button.Type() == prmEventButton::PRESSED) {
ClutchEvents.ManipClutchPreviousState = this->RobotState;
SetState(mtsIntuitiveResearchKitArmTypes::DVRK_MANUAL);
} else {
if (RobotState == mtsIntuitiveResearchKitArmTypes::DVRK_MANUAL) {
// Enable PID
PID.Enable(true);
// set command joint position to joint current
JointSet.ForceAssign(JointGet);
SetPositionJointLocal(JointSet);
// go back to state before clutching
SetState(ClutchEvents.ManipClutchPreviousState);
}
}
}
void mtsIntuitiveResearchKitMTM::RunClutch(void)
{
// J1-J3
vctDoubleVec q(7, 0.0);
vctDoubleVec qd(7, 0.0);
vctDoubleVec tau(7, 0.0);
q.Assign(JointGet.Ref(7));
vctDoubleVec torqueDesired(8, 0.0);
tau.ForceAssign(Manipulator.CCG(q, qd));
tau[0] = q(0) * 0.0564 + 0.08;
torqueDesired.Ref(7).Assign(tau);
TorqueSet.SetForceTorque(torqueDesired);
PID.SetTorqueJoint(TorqueSet);
// J4-J7
JointSet.Assign(JointGet);
CartesianClutched.Translation().Assign(CartesianGet.Translation());
Manipulator.InverseKinematics(JointSet, CartesianClutched);
SetPositionJointLocal(JointSet);
}
void mtsIntuitiveResearchKitPSM::RunConstraintControllerCartesian(void)
{
// Update the optimizer
// Go through the VF list, update state data pointers, assign tableau references, and fill in the references
if (IsGoalSet) {
IsGoalSet = false;
// Update kinematics and VF data objects
Optimizer->UpdateParams(JointGet,
Manipulator,
this->GetPeriodicity(),
CartesianGet,
vctFrm4x4(CartesianSetParam.Goal())
);
vctDoubleVec dq;
// Make sure the return value is meaningful
if (Optimizer->Solve(dq)) {
// make appropriate adjustments to incremental motion specific to davinci
// send command to move to specified position
vctDoubleVec FinalJoint(6);
FinalJoint.Assign(JointGet,6);
FinalJoint = FinalJoint + dq;
FinalJoint.resize(7);
// find closest solution mod 2 pi
double diffTurns = nearbyint(-dq[3] / (2.0 * cmnPI));
FinalJoint[3] = FinalJoint[3] + diffTurns * 2.0 * cmnPI;
// Send the final joint commands to the LLC
SetPositionJointLocal(FinalJoint);
}
else {
CMN_LOG_CLASS_RUN_ERROR << "Control Optimizer failed " << std::endl;
}
}
}
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;
}
}
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 mtsIntuitiveResearchKitArm::RunPositionJoint(void)
{
if (IsGoalSet) {
SetPositionJointLocal(JointSet);
}
}
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 mtsIntuitiveResearchKitMTM::RunHomingCalibrateRoll(void)
{
static const double maxTrackingError = 1.0 * cmnPI; // 1/2 turn
static const double maxRollRange = 6.0 * cmnPI + maxTrackingError; // that actual device is limited to ~2.6 turns
static const double extraTime = 2.0 * cmn_s;
const double currentTime = this->StateTable.GetTic();
// trigger search of lower limit
if (!HomingCalibrateRollSeekLower) {
// disable joint limits on PID
PID.SetCheckJointLimit(false);
// compute joint goal position, we assume PID is on from previous state
const double currentRoll = JointGet.Element(JNT_WRIST_ROLL);
JointTrajectory.Start.SetAll(0.0);
JointTrajectory.Start.Element(JNT_WRIST_ROLL) = currentRoll;
JointTrajectory.Goal.SetAll(0.0);
JointTrajectory.Goal.Element(JNT_WRIST_ROLL) = currentRoll - maxRollRange;
JointTrajectory.LSPB.Set(JointTrajectory.Start, JointTrajectory.Goal,
JointTrajectory.Velocity, JointTrajectory.Acceleration,
currentTime, robLSPB::LSPB_DURATION);
HomingTimer = currentTime + JointTrajectory.LSPB.Duration();
// set flag to indicate that homing has started
HomingCalibrateRollSeekLower = true;
return;
}
// looking for lower limit has start but not found yet
if (HomingCalibrateRollSeekLower
&& (HomingCalibrateRollLower == cmnTypeTraits<double>::MaxPositiveValue())) {
JointTrajectory.LSPB.Evaluate(currentTime, JointSet);
SetPositionJointLocal(JointSet);
// detect tracking error and set lower limit
const double trackingError =
std::abs(JointGet.Element(JNT_WRIST_ROLL) - JointSet.Element(JNT_WRIST_ROLL));
if (trackingError > maxTrackingError) {
HomingCalibrateRollLower = JointGet.Element(JNT_WRIST_ROLL);
MessageEvents.Status(this->GetName() + " found roll lower limit");
} else {
// time out
if (currentTime > HomingTimer + extraTime) {
MessageEvents.Error(this->GetName() + " unable to hit roll lower limit");
this->SetState(mtsIntuitiveResearchKitArmTypes::DVRK_UNINITIALIZED);
}
}
return;
}
// trigger search of upper limit
if (!HomingCalibrateRollSeekUpper) {
// compute joint goal position, we assume PID is on from previous state
const double currentRoll = JointGet.Element(JNT_WRIST_ROLL);
JointTrajectory.Start.SetAll(0.0);
JointTrajectory.Start.Element(JNT_WRIST_ROLL) = currentRoll;
JointTrajectory.Goal.SetAll(0.0);
JointTrajectory.Goal.Element(JNT_WRIST_ROLL) = currentRoll + maxRollRange;
JointTrajectory.LSPB.Set(JointTrajectory.Start, JointTrajectory.Goal,
JointTrajectory.Velocity, JointTrajectory.Acceleration,
currentTime, robLSPB::LSPB_DURATION);
HomingTimer = currentTime + JointTrajectory.LSPB.Duration();
// set flag to indicate that homing has started
HomingCalibrateRollSeekUpper = true;
return;
}
// looking for lower limit has start but not found yet
if (HomingCalibrateRollSeekUpper
&& (HomingCalibrateRollUpper == cmnTypeTraits<double>::MinNegativeValue())) {
JointTrajectory.LSPB.Evaluate(currentTime, JointSet);
SetPositionJointLocal(JointSet);
// detect tracking error and set lower limit
const double trackingError =
std::abs(JointGet.Element(JNT_WRIST_ROLL) - JointSet.Element(JNT_WRIST_ROLL));
if (trackingError > maxTrackingError) {
HomingCalibrateRollUpper = JointGet.Element(JNT_WRIST_ROLL);
MessageEvents.Status(this->GetName() + " found roll upper limit");
} else {
// time out
if (currentTime > HomingTimer + extraTime) {
MessageEvents.Error(this->GetName() + " unable to hit roll upper limit");
this->SetState(mtsIntuitiveResearchKitArmTypes::DVRK_UNINITIALIZED);
}
}
return;
}
// compute trajectory to go to center point
if (!HomingCalibrateRollSeekCenter) {
// compute joint goal position, we assume PID is on from previous state
JointTrajectory.Start.SetAll(0.0);
JointTrajectory.Start.Element(JNT_WRIST_ROLL) = JointGet.Element(JNT_WRIST_ROLL);
JointTrajectory.Goal.SetAll(0.0);
JointTrajectory.Goal.Element(JNT_WRIST_ROLL) = HomingCalibrateRollLower + 480.0 * cmnPI_180;
JointTrajectory.LSPB.Set(JointTrajectory.Start, JointTrajectory.Goal,
JointTrajectory.Velocity, JointTrajectory.Acceleration,
currentTime, robLSPB::LSPB_DURATION);
HomingTimer = currentTime + JointTrajectory.LSPB.Duration();
// set flag to indicate that homing has started
HomingCalibrateRollSeekCenter = true;
return;
}
// going to center position and check we have arrived
if (currentTime <= HomingTimer) {
JointTrajectory.LSPB.Evaluate(currentTime, JointSet);
SetPositionJointLocal(JointSet);
} else {
// request final position in case trajectory rounding prevent us to get there
SetPositionJointLocal(JointTrajectory.Goal);
// check position
JointTrajectory.GoalError.DifferenceOf(JointTrajectory.Goal, JointGet);
JointTrajectory.GoalError.AbsSelf();
bool isHomed = !JointTrajectory.GoalError.ElementwiseGreaterOrEqual(JointTrajectory.GoalTolerance).Any();
if (isHomed) {
// reset encoder on last joint as well as PID target position to reflect new roll position = 0
RobotIO.ResetSingleEncoder(static_cast<int>(JNT_WRIST_ROLL));
JointSet.SetAll(0.0);
SetPositionJointLocal(JointSet);
PID.SetCheckJointLimit(true);
MessageEvents.Status(this->GetName() + " roll calibrated");
this->SetState(mtsIntuitiveResearchKitArmTypes::DVRK_READY);
} else {
// time out
if (currentTime > HomingTimer + extraTime) {
CMN_LOG_CLASS_INIT_WARNING << GetName() << ": RunHomingCalibrateRoll: unable to reach home position, error in degrees is "
<< JointTrajectory.GoalError * (180.0 / cmnPI) << std::endl;
MessageEvents.Error(this->GetName() + " unable to reach home position during calibration on pots.");
this->SetState(mtsIntuitiveResearchKitArmTypes::DVRK_UNINITIALIZED);
}
}
}
}
void mtsIntuitiveResearchKitPSM::RunEngagingTool(void)
{
if (!EngagingToolStarted) {
EngagingStopwatch.Reset();
EngagingStopwatch.Start();
// get current joint desired position from PID
PID.GetPositionJointDesired(JointDesired);
// PID has data in mm, we are working in meters here
JointDesired.Element(2) /= 1000.0;
EngagingJointSet.ForceAssign(JointDesired);
// disable joint limits
PID.SetCheckJointLimit(false);
EngagingToolStarted = true;
return;
}
if (EngagingStopwatch.GetElapsedTime() > (2500 * cmn_ms)){
// get current joint desired position from PID
PID.GetPositionJointDesired(JointDesired);
// PID has data in mm, we are working in meters here
JointDesired.Element(2) /= 1000.0;
// open gripper
JointDesired[6] = 10.0 * cmnPI / 180.0;
PID.SetCheckJointLimit(true);
SetPositionJointLocal(JointDesired);
// Adapter engage done
EngagingStopwatch.Reset();
SetState(PSM_READY);
}
else if (EngagingStopwatch.GetElapsedTime() > (2000 * cmn_ms)){
EngagingJointSet[3] = -280.0 * cmnPI / 180.0;
EngagingJointSet[4] = 10.0 * cmnPI / 180.0;
EngagingJointSet[5] = 10.0 * cmnPI / 180.0;
EngagingJointSet[6] = 10.0 * cmnPI / 180.0;
JointDesired.ForceAssign(EngagingJointSet);
SetPositionJointLocal(JointDesired);
}
else if (EngagingStopwatch.GetElapsedTime() > (1500 * cmn_ms)){
EngagingJointSet[3] = -280.0 * cmnPI / 180.0;
EngagingJointSet[4] = 10.0 * cmnPI / 180.0;
EngagingJointSet[5] = -10.0 * cmnPI / 180.0;
EngagingJointSet[6] = 10.0 * cmnPI / 180.0;
JointDesired.ForceAssign(EngagingJointSet);
SetPositionJointLocal(JointDesired);
}
else if (EngagingStopwatch.GetElapsedTime() > (1000 * cmn_ms)){
EngagingJointSet[3] = 280.0 * cmnPI / 180.0;
EngagingJointSet[4] = -10.0 * cmnPI / 180.0;
EngagingJointSet[5] = 10.0 * cmnPI / 180.0;
EngagingJointSet[6] = 10.0 * cmnPI / 180.0;
JointDesired.ForceAssign(EngagingJointSet);
SetPositionJointLocal(JointDesired);
}
else if (EngagingStopwatch.GetElapsedTime() > (500 * cmn_ms)){
EngagingJointSet[3] = -280.0 * cmnPI / 180.0;
EngagingJointSet[4] = -10.0 * cmnPI / 180.0;
EngagingJointSet[5] = -10.0 * cmnPI / 180.0;
EngagingJointSet[6] = 10.0 * cmnPI / 180.0;
JointDesired.ForceAssign(EngagingJointSet);
SetPositionJointLocal(JointDesired);
}
}
void mtsIntuitiveResearchKitECM::RunHomingCalibrateArm(void)
{
if (mIsSimulated) {
this->SetState(mtsIntuitiveResearchKitArmTypes::DVRK_READY);
return;
}
static const double extraTime = 5.0 * cmn_s;
const double currentTime = this->StateTable.GetTic();
// trigger motion
if (!HomingCalibrateArmStarted) {
// disable joint limits
PID.SetCheckJointLimit(false);
// enable PID and start from current position
JointSet.ForceAssign(JointGet);
SetPositionJointLocal(JointSet);
// configure PID to fail in case of tracking error
vctDoubleVec tolerances(NumberOfJoints());
tolerances.SetAll(7.0 * cmnPI_180); // 7 degrees on angles
tolerances.Element(2) = 10.0 * cmn_mm; // 10 mm
PID.SetTrackingErrorTolerance(tolerances);
PID.EnableTrackingError(true);
// finally enable PID
PID.Enable(true);
// compute joint goal position
JointTrajectory.Goal.SetSize(NumberOfJoints());
if (HomingGoesToZero) {
// move to zero position
JointTrajectory.Goal.SetAll(0.0);
} else {
// stay at current position by default
JointTrajectory.Goal.Assign(JointGet);
}
JointTrajectory.LSPB.Set(JointGet, JointTrajectory.Goal,
JointTrajectory.Velocity, JointTrajectory.Acceleration,
currentTime, robLSPB::LSPB_DURATION);
HomingTimer = currentTime + JointTrajectory.LSPB.Duration();
// set flag to indicate that homing has started
HomingCalibrateArmStarted = true;
}
// compute a new set point based on time
if (currentTime <= HomingTimer) {
JointTrajectory.LSPB.Evaluate(currentTime, JointSet);
SetPositionJointLocal(JointSet);
} else {
// request final position in case trajectory rounding prevent us to get there
SetPositionJointLocal(JointTrajectory.Goal);
// check position
JointTrajectory.GoalError.DifferenceOf(JointTrajectory.Goal, JointGet);
JointTrajectory.GoalError.AbsSelf();
bool isHomed = !JointTrajectory.GoalError.ElementwiseGreaterOrEqual(JointTrajectory.GoalTolerance).Any();
if (isHomed) {
PID.SetCheckJointLimit(true);
HomedOnce = true;
MessageEvents.Status(this->GetName() + " arm ready");
this->SetState(mtsIntuitiveResearchKitArmTypes::DVRK_READY);
} else {
// time out
if (currentTime > HomingTimer + extraTime) {
CMN_LOG_CLASS_INIT_WARNING << GetName() << ": RunHomingCalibrateArm: unable to reach home position, error in degrees is "
<< JointTrajectory.GoalError * (180.0 / cmnPI) << std::endl;
MessageEvents.Error(this->GetName() + " unable to reach home position during calibration on pots.");
this->SetState(mtsIntuitiveResearchKitArmTypes::DVRK_UNINITIALIZED);
}
}
}
}
