void mtsRobotIO1394QtWidget::Init(void)
{
DummyValueWhenNotConnected = 0;
LastEnableState.SetSize(NumberOfActuators);
LastEnableState.SetAll(false);
UnitFactor.SetSize(NumberOfActuators);
JointPosition.SetSize(NumberOfActuators);
ActuatorPosition.SetSize(NumberOfActuators);
ActuatorPositionGet.Position().SetSize(NumberOfActuators);
ActuatorVelocity.SetSize(NumberOfActuators);
PotentiometersVolts.SetSize(NumberOfActuators);
PotentiometersPosition.SetSize(NumberOfActuators);
ActuatorFeedbackCurrent.SetSize(NumberOfActuators);
ActuatorFeedbackCurrent.Zeros();
ActuatorRequestedCurrent.SetSize(NumberOfActuators);
ActuatorRequestedCurrent.Zeros();
ActuatorAmpTemperature.SetSize(NumberOfActuators);
StartTime = osaGetTime();
SetupCisstInterface();
setupUi();
startTimer(TimerPeriodInMilliseconds); // ms
}
int main( int argc, char** argv ){
mlockall(MCL_CURRENT | MCL_FUTURE);
RT_TASK task;
rt_task_shadow( &task, "GroupTest", 99, 0 );
cmnLogger::SetMask( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL );
if( argc != 2 ){
std::cout << "Usage: " << argv[0] << " rtcan[0-1]" << std::endl;
return -1;
}
osaRTSocketCAN can( argv[1], osaCANBus::RATE_1000 );
if( can.Open() != osaCANBus::ESUCCESS ){
std::cerr << argv[0] << "Failed to open " << argv[1] << std::endl;
return -1;
}
osaBH8_280 BH( &can );
if( BH.Initialize() != osaBH8_280::ESUCCESS ){
std::cerr << "Failed to initialize WAM" << std::endl;
return -1;
}
std::cout << "\n\n\n\n";
double t1 = osaGetTime();
size_t cnt=0;
while( 1 ){
Eigen::VectorXd q( 4, 0.1 );
BH.GetPositions( q );
//BH.SetPositions( q );
}
cmnGetChar();
return 0;
}
//#define _COMMUNICATION_TEST_
void mtsManagerProxyClient::ManagerClientI::Run()
{
#ifdef _COMMUNICATION_TEST_
int count = 0;
while (IsActiveProxy()) {
osaSleep(1 * cmn_s);
std::cout << "\tClient [" << ManagerProxyClient->GetProxyName() << "] running (" << ++count << ")" << std::endl;
std::stringstream ss;
ss << "Msg " << count << " from Client " << ManagerProxyClient->GetProxyName();
ManagerProxyClient->SendTestMessageFromClientToServer(ss.str());
}
#else
double lastTickChecked = 0.0, now;
while (IsActiveProxy()) {
now = osaGetTime();
if (now < lastTickChecked + mtsProxyConfig::RefreshPeriodForManagers) {
osaSleep(10 * cmn_ms);
continue;
}
lastTickChecked = now;
try {
Server->Refresh();
} catch (const ::Ice::Exception & ex) {
LogError(mtsManagerProxyClient, "refresh failed (" << Server->ice_toString() << ")" << std::endl << ex);
if (ManagerProxyClient) {
ManagerProxyClient->OnServerDisconnect(ex);
}
}
}
#endif
#ifdef ENABLE_DETAILED_MESSAGE_EXCHANGE_LOG
LogPrint(mtsManagerProxyClient, "mtsManagerProxyClient::ManagerClientI - terminated");
#endif
}
int main( int argc, char** argv ){
cmnLogger::SetMask( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL );
if( argc != 2 ){
std::cout << "Usage: " << argv[0] << " can[0-1]" << std::endl;
return -1;
}
osaSocketCAN can( argv[1], osaCANBus::RATE_1000 );
if( can.Open() != osaCANBus::ESUCCESS ){
CMN_LOG_RUN_ERROR << argv[0] << "Failed to open " << argv[1] << std::endl;
return -1;
}
osaBH8_280 BH( &can );
if( BH.Initialize() != osaBH8_280::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to initialize WAM" << std::endl;
return -1;
}
std::cout << "\n\n\n\n";
double t1 = osaGetTime();
size_t cnt=0;
while( 1 ){
vctDynamicVector<double> q( 4, 0.1 );
BH.GetPositions( q );
//BH.SetPositions( q );
}
cmnGetChar();
return 0;
}
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];
}
}
int main( int argc, char** argv ){
#if (CISST_OS == CISST_LINUX_XENOMAI)
mlockall(MCL_CURRENT | MCL_FUTURE);
RT_TASK task;
rt_task_shadow( &task, "GroupTest", 99, 0 );
#endif
cmnLogger::SetMask( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL );
if( argc != 2 ){
std::cout << "Usage: " << argv[0] << " can[0-1]" << std::endl;
return -1;
}
#if (CISST_OS == CISST_LINUX_XENOMAI)
osaRTSocketCAN can( argv[1], osaCANBus::RATE_1000 );
#else
osaSocketCAN can( argv[1], osaCANBus::RATE_1000 );
#endif
if( can.Open() != osaCANBus::ESUCCESS ){
CMN_LOG_RUN_ERROR << argv[0] << "Failed to open " << argv[1] << std::endl;
return -1;
}
osaWAM WAM( &can );
if( WAM.Initialize() != osaWAM::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to initialize WAM" << std::endl;
return -1;
}
vctDynamicVector<double> qinit( 7, 0.0 );
qinit[1] = -cmnPI_2;
qinit[3] = cmnPI;
if( WAM.SetPositions( qinit ) != osaWAM::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to set position: " << qinit << std::endl;
return -1;
}
cmnPath path;
path.AddRelativeToCisstShare("/models/WAM");
std::string fname = path.Find("wam7.rob", cmnPath::READ);
// Rotate the base
vctMatrixRotation3<double> Rw0( 0.0, 0.0, -1.0,
0.0, 1.0, 0.0,
1.0, 0.0, 0.0 );
vctFixedSizeVector<double,3> tw0(0.0);
vctFrame4x4<double> Rtw0( Rw0, tw0 );
// Gain matrices
vctDynamicMatrix<double> Kp(7, 7, 0.0), Kd(7, 7, 0.0);
Kp[0][0] = 250; Kd[0][0] = 3.0;
Kp[1][1] = 250; Kd[1][1] = 3.0;
Kp[2][2] = 250; Kd[2][2] = 3.0;
Kp[3][3] = 200; Kd[3][3] = 3;
Kp[4][4] = 50; Kd[4][4] = 0.8;
Kp[5][5] = 50; Kd[5][5] = 0.8;
Kp[6][6] = 10; Kd[6][6] = .1;
osaPDGC PDGC( fname, Rtw0, Kp, Kd, qinit );
std::cout << "Activate the WAM" << std::endl;
bool activated = false;
double t1 = osaGetTime();
while( 1 ){
// Get the positions
vctDynamicVector<double> q;
if( WAM.GetPositions( q ) != osaWAM::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to get positions" << std::endl;
return -1;
}
// Check if the pucks are activated
if( !activated ) {
osaWAM::Mode mode;
if( WAM.GetMode( mode ) != osaWAM::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to get mode" << std::endl;
return -1;
}
if( mode == osaWAM::MODE_ACTIVATED )
{ activated = true; }
}
// if pucks are activated, run the controller
vctDynamicVector<double> tau( q.size(), 0.0 );
double t2 = osaGetTime();
if( activated ){
if( PDGC.Evaluate( qinit, q, tau, t2-t1 ) != osaPDGC::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to evaluate controller" << std::endl;
return -1;
}
}
t1 = t2;
// apply torques
if( WAM.SetTorques( tau ) != osaWAM::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to set torques" << std::endl;
return -1;
}
std::cout << "q: " << q << std::endl;
std::cout << "tau: " << tau << std::endl;
}
return 0;
}
