void NDISimulator::Run(void){
ProcessQueuedCommands();
// generate a random tranformation matrix for testing (rotation around z)
MarkerPosition.Position().Translation().X() = 0;
MarkerPosition.Position().Translation().Y() = 0;
MarkerPosition.Position().Translation().Z() = 0;
MarkerPosition.Position().Rotation().Element(0,0) = cos(theta);
MarkerPosition.Position().Rotation().Element(0,1) = -sin(theta);
MarkerPosition.Position().Rotation().Element(0,2) = 0;
MarkerPosition.Position().Rotation().Element(1,0) = sin(theta);
MarkerPosition.Position().Rotation().Element(1,1) = cos(theta);
MarkerPosition.Position().Rotation().Element(1,2) = 0;
MarkerPosition.Position().Rotation().Element(2,0) = 0;
MarkerPosition.Position().Rotation().Element(2,1) = 0;
MarkerPosition.Position().Rotation().Element(2,2) = 1;
theta += 0.1;
}
void sdpPlayerExample::Run(void)
{
ProcessQueuedEvents();
ProcessQueuedCommands();
//update the model (load data) etc.
if (State == PLAY) {
double currentTime = TimeServer.GetAbsoluteTimeInSeconds();
Time = currentTime - PlayStartTime.Timestamp() + PlayStartTime.Data;
if (Time > PlayUntilTime) {
Time = PlayUntilTime;
State = STOP;
}
else {
//Load and Prep current data
}
}
//make sure we are at the correct seek position.
else if (State == SEEK) {
//Load and Prep current data
}
else if (State == STOP) {
//do Nothing
}
//now display updated data in the qt thread space.
if (Widget.isVisible()) {
emit QSignalUpdateQT();
}
}
void Run(){
ProcessQueuedCommands();
for( size_t i=0; i<qout.Position().size(); i++ )
{ qout.Position()[i] += 0.001; }
std::cout << "q: " << qout.Position() << std::endl;
std::cout << "tau: " << tin.ForceTorque() << std::endl;
}
void mtsQtApplication::Process(void)
{
if (InterfaceProvidedToManager) {
InterfaceProvidedToManager->ProcessMailBoxes();
}
ProcessQueuedCommands();
}
void mtsTeleOperationECM::Run(void)
{
ProcessQueuedCommands();
ProcessQueuedEvents();
// run based on state
mTeleopState.Run();
}
void ireTask::Run(void)
{
ProcessQueuedCommands();
ProcessQueuedEvents();
// Need following call to give the IRE thread some time to execute.
ireFramework::JoinIREShell(0.1);
if (ireFramework::IsFinished())
Kill();
}
void Run(){
ProcessQueuedCommands();
t += GetPeriodicity();
double dx = 0.1 * sin( t - cmnPI_2 ) + 0.1;
Rt[0][3] = x + -dx;
mtsFrm4x4 mtsRt( Rt );
SetPositionCartesian( mtsRt );
}
void C2ServerTask::Run(void) {
if (UIOpened()) {
// process the commands received, i.e. possible SetServerAmplitude
ProcessQueuedCommands();
// compute the new values based on the current time and amplitude
fltkMutex.Lock();
{
if (UI.VoidEventRequested1) {
CMN_LOG_CLASS_RUN_VERBOSE << "Run: VoidEventRequested1" << std::endl;
this->EventVoid1();
UI.VoidEventRequested1 = false;
}
if (UI.WriteEventRequested1) {
CMN_LOG_CLASS_RUN_VERBOSE << "Run: WriteEventRequested1" << std::endl;
this->EventWrite1(mtsDouble(UI.ReadValue1->value()));
UI.WriteEventRequested1 = false;
}
if (UI.VoidEventRequested2) {
CMN_LOG_CLASS_RUN_VERBOSE << "Run: VoidEventRequested2" << std::endl;
this->EventVoid2();
UI.VoidEventRequested2 = false;
}
if (UI.WriteEventRequested2) {
CMN_LOG_CLASS_RUN_VERBOSE << "Run: WriteEventRequested2" << std::endl;
this->EventWrite2(mtsDouble(UI.ReadValue2->value()));
UI.WriteEventRequested2 = false;
}
this->ReadValue1 = UI.ReadValue1->value();
this->ReadValue2 = UI.ReadValue2->value();
if (UI.Disconnect) {
CMN_LOG_CLASS_RUN_VERBOSE << "Run: Disconnect" << std::endl;
this->DisconnectGCM();
UI.Disconnect = false;
}
if (UI.Reconnect) {
CMN_LOG_CLASS_RUN_VERBOSE << "Run: Reconnect" << std::endl;
this->ReconnectGCM();
UI.Reconnect = false;
}
Fl::check();
}
fltkMutex.Unlock();
}
}
void Run(){
ProcessQueuedCommands();
prmPositionJointGet q;
GetPositions( q );
std::cout << q.Position() << std::endl;
prmForceTorqueJointSet t;
t.SetSize( 7 );
t.ForceTorque().SetAll( 0.0 );
SetTorques( t );
}
void Run(){
ProcessQueuedCommands();
prmPositionJointGet qin;
GetPositions( qin );
prmPositionJointSet qout;
qout.SetSize( 7 );
qout.Goal() = q;
SetPositions( qout );
for( size_t i=0; i<q.size(); i++ ) q[i] += 0.001;
}
void Run(){
ProcessQueuedCommands();
// rotate hubble
vctFixedSizeVector<double,3> u( 0.0, 0.0, 1.0 );
vctAxisAngleRotation3<double> Rwh( u, theta );
vctFrm3 Rtwh;
Rtwh.Rotation().FromRaw( Rwh );
Rtwh.Translation().Assign( vctFixedSizeVector<double,3>( 0.0, 0.0, 0.5 ) );
Rt.Position() = Rtwh;
theta += 0.001;
}
void mtsROSBridge::Run(void)
{
ProcessQueuedCommands();
ProcessQueuedEvents();
const PublishersType::iterator end = Publishers.end();
PublishersType::iterator iter;
for (iter = Publishers.begin();
iter != end;
++iter) {
(*iter)->Execute();
}
if (mSpin) ros::spinOnce();
}
void mtsComponentViewer::Run(void)
{
if (!UDrawPipeConnected) {
ConnectToUDrawGraph();
if (UDrawPipeConnected) {
CMN_LOG_CLASS_INIT_VERBOSE << "Run: Sending all info" << std::endl;
// Should flush all events before calling SendAllInfo
SendAllInfo();
}
}
ProcessQueuedCommands();
ProcessQueuedEvents();
if (UDrawPipeConnected)
ProcessResponse();
}
void mtsIntuitiveResearchKitArm::Run(void)
{
ProcessQueuedEvents();
GetRobotData();
switch (RobotState) {
case mtsIntuitiveResearchKitArmTypes::DVRK_UNINITIALIZED:
break;
case mtsIntuitiveResearchKitArmTypes::DVRK_HOMING_POWERING:
RunHomingPower();
break;
case mtsIntuitiveResearchKitArmTypes::DVRK_HOMING_CALIBRATING_ARM:
RunHomingCalibrateArm();
break;
case mtsIntuitiveResearchKitArmTypes::DVRK_ARM_CALIBRATED:
break;
case mtsIntuitiveResearchKitArmTypes::DVRK_READY:
break;
case mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_JOINT:
RunPositionJoint();
break;
case mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_GOAL_JOINT:
RunPositionGoalJoint();
break;
case mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_CARTESIAN:
RunPositionCartesian();
break;
case mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_GOAL_CARTESIAN:
RunPositionGoalCartesian();
break;
case mtsIntuitiveResearchKitArmTypes::DVRK_MANUAL:
break;
default:
RunArmSpecific();
break;
}
RunEvent();
ProcessQueuedCommands();
mCounter++;
CartesianGetPrevious = CartesianGet;
}
void Run(){
ProcessQueuedCommands();
cnt++;
// Create and send CAN a frame
osaCANBusFrame::ID id = 1;
vctDynamicVector<osaCANBusFrame::Data> data( 8, 1, 2, 3, 4, 5, 6, 7, cnt );
mtsCANBusFrame framesend( id, data );
write( framesend );
std::cout << "Sending: " << std::endl
<< (osaCANBusFrame)framesend << std::endl;
// Receive a CAN frame. If the CAN component is in loopback mode we should
// receive the frame we just sent
mtsCANBusFrame framerecv;
read( framerecv );
std::cout << "Received: " << std::endl
<< (osaCANBusFrame)framerecv << std::endl << std::endl;
}
void mtsODEManipulatorTask::Run(){
ProcessQueuedCommands();
if( manipulator.get() != NULL ){
if( manipulator->SetPositions( qin.Goal() ) !=
osaOSGManipulator::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to get position for " << GetName()
<< std::endl;
}
if( manipulator->GetPositions( qout.Position()) !=
osaOSGManipulator::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to get position for " << GetName()
<< std::endl;
}
}
}
void mtsIntuitiveResearchKitPSM::Run(void)
{
Counter++;
ProcessQueuedEvents();
GetRobotData();
switch (RobotState) {
case PSM_UNINITIALIZED:
break;
case PSM_HOMING_POWERING:
RunHomingPower();
break;
case PSM_HOMING_CALIBRATING_ARM:
RunHomingCalibrateArm();
break;
case PSM_ARM_CALIBRATED:
break;
case PSM_ENGAGING_ADAPTER:
RunEngagingAdapter();
break;
case PSM_ADAPTER_ENGAGED:
// choose next state
break;
case PSM_ENGAGING_TOOL:
RunEngagingTool();
break;
case PSM_READY:
case PSM_POSITION_CARTESIAN:
RunPositionCartesian();
break;
case PSM_MANUAL:
break;
default:
break;
}
RunEvent();
ProcessQueuedCommands();
}
void mtsOSGManipulatorTask::Run(){
ProcessQueuedCommands();
if( manipulator.get() != NULL ){
vctDynamicVector<double> vctq( prmqout.Position() );
if( inputtype == PROVIDE_INPUT ){
bool valid=false;
prmqin.GetValid( valid );
if( valid ) { vctq = prmqin.Goal(); }
}
else{
prmPositionJointGet prmqinput;
GetPositionJoint( prmqinput );
bool valid=false;
prmqinput.GetValid( valid );
if( valid ) { prmqinput.GetPosition( vctq ); }
}
if( manipulator->SetPositions( vctq ) !=
osaOSGManipulator::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to get position for " << GetName()
<< std::endl;
}
if( manipulator->GetPositions( prmqout.Position()) !=
osaOSGManipulator::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to get position for " << GetName()
<< std::endl;
}
prmRtout.Position().FromRaw( manipulator->ForwardKinematics( vctq ) );
prmRtout.SetValid( true );
}
}
void mtsIntuitiveResearchKitMTM::Run(void)
{
ProcessQueuedEvents();
GetRobotData();
switch (RobotState) {
case mtsIntuitiveResearchKitMTMTypes::MTM_UNINITIALIZED:
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_HOMING_POWERING:
RunHomingPower();
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_HOMING_CALIBRATING_ARM:
RunHomingCalibrateArm();
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_HOMING_CALIBRATING_ROLL:
RunHomingCalibrateRoll();
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_READY:
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_POSITION_CARTESIAN:
RunPositionCartesian();
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_GRAVITY_COMPENSATION:
RunGravityCompensation();
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_CLUTCH:
RunClutch();
default:
break;
}
RunEvent();
ProcessQueuedCommands();
// update previous value
CartesianGetPrevious = CartesianGet;
}
void mtsComponentAddLatency::Run(void)
{
ProcessQueuedEvents();
ProcessQueuedCommands();
mtsExecutionResult result;
DelayedReadList::iterator readIterator = DelayedReads.begin();
const DelayedReadList::iterator readEnd = DelayedReads.end();
this->LatencyStateTable.Start();
for (;
readIterator != readEnd;
++readIterator) {
result = (*readIterator)->Execute();
}
this->LatencyStateTable.Advance();
// see if we have some old void/write commands
const double time = mtsComponentManager::GetInstance()->GetTimeServer().GetRelativeTime() - this->Latency;
DelayedVoidList::iterator voidIterator = DelayedVoids.begin();
const DelayedVoidList::iterator voidEnd = DelayedVoids.end();
this->LatencyStateTable.Start();
for (;
voidIterator != voidEnd;
++voidIterator) {
(*voidIterator)->ProcessQueuedCommands(time);
}
DelayedWriteList::iterator writeIterator = DelayedWrites.begin();
const DelayedWriteList::iterator writeEnd = DelayedWrites.end();
this->LatencyStateTable.Start();
for (;
writeIterator != writeEnd;
++writeIterator) {
(*writeIterator)->ProcessQueuedCommands(time);
}
}
void Run(){
ProcessQueuedCommands();
Rt.Position().Translation()[2] += 0.001;
}
void mtsMedtronicStealthlink::Run(void)
{
ProcessQueuedCommands();
}
void mtsTeleop::Run(void)
{
// increment counter
counter_++;
// cisst process queued commands
ProcessQueuedCommands();
// refresh data
ros::spinOnce();
// publish
#if 1
if (counter_%10 == 0) {
// std::cerr << "clutch = " << is_clutch_pressed_ << std::endl;
// std::cerr << " mtm = " << std::endl
// << mtm_pose_cur_ << std::endl;
}
#endif
vctMatRot3 mtm2psm;
mtm2psm.Assign(-1.0, 0.0, 0.0,
0.0, -1.0, 0.0,
0.0, 0.0, 1.0);
vctMatRot3 mtm02psm0;
mtm02psm0.Assign(1.0, 0.0, 0.0,
0.0, 0.0, 1.0,
0.0, -1.0, 0.0);
vctMatRot3 psm6tomtm7;
psm6tomtm7.Assign(1.0, 0.0, 0.0,
0.0, 0.0, -1.0,
0.0, 1.0, 0.0);
if (is_enabled_) {
// assign current psm pose
psm_pose_cmd_.Assign(psm_pose_cur_);
// translation
double scale = 0.2;
vct3 mtm_tra = mtm_pose_cur_.Translation() - mtm_pose_pre_.Translation();
vct3 psm_tra = scale * mtm_tra;
psm_pose_cmd_.Translation() = psm_pose_cmd_.Translation() + mtm2psm * psm_tra;
// rotation
vctMatRot3 psm_motion_rot;
//psm_motion_rot = mtm2psm * mtm_pose_cur_.Rotation() * psm6tomtm7;
psm_motion_rot = mtm02psm0* mtm_pose_cur_.Rotation() * psm6tomtm7;
psm_pose_cmd_.Rotation().FromNormalized(psm_motion_rot);
// std::cerr << " teleop enabled " << counter_ << std::endl;
} else {
// In this mode, MTM orientation follows PSM orientation
// keep current pose
psm_pose_cmd_.Assign(psm_pose_cur_);
// mtm needs to follow psm orientation
mtm_pose_cmd_.Assign(mtm_pose_cur_);
vctMatRot3 mtm_rot_cmd;
mtm_rot_cmd = mtm02psm0.Inverse() * psm_pose_cur_.Rotation() * psm6tomtm7.Inverse();
mtm_pose_cmd_.Rotation().FromNormalized(mtm_rot_cmd);
}
mtsCISSTToROS(psm_pose_cmd_, msg_psm_pose_);
pub_psm_pose_.publish(msg_psm_pose_);
mtsCISSTToROS(mtm_pose_cmd_, msg_mtm_pose_);
pub_mtm_pose_.publish(msg_mtm_pose_);
// save current pose as previous
mtm_pose_pre_.Assign(mtm_pose_cur_);
#if 0
if (counter_%10 == 0) {
std::cerr << " mtm = " << std::endl
<< mtm_pose_cur_ << std::endl;
std::cerr << "psm = " << std::endl
<< psm_pose_cur_ << std::endl << std::endl;
}
#endif
}
void mtsAtracsysFusionTrack::Run(void)
{
// process mts commands
ProcessQueuedCommands();
// get latest frame from fusion track library/device
if (ftkGetLastFrame(Internals->Library,
Internals->Device,
&(Internals->Frame),
0 ) != FTK_OK) {
CMN_LOG_CLASS_RUN_DEBUG << "Run: timeout on ftkGetLastFrame" << std::endl;
return;
}
// check results of last frame
switch (Internals->Frame.markersStat) {
case QS_WAR_SKIPPED:
CMN_LOG_CLASS_RUN_ERROR << "Run: marker fields in the frame are not set correctly" << std::endl;
case QS_ERR_INVALID_RESERVED_SIZE:
CMN_LOG_CLASS_RUN_ERROR << "Run: frame.markersVersionSize is invalid" << std::endl;
default:
CMN_LOG_CLASS_RUN_ERROR << "Run: invalid status" << std::endl;
case QS_OK:
break;
}
// make sure we're not getting more markers than allocated
size_t count = Internals->Frame.markersCount;
if (count > Internals->NumberOfMarkers) {
CMN_LOG_CLASS_RUN_WARNING << "Run: marker overflow, please increase number of markers. Only the first "
<< Internals->NumberOfMarkers << " marker(s) will processed." << std::endl;
count = Internals->NumberOfMarkers;
}
// initialize all tools
const ToolsType::iterator end = Tools.end();
ToolsType::iterator iter;
for (iter = Tools.begin(); iter != end; ++iter) {
iter->second->StateTable.Start();
iter->second->Position.SetValid(false);
}
// for each marker, get the data and populate corresponding tool
for (size_t index = 0; index < count; ++index) {
ftkMarker * currentMarker = &(Internals->Markers[index]);
// find the appropriate tool
if (Internals->GeometryIdToTool.find(currentMarker->geometryId) == Internals->GeometryIdToTool.end()) {
CMN_LOG_CLASS_RUN_WARNING << "Run: found a geometry Id not registered using AddTool, this marker will be ignored ("
<< this->GetName() << ")" << std::endl;
}
else {
mtsAtracsysFusionTrackTool * tool = Internals->GeometryIdToTool.at(currentMarker->geometryId);
tool->Position.SetValid(true);
tool->Position.Position().Translation().Assign(currentMarker->translationMM[0],
currentMarker->translationMM[1],
currentMarker->translationMM[2]);
for (size_t row = 0; row < 3; ++row) {
for (size_t col = 0; col < 3; ++col) {
tool->Position.Position().Rotation().Element(row, col) = currentMarker->rotation[row][col];
}
}
tool->RegistrationError = currentMarker->registrationErrorMM;
/*
printf("Marker:\n");
printf("XYZ (%.2f %.2f %.2f)\n",
currentMarker->translationMM[0],
currentMarker->translationMM[1],
currentMarker->translationMM[2]);
*/
}
}
// finalize all tools
for (iter = Tools.begin(); iter != end; ++iter) {
iter->second->StateTable.Advance();
}
// ---- 3D Fiducials ---
switch (Internals->Frame.threeDFiducialsStat)
{
case QS_WAR_SKIPPED:
CMN_LOG_CLASS_RUN_ERROR << "Run: 3D status fields in the frame is not set correctly" << std::endl;
case QS_ERR_INVALID_RESERVED_SIZE:
CMN_LOG_CLASS_RUN_ERROR << "Run: frame.threeDFiducialsVersionSize is invalid" << std::endl;
default:
CMN_LOG_CLASS_RUN_ERROR << "Run: invalid status" << std::endl;
case QS_OK:
break;
}
ThreeDFiducialPosition.clear();
NumberOfThreeDFiducials = Internals->Frame.threeDFiducialsCount;
ThreeDFiducialPosition.resize(NumberOfThreeDFiducials);
//printf("3D fiducials:\n");
for (uint32 m = 0; m < NumberOfThreeDFiducials; m++)
{
/*
printf("\tINDEXES (%u %u)\t XYZ (%.2f %.2f %.2f)\n\t\tEPI_ERR: %.2f\tTRI_ERR: %.2f\tPROB: %.2f\n",
Internals->threedFiducials[m].leftIndex,
Internals->threedFiducials[m].rightIndex,
Internals->threedFiducials[m].positionMM.x,
Internals->threedFiducials[m].positionMM.y,
Internals->threedFiducials[m].positionMM.z,
Internals->threedFiducials[m].epipolarErrorPixels,
Internals->threedFiducials[m].triangulationErrorMM,
Internals->threedFiducials[m].probability);
*/
ThreeDFiducialPosition[m].X() = Internals->threedFiducials[m].positionMM.x;
ThreeDFiducialPosition[m].Y() = Internals->threedFiducials[m].positionMM.y;
ThreeDFiducialPosition[m].Z() = Internals->threedFiducials[m].positionMM.z;
}
}
void mtsOptoforce3D::Run(void)
{
struct optopacket {
unsigned char header[4];
unsigned short count;
unsigned short status;
short fx;
short fy;
short fz;
unsigned short checksum;
};
union PacketDataType {
unsigned char bytes[16];
optopacket packet;
};
PacketDataType buffer;
ProcessQueuedCommands();
if (connected) {
bool found = false;
unsigned short recvChecksum;
#if (CISST_OS == CISST_WINDOWS)
// On Windows, serialPort.Read seems to always return the requested number
// of characters, which is sizeof(buffer).
// Thus, we have to check whether part of the expected packet has been combined
// with another packet, such as the 7 byte response to the command sent to the sensor.
int n = serialPort.Read((char *)&buffer, sizeof(buffer));
while (!found) {
for (int i = 0; i < n - 3; i++) {
if ((buffer.bytes[i] == 170) && (buffer.bytes[i + 1] == 7)
&& (buffer.bytes[i + 2] == 8) && (buffer.bytes[i + 3] == 10)) {
if (i != 0) { // If pattern not found at beginning of buffer
memmove(buffer.bytes, buffer.bytes + i, n - i); // shift so that 170 is in buffer[0]
serialPort.Read(buffer.bytes + n - i, i); // fill the rest of the buffer
}
found = true;
break;
}
}
if (!found) { // If pattern not yet found
memmove(buffer.bytes, buffer.bytes + n - 4, 4); // move last 4 characters to beginning of buffer
serialPort.Read(buffer.bytes + 4, sizeof(buffer.bytes) - 4); // get another 12 characters
}
}
// Now, process the data
RawSensor.X() = (double)static_cast<short>(_byteswap_ushort(buffer.packet.fx)) * scale.X();
RawSensor.Y() = (double)static_cast<short>(_byteswap_ushort(buffer.packet.fy)) * scale.Y();
RawSensor.Z() = (double)static_cast<short>(_byteswap_ushort(buffer.packet.fz)) * scale.Z();
Count = _byteswap_ushort(buffer.packet.count);
Status = _byteswap_ushort(buffer.packet.status);
recvChecksum = _byteswap_ushort(buffer.packet.checksum);
#else
// On Linux, serialPort.Read seems to return a complete packet, even if it is less than the
// requested size.
// Thus, we can discard packets that are not the correct size.
while (!found) {
if (serialPort.Read((char *)&buffer, sizeof(buffer)) == sizeof(buffer)) {
// Check for expected 4 byte packet header
found = ((buffer.bytes[0] == 170) && (buffer.bytes[1] == 7)
&& (buffer.bytes[2] == 8) && (buffer.bytes[3] == 10));
}
}
// Now, process the data
RawSensor.X() = (double)static_cast<short>(bswap_16(buffer.packet.fx)) * scale.X();
RawSensor.Y() = (double)static_cast<short>(bswap_16(buffer.packet.fy)) * scale.Y();
RawSensor.Z() = (double)static_cast<short>(bswap_16(buffer.packet.fz)) * scale.Z();
Count = bswap_16(buffer.packet.count);
Status = bswap_16(buffer.packet.status);
recvChecksum = bswap_16(buffer.packet.checksum);
#endif
// Verify the checksum (last 2 bytes).
unsigned short checksum = buffer.bytes[0];
for (size_t i = 1; i < sizeof(buffer) - 2; i++)
checksum += buffer.bytes[i];
// (Status == 0) means no errors or overload warnings.
// For now, we check ((Status&0xFC00) == 0), which ignores overload warnings.
bool valid = (checksum == recvChecksum) && ((Status & 0xFC00) == 0);
ForceTorque.SetValid(valid);
if (valid) {
if (calib_valid) {
Force = RawSensor;
}
else if (matrix_a_valid) {
// Obtain forces by applying the calibration matrix, which depends on sensor-specific calibration
// values (A) and the assumed length to where the forces are applied (Length), which determines matrix_l (L).
// The calibration matrix, matrix_cal, is equal to inv(A*L)
Force = matrix_cal*RawSensor - bias; // F = inv(A*L)*S - bias
// Stablize the sensor readings
ForceCurrent.Assign(Force);
for (int i=0; i<3; i++) {
if(fabs(ForceCurrent.Element(i)) < 0.2)
ForceCurrent.Element(i) = 0.0;
}
Force.Assign(ForceCurrent);
for (int i=0; i<3; i++)
Force.Element(i) = 0.5*ForceCurrent.Element(i) +
0.5*ForcePrevious.Element(i);
ForcePrevious.Assign(Force);
}
else {
Force = RawSensor - bias;
}
ForceTorque.SetForce(vctDouble6(Force.X(), Force.Y(), Force.Z(), 0.0, 0.0, 0.0));
}
}
else {
ForceTorque.SetValid(false);
osaSleep(0.1); // If not connected, wait
}
}
void Run(){
ProcessQueuedCommands();
if( IsEnabled() ){
if( !online ){
online = true;
for( size_t i=0; i<qready.size(); i++ ){
if( 0.015 < fabs( q[i] - qready[i] ) ){
online = false;
if( q[i] < qready[i] )
{ q[i] += 0.0005; }
else if( qready[i] < q[i] )
{ q[i] -= 0.0005; }
}
}
std::cout << q << std::endl;
// this goes to the ikin
mtsFrm4x4 mtsRt( manipulator->ForwardKinematics( q ) );
SetPositionCartesian( mtsRt );
// this goes to the teleop
prmTelemetry.Position().FromRaw( mtsRt );
prmTelemetry.SetValid( true );
if( online )
{ std::cout << "System is online. 'q' to quit" << std::endl; }
}
else{
bool valid = false;
prmCommand.GetValid( valid );
if( valid ){
vctFrm3 frm3Rt;
prmCommand.GetPosition( frm3Rt );
// this goes to the ikin
vctQuaternionRotation3<double> R( frm3Rt.Rotation(), VCT_NORMALIZE );
mtsFrm4x4 mtsRt( vctFrame4x4<double>( R, frm3Rt.Translation() ) );
SetPositionCartesian( mtsRt );
// return the same value to the telop
prmTelemetry.Position().FromRaw( mtsRt );
prmTelemetry.SetValid( true );
}
}
}
}
void mtsTeleOperation::Run(void)
{
ProcessQueuedCommands();
ProcessQueuedEvents();
// increment counter
Counter++;
// get master Cartesian position
mtsExecutionResult executionResult;
executionResult = Master.GetPositionCartesian(Master.PositionCartesianCurrent);
if (!executionResult.IsOK()) {
CMN_LOG_CLASS_RUN_ERROR << "Run: call to Master.GetPositionCartesian failed \""
<< executionResult << "\"" << std::endl;
MessageEvents.Error(this->GetName() + ": unable to get cartesian position from master");
this->Enable(false);
}
vctFrm4x4 masterPosition(Master.PositionCartesianCurrent.Position());
// get slave Cartesian position
executionResult = Slave.GetPositionCartesian(Slave.PositionCartesianCurrent);
if (!executionResult.IsOK()) {
CMN_LOG_CLASS_RUN_ERROR << "Run: call to Slave.GetPositionCartesian failed \""
<< executionResult << "\"" << std::endl;
MessageEvents.Error(this->GetName() + ": unable to get cartesian position from slave");
this->Enable(false);
}
/*!
mtsTeleOperation can run in 4 control modes, which is controlled by
footpedal Clutch & OperatorPresent.
Mode 1: OperatorPresent = False, Clutch = False
MTM and PSM stop at their current position. If PSM ManipClutch is
pressed, then the user can manually move PSM.
NOTE: MTM always tries to allign its orientation with PSM's orientation
Mode 2/3: OperatorPresent = False/True, Clutch = True
MTM can move freely in workspace, however its orientation is locked
PSM can not move
Mode 4: OperatorPresent = True, Clutch = False
PSM follows MTM motion
*/
if (IsEnabled
&& Master.PositionCartesianCurrent.Valid()
&& Slave.PositionCartesianCurrent.Valid()) {
// follow mode
if (!IsClutched && IsOperatorPresent) {
// compute master Cartesian motion
vctFrm4x4 masterCartesianMotion;
masterCartesianMotion = Master.CartesianPrevious.Inverse() * masterPosition;
// translation
vct3 masterTranslation;
vct3 slaveTranslation;
if (this->TranslationLocked) {
slaveTranslation = Slave.CartesianPrevious.Translation();
} else {
masterTranslation = (masterPosition.Translation() - Master.CartesianPrevious.Translation());
slaveTranslation = masterTranslation * this->Scale;
slaveTranslation = RegistrationRotation * slaveTranslation + Slave.CartesianPrevious.Translation();
}
// rotation
vctMatRot3 slaveRotation;
if (this->RotationLocked) {
slaveRotation.From(Slave.CartesianPrevious.Rotation());
} else {
slaveRotation = RegistrationRotation * masterPosition.Rotation();
}
// compute desired slave position
vctFrm4x4 slaveCartesianDesired;
slaveCartesianDesired.Translation().Assign(slaveTranslation);
slaveCartesianDesired.Rotation().FromNormalized(slaveRotation);
Slave.PositionCartesianDesired.Goal().FromNormalized(slaveCartesianDesired);
// Slave go this cartesian position
Slave.SetPositionCartesian(Slave.PositionCartesianDesired);
// Gripper
if (Master.GetGripperPosition.IsValid()) {
double gripperPosition;
Master.GetGripperPosition(gripperPosition);
Slave.SetJawPosition(gripperPosition);
} else {
Slave.SetJawPosition(5.0 * cmnPI_180);
}
} else if (!IsClutched && !IsOperatorPresent) {
// Do nothing
}
} else {
CMN_LOG_CLASS_RUN_DEBUG << "mtsTeleOperation disabled" << std::endl;
}
}
void mtsMicroScribeDigitizer::Run(void)
{
// timestamp of last reconnect trial in case of disconnection
static double lastReconnectTryTime = 0;
ProcessQueuedCommands();
// If not connected, try reconnect at every 1 sec
if (!DeviceConnected) {
if (lastReconnectTryTime == 0) {
lastReconnectTryTime = osaGetTime();
} else if (lastReconnectTryTime + 1.0 * cmn_s < osaGetTime()) {
CMN_LOG_CLASS_INIT_VERBOSE << "Run: Try reconnecting to digitizer..." << std::endl;
if (ARM_SUCCESS == ArmReconnect()) {
OnDeviceConnection();
} else {
CMN_LOG_CLASS_INIT_VERBOSE << "Run: Failed to reconnect to digitizer." << std::endl;
}
}
return;
}
// read digitizer tip position
if (ARM_NOT_CONNECTED == ArmGetTipPosition(&TipPositionVendor)) {
// Disconnection detected
OnDeviceDisconnection();
return;
}
// read digitizer tip orientation
if (ARM_NOT_CONNECTED == ArmGetTipOrientation(&TipOrientationVendor)) {
// Disconnection detected
OnDeviceDisconnection();
return;
}
// read digitizer tip orientation unit vector
if (ARM_NOT_CONNECTED == ArmGetTipOrientationUnitVector(&TipOrientationUnitVectorVendor)) {
// Disconnection detected
OnDeviceDisconnection();
return;
}
// read digitizer button states
if (ARM_NOT_CONNECTED == ArmGetButtonsState(&ButtonStateVendor)) {
// Disconnection detected
OnDeviceDisconnection();
return;
}
// read digitizer joint readings
if (ARM_NOT_CONNECTED == ArmGetJointAngles(ARM_DEGREES, // or ARM_RADIANS
JointReadingsVendor))
{
// Disconnection detected
OnDeviceDisconnection();
return;
}
// Convert vendor-defined container to cisst container
// position
TipPosition(X) = TipPositionVendor.x;
TipPosition(Y) = TipPositionVendor.y;
TipPosition(Z) = TipPositionVendor.z;
// orientation
TipOrientation(ROLL) = TipOrientationVendor.x;
TipOrientation(PITCH) = TipOrientationVendor.y;
TipOrientation(YAW) = TipOrientationVendor.z;
// orientation unit vector
TipOrientationUnitVector(ROLL) = TipOrientationUnitVectorVendor.x;
TipOrientationUnitVector(PITCH) = TipOrientationUnitVectorVendor.y;
TipOrientationUnitVector(YAW) = TipOrientationUnitVectorVendor.z;
// button state
static DWORD lastButtonStateVendor = 0;
bool buttonStateChange = false;
if (!(lastButtonStateVendor & ARM_BUTTON_1) && (ButtonStateVendor & ARM_BUTTON_1)) {
EventButton1Down();
ButtonState(BUTTON_1) = DOWN;
buttonStateChange = true;
} else if ((lastButtonStateVendor & ARM_BUTTON_1) && !(ButtonStateVendor & ARM_BUTTON_1)) {
EventButton1Up();
ButtonState(BUTTON_1) = UP;
buttonStateChange = true;
}
if (!(lastButtonStateVendor & ARM_BUTTON_2) && (ButtonStateVendor & ARM_BUTTON_2)) {
EventButton2Down();
ButtonState(BUTTON_2) = DOWN;
buttonStateChange = true;
} else if ((lastButtonStateVendor & ARM_BUTTON_2) && !(ButtonStateVendor & ARM_BUTTON_2)) {
EventButton2Up();
ButtonState(BUTTON_2) = UP;
buttonStateChange = true;
}
if (buttonStateChange) {
lastButtonStateVendor = ButtonStateVendor;
}
// Joint readings
for (size_t i = 0; i < NUM_DOF; ++i) {
JointReadings(i) = JointReadingsVendor[i];
}
}
void mtsIntuitiveResearchKitConsole::Run(void)
{
ProcessQueuedCommands();
ProcessQueuedEvents();
}
void guiTask::Run(void){
ProcessQueuedEvents();
ProcessQueuedCommands();
if(clarityClientUI.beepClicked){
clarityClientUI.beepClicked = false;
mtsInt numberOfBeeps = (int)clarityClientUI.beepCount->value();
NDI.Beep(numberOfBeeps);
}
if(clarityClientUI.initializeClicked){
clarityClientUI.initializeClicked = false;
NDI.Initialize();
NDI.Query();
NDI.Enable();
}
if(clarityClientUI.trackClicked){
clarityClientUI.trackClicked = false;
NDI.Track(trackToggle);
trackToggle = !trackToggle;
}
RobotBase.GetPosition(robotBasePosition);
RobotBaseQuaternion.W() = robotBasePosition.Element(3);
RobotBaseQuaternion.X() = robotBasePosition.Element(4);
RobotBaseQuaternion.Y() = robotBasePosition.Element(5);
RobotBaseQuaternion.Z() = robotBasePosition.Element(6);
RobotBaseFrame.Rotation().FromRaw(RobotBaseQuaternion);
r32 = RobotBaseFrame.Rotation().Element(2,1);
r33 = RobotBaseFrame.Rotation().Element(2,2);
r31 = RobotBaseFrame.Rotation().Element(2,0);
r21 = RobotBaseFrame.Rotation().Element(1,0);
r11 = RobotBaseFrame.Rotation().Element(0,0);
RobotBaseEulerAngles.Element(0) = atan2(r32,r33)*cmn180_PI;
RobotBaseEulerAngles.Element(1) = atan2(-r31,sqrt(pow(r32,2.0) + pow(r33,2.0)))*cmn180_PI;
RobotBaseEulerAngles.Element(2) = atan2(r21,r11)*cmn180_PI;
RobotTip.GetPosition(robotTipPosition);
RobotTipQuaternion.W() = robotTipPosition.Element(3);
RobotTipQuaternion.X() = robotTipPosition.Element(4);
RobotTipQuaternion.Y() = robotTipPosition.Element(5);
RobotTipQuaternion.Z() = robotTipPosition.Element(6);
RobotTipFrame.Rotation().FromRaw(RobotTipQuaternion);
r32 = RobotTipFrame.Rotation().Element(2,1);
r33 = RobotTipFrame.Rotation().Element(2,2);
r31 = RobotTipFrame.Rotation().Element(2,0);
r21 = RobotTipFrame.Rotation().Element(1,0);
r11 = RobotTipFrame.Rotation().Element(0,0);
RobotTipEulerAngles.Element(0) = atan2(r32,r33)*cmn180_PI;
RobotTipEulerAngles.Element(1) = atan2(-r31,sqrt(pow(r32,2.0) + pow(r33,2.0)))*cmn180_PI;
RobotTipEulerAngles.Element(2) = atan2(r21,r11)*cmn180_PI;
for (unsigned int i=0; i<6; i++){
if(robotBasePosition.Element(0) != 999.9){
if(i<3)
clarityClientUI.BasePosition[i]->value(robotBasePosition.Element(i));
else
clarityClientUI.BasePosition[i]->value(RobotBaseEulerAngles.Element(i-3));
}
else
clarityClientUI.BasePosition[i]->value(99999.999);
if(robotTipPosition.Element(0) != 999.9){
if(i<3)
clarityClientUI.TipPosition[i]->value(robotTipPosition.Element(i));
else
clarityClientUI.TipPosition[i]->value(RobotTipEulerAngles.Element(i-3));
}
else
clarityClientUI.TipPosition[i]->value(99999.999);
}
if (Fl::check() == 0)
Kill();
}
