void mtsMicroScribeDigitizer::InitComponent(void)
{
mtsInterfaceProvided * provided = AddInterfaceProvided(DigitizerInterfaceName);
if (provided) {
StateTable.AddData(DeviceStatus, "DeviceStatus");
StateTable.AddData(TipPosition, "TipPosition");
StateTable.AddData(TipOrientation, "TipOrientation");
StateTable.AddData(TipOrientationUnitVector, "TipOrientationUnitVector");
StateTable.AddData(ButtonState, "ButtonState");
StateTable.AddData(JointReadings, "JointReadings");
provided->AddCommandReadState(StateTable, DeviceStatus, DigitizerCommandNames::GetDeviceStatus);
provided->AddCommandReadState(StateTable, TipPosition, DigitizerCommandNames::GetTipPosition);
provided->AddCommandReadState(StateTable, TipOrientation, DigitizerCommandNames::GetTipOrientation);
provided->AddCommandReadState(StateTable, TipOrientationUnitVector, DigitizerCommandNames::GetTipOrientationUnitVector);
provided->AddCommandReadState(StateTable, ButtonState, DigitizerCommandNames::GetButtonState);
provided->AddCommandReadState(StateTable, JointReadings, DigitizerCommandNames::GetJointReadings);
provided->AddCommandRead(&mtsMicroScribeDigitizer::GetDigitizerInfo, this, DigitizerCommandNames::GetDigitizerInfo);
provided->AddEventVoid(EventButton1Up, DigitizerEventNames::EventButton1Up);
provided->AddEventVoid(EventButton1Down, DigitizerEventNames::EventButton1Down);
provided->AddEventVoid(EventButton2Up, DigitizerEventNames::EventButton2Up);
provided->AddEventVoid(EventButton2Down, DigitizerEventNames::EventButton2Down);
provided->AddEventVoid(EventDigitizerConnected, DigitizerEventNames::EventDigitizerConnected);
provided->AddEventVoid(EventDigitizerDisconnected, DigitizerEventNames::EventDigitizerDisconnected);
}
// Device initialization
if (ARM_SUCCESS != ArmStart(NULL)) {
// Retry after calling ArmEnd()
ArmEnd();
if (ARM_SUCCESS != ArmStart(NULL)) {
cmnThrow("mtsMicroScribeDigitizer: Startup: Failed to initialize MicroScribe Digitizer");
}
}
// Custom error handler registration
if (ARM_SUCCESS != ArmSetErrorHandlerFunction(NO_HCI_HANDLER, ErrorHandler_NoHCI)) {
cmnThrow("mtsMicroScribeDigitizer: Startup: Failed to register error handler: NO_HCI_HANDLER");
}
if (ARM_SUCCESS != ArmSetErrorHandlerFunction(BAD_PORT_HANDLER, ErrorHandler_BadPort)) {
cmnThrow("mtsMicroScribeDigitizer: Startup: Failed to register error handler: BAD_PORT_HANDLER");
}
if (ARM_SUCCESS != ArmSetErrorHandlerFunction(CANT_OPEN_HANDLER, ErrorHandler_CantOpen)) {
cmnThrow("mtsMicroScribeDigitizer: Startup: Failed to register error handler: CANT_OPEN_HANDLER");
}
if (ARM_SUCCESS != ArmSetErrorHandlerFunction(CANT_BEGIN_HANDLER, ErrorHandler_CantBegin)) {
cmnThrow("mtsMicroScribeDigitizer: Startup: Failed to register error handler: CANT_BEGIN_HANDLER");
}
}
/*! FillInTableauRefs
*/
void mtsVFSensorCompliance::FillInTableauRefs(const CONTROLLERMODE mode, const double TickTime)
{
// fill in refs
// min || J(q)*dq - gain*overallGain*sensorValues ||
// Check if we have a jacobian to use
if(Kinematics.size() < 1)
{
CMN_LOG_CLASS_RUN_ERROR << "Error: Sensor Compliance VF given improper input" << std::endl;
cmnThrow("Error: Sensor Compliance VF given improper input");
}
mtsVFDataSensorCompliance * SensorComplianceData = (mtsVFDataSensorCompliance *)(Data);
vctDoubleMat * JacP = &(Kinematics.at(0)->Jacobian);
vctDynamicVector<double> * SensorValues = &(Sensors.at(0)->Values);
vctDynamicVector<double> overallGain = Sensors.at(1)->Values;
//choose only selected sensor values (indices stored in SensorSelections)
vctDynamicVector<double> UsedValues(SensorComplianceData->Gain.cols());
for (size_t s=0; s < SensorComplianceData->SensorSelections.size(); s++)
{
UsedValues[s] = (overallGain[0]/100.0)*((*SensorValues)[SensorComplianceData->SensorSelections[s]]);
}
// Check if we have all dependencies met
if(Sensors.size() < 2 || SensorComplianceData->Gain.rows() != JacP->rows() || SensorComplianceData->Gain.cols() != UsedValues.size())
{
CMN_LOG_CLASS_RUN_ERROR << "Error: Sensor Compliance VF given improper input" << std::endl;
cmnThrow("Error: Sensor Compliance VF given improper input");
}
//set the reference to the right hand side of the above equation (gain*Force)
ObjectiveVectorRef.Assign(SensorComplianceData->Gain * UsedValues);
//now set reference to the left hand side of the above equation, the jacobian (only rows indicated in DOFSelections)
for(size_t r = 0; r < SensorComplianceData->DOFSelections.size(); r++)
{
for (size_t c = 0; c < JacP->cols(); c++)
{
if(r == c)
{
ObjectiveMatrixRef[r][c] = TickTime*(*JacP)[r][c];
}
}
}
ConvertRefs(mode,TickTime);
}
void nmrLSMinNorm::Data::CheckInfo() const{
if( INFO < 0 ){
std::ostringstream message;
message << "nmrLSMinNorm: The " << -INFO
<< "th argument had an illegal value."
<< std::endl;
cmnThrow( std::runtime_error( message.str() ) );
}
if( 0 < INFO ){
std::string message( "nmrLSMinNorm: SVD failed to converge\n" );
cmnThrow( std::runtime_error( message ) );
}
}
void cmnDataJSON<char>::DeSerializeText(DataType & data, const Json::Value & jsonValue) throw (std::runtime_error) {
std::string temp = jsonValue.asString();
if (temp.size() != 1) {
cmnThrow("cmnDataJSON<char>::DeSerializeText: string with more than one character");
}
data = temp[0];
}
void mtsPIDQtWidget::Startup(void)
{
CMN_LOG_CLASS_INIT_VERBOSE << "mtsPIDQtWidget::Startup" << std::endl;
// Set desired pos to cur pos
SlotResetPIDGain();
SlotMaintainPosition();
mtsExecutionResult result;
prmJointTypeVec jointType;
result = PID.GetJointType(jointType);
if (!result) {
CMN_LOG_CLASS_INIT_ERROR << "Startup: Robot interface isn't connected properly, unable to get joint type. Function call returned: "
<< result << std::endl;
UnitFactor.SetAll(0.0);
} else {
// set unitFactor;
for (size_t i = 0; i < this->NumberOfAxis; i++) {
if (jointType[i] == PRM_REVOLUTE) {
UnitFactor[i] = cmn180_PI;
} else if (jointType[i] == PRM_PRISMATIC) {
UnitFactor[i] = 1.0 / cmn_mm;
} else {
cmnThrow("mtsRobotIO1394QtWidget: Unknown joint type");
}
}
}
// Show the GUI
if (!parent()) {
show();
}
}
void cmnDataSerializeBinary(const std::string & data,
std::ostream & outputStream)
throw (std::runtime_error)
{
cmnData<size_t>::SerializeBinary(data.size(), outputStream);
outputStream.write(data.data(), data.size());
if (outputStream.fail()) {
cmnThrow("cmnDataSerializeBinary(std::string): error occured with std::ostream::write");
}
}
void cmnDataSerializeBinary_size_t(const size_t & data,
std::ostream & outputStream)
throw (std::runtime_error)
{
outputStream.write(reinterpret_cast<const char *>(&data),
sizeof(size_t));
if (outputStream.fail()) {
cmnThrow("cmnDataSerializeBinary(type): error occured with std::ostream::write");
}
}
void cmnDataSerializeText_size_t(const size_t & data,
std::ostream & outputStream,
const char CMN_UNUSED(delimiter))
throw (std::runtime_error)
{
outputStream << data;
if (outputStream.fail()) {
cmnThrow("cmnDataSerializeText_size_t: error occured with std::ostream::write");
}
}
void cmnDataDeSerializeBinary_size_t(size_t & data,
std::istream & inputStream,
const cmnDataFormat & localFormat,
const cmnDataFormat & remoteFormat)
throw (std::runtime_error)
{
// first case, both use same size
if (remoteFormat.GetSizeTSize() == localFormat.GetSizeTSize()) {
inputStream.read(reinterpret_cast<char *>(&data),
sizeof(size_t));
if (inputStream.fail()) {
cmnThrow("cmnDataDeSerializeBinary(size_t): error occured with std::istream::read");
}
return;
}
// different sizes
// local is 64, hence remote is 32
if (localFormat.GetSizeTSize() == cmnDataFormat::CMN_DATA_SIZE_T_SIZE_64) {
unsigned int tmp;
inputStream.read(reinterpret_cast<char *>(&tmp),
sizeof(tmp));
if (inputStream.fail()) {
cmnThrow("cmnDataDeSerializeBinary(size_t): error occured with std::istream::read");
}
data = tmp;
return;
}
// local is 32, hence remote is 64
if (localFormat.GetSizeTSize() == cmnDataFormat::CMN_DATA_SIZE_T_SIZE_32) {
unsigned long long int tmp;
inputStream.read(reinterpret_cast<char *>(&tmp),
sizeof(tmp));
if (inputStream.fail()) {
cmnThrow("cmnDataDeSerializeBinary(size_t): error occured with std::istream::read");
}
if (tmp > std::numeric_limits<size_t>::max()) {
cmnThrow("cmnDataDeSerializeBinary(size_t): received a size_t larger than what can be handled on 32 bits");
}
data = static_cast<size_t>(tmp); // this should be safe now
return;
}
}
void
nmrSymmetricEigenProblem::Data::CheckSystem
( const vctDynamicMatrix<double>& A,
const vctDynamicVector<double>& D,
const vctDynamicMatrix<double>& V ){
// test that number of rows match
if( A.rows() != A.cols() ){
std::ostringstream message;
message << "nmrSymmetricEigenProblem: Matrix A has " << A.rows() << " rows "
<< " and " << A.cols() << " columns.";
cmnThrow( std::runtime_error( message.str() ) );
}
// check for fortran format
if( !A.IsFortran() ){
std::string message( "nmrSymmetricEigenProblem: Invalid matrix A format." );
cmnThrow( std::runtime_error( message ) );
}
// test that number of rows match
if( V.rows() != A.rows() || V.cols() != A.cols() ){
std::ostringstream message;
message << "nmrSymmetricEigenProblem: Matrix V has " << V.rows() << " rows "
<< " and " << V.cols() << " columns.";
cmnThrow( std::runtime_error( message.str() ) );
}
if( !V.IsFortran() ){
std::string message( "nmrSymmetricEigenProblem: Invalid matrix V format." );
cmnThrow( std::runtime_error( message ) );
}
if( D.size() != A.rows() ){
std::ostringstream message;
message << "nmrSymmetricEigenProblem: Vector D has " << D.size() << " rows";
cmnThrow( std::runtime_error( message.str() ) );
}
}
void nmrLSMinNorm::Data::CheckSystem( const vctDynamicMatrix<double>& A,
const vctDynamicMatrix<double>& b ) const{
// test that number of rows match
if( A.rows() != b.rows() ){
std::ostringstream message;
message << "nmrLSMinNorm: Matrix A has " << A.rows() << " rows. "
<< "Matrix B has " << b.rows() << " rows.";
cmnThrow( std::runtime_error( message.str() ) );
}
// check for fortran format
if( !( A.IsFortran() ) ){
std::string message( "nmrLSMinNorm: Invalid matrix A format." );
cmnThrow( std::runtime_error( message ) );
}
if( !( b.IsFortran() ) ){
std::string message( "nmrLSMinNorm: Invalid matrix b format." );
cmnThrow( std::runtime_error( message ) );
}
}
void cmnDataDeSerializeBinary(std::string & data,
std::istream & inputStream,
const cmnDataFormat & localFormat,
const cmnDataFormat & remoteFormat)
throw (std::runtime_error)
{
size_t size;
// retrieve size of string
cmnData<size_t>::DeSerializeBinary(size, inputStream, localFormat, remoteFormat);
data.resize(size);
inputStream.read(const_cast<char *>(data.data()), size);
if (inputStream.fail()) {
cmnThrow("cmnDataDeSerializeBinary(std::string): error occured with std::istream::read");
}
}
void mtsRobotIO1394QtWidget::Startup(void)
{
CMN_LOG_CLASS_INIT_VERBOSE << "Startup" << std::endl;
vctDoubleVec actuatorCurrentMax(this->NumberOfActuators);
mtsExecutionResult result = Robot.GetActuatorCurrentMax(actuatorCurrentMax);
if (!result) {
CMN_LOG_CLASS_INIT_ERROR << "Startup: Robot interface isn't connected properly, unable to get actuator current max. Function call returned: "
<< result << std::endl;
} else {
// convert to mA
actuatorCurrentMax.Multiply(1000.0);
QVWActuatorCurrentSpinBoxWidget->SetRange(-actuatorCurrentMax, actuatorCurrentMax);
QVWActuatorCurrentSliderWidget->SetRange(-actuatorCurrentMax, actuatorCurrentMax);
}
prmJointTypeVec jointType;
result = Robot.GetJointType(jointType);
if (!result) {
CMN_LOG_CLASS_INIT_ERROR << "Startup: Robot interface isn't connected properly, unable to get joint type. Function call returned: "
<< result << std::endl;
UnitFactor.SetAll(0.0);
} else {
// set unitFactor;1
for (size_t i = 0; i < this->NumberOfActuators; i++ ) {
if (jointType[i] == PRM_REVOLUTE) {
UnitFactor[i] = cmn180_PI;
} else if (jointType[i] == PRM_PRISMATIC) {
UnitFactor[i] = 1.0 / cmn_mm; // convert internal values to mm
} else {
cmnThrow("mtsRobotIO1394QtWidget: Unknown joint type");
}
}
}
// get serial number
result = Robot.GetSerialNumber(SerialNumber);
if (!result) {
CMN_LOG_CLASS_INIT_ERROR << "Startup: Robot interface isn't connected properly, unable to get serial number. Function call returned: "
<< result << std::endl;
}
QLSerialNumber->setText(QString::number(SerialNumber));
if (!parent()) {
show();
}
}
size_t cmnDataDeSerializeBinary_size_t(size_t & data, const char * buffer, size_t bufferSize,
const cmnDataFormat & localFormat,
const cmnDataFormat & remoteFormat)
{
// first case, both use same size
if (remoteFormat.GetSizeTSize() == localFormat.GetSizeTSize()) {
const size_t sizeOfData = sizeof(size_t);
if (bufferSize < sizeOfData) {
return 0;
}
data = *(reinterpret_cast<const size_t *>(buffer));
bufferSize -= sizeOfData;
return sizeOfData;
}
// different sizes
// local is 64, hence remote is 32
if (localFormat.GetSizeTSize() == cmnDataFormat::CMN_DATA_SIZE_T_SIZE_64) {
unsigned int tmp;
const size_t sizeOfData = sizeof(unsigned int);
if (bufferSize < sizeOfData) {
return 0;
}
tmp = *(reinterpret_cast<const unsigned int *>(buffer));
bufferSize -= sizeOfData;
data = tmp;
return sizeOfData;
}
// local is 32, hence remote is 64
if (localFormat.GetSizeTSize() == cmnDataFormat::CMN_DATA_SIZE_T_SIZE_32) {
unsigned long long int tmp;
const size_t sizeOfData = sizeof(unsigned long long int);
if (bufferSize < sizeOfData) {
return 0;
}
tmp = *(reinterpret_cast<const unsigned long long int *>(buffer));
bufferSize -= sizeOfData;
if (tmp > std::numeric_limits<size_t>::max()) {
cmnThrow("cmnDataDeSerializeBinary(size_t): received a size_t larger than what can be handled on 32 bits");
}
data = static_cast<size_t>(tmp); // this should be safe now
return sizeOfData;
}
return 0;
}
mtsComponentViewer::mtsComponentViewer(const std::string & name) :
mtsTaskFromSignal(name),
UDrawPipeConnected(false),
UDrawResponse(""),
ShowProxies(false),
ConnectionStarted(false),
WaitingForResponse(false)
{
SetInitializationDelay(30.0); // Allow up to 30 seconds for it to start
mtsInterfaceRequired * required = EnableDynamicComponentManagement();
if (required) {
ManagerComponentServices->AddComponentEventHandler(&mtsComponentViewer::AddComponentHandler, this);
ManagerComponentServices->ChangeStateEventHandler(&mtsComponentViewer::ChangeStateHandler, this);
ManagerComponentServices->AddConnectionEventHandler(&mtsComponentViewer::AddConnectionHandler, this);
ManagerComponentServices->RemoveConnectionEventHandler(&mtsComponentViewer::RemoveConnectionHandler, this);
} else {
cmnThrow(std::runtime_error("mtsComponentViewer constructor: failed to enable dynamic component composition"));
}
}
mtsManagerComponentServer::mtsManagerComponentServer(mtsManagerGlobal * gcm)
: mtsManagerComponentBase(mtsManagerComponentBase::GetNameOfManagerComponentServer()),
GCM(gcm),
InterfaceGCMFunctionMap("InterfaceGCMFunctionMap")
{
// Prevent this component from being created more than once
// MJ: singleton can be implemented instead.
static int instanceCount = 0;
if (instanceCount != 0) {
cmnThrow(std::runtime_error("Error in creating manager component server: it's already created"));
}
gcm->SetMCS(this);
InterfaceGCMFunctionMap.SetOwner(*this);
// For system-wide thread-safe logging
mtsInterfaceProvided * provided = AddInterfaceProvided(
mtsManagerComponentBase::InterfaceNames::InterfaceSystemLoggerProvided);
if (provided) {
provided->AddEventWrite(this->EventPrintLog,
mtsManagerComponentBase::EventNames::PrintLog,
mtsLogMessage());
}
}
void mtsMicroScribeDigitizer::Configure(const std::string & filename)
{
// Setup error handlers
#define SETUP_ERROR_HANDLER(_errorCode, _handlerName)\
if (ARM_SUCCESS != ArmSetErrorHandlerFunction(_errorCode, ErrorHandler_##_handlerName)) {\
CMN_LOG_CLASS_INIT_ERROR << "Startup: Failed to set error handler: "#_errorCode << std::endl;\
cmnThrow("Startup: Failed to set error handler: "#_errorCode);\
}
SETUP_ERROR_HANDLER(NO_HCI_HANDLER, NoHCI);
SETUP_ERROR_HANDLER(BAD_PORT_HANDLER, BadPort);
SETUP_ERROR_HANDLER(CANT_OPEN_HANDLER, CantOpen);
SETUP_ERROR_HANDLER(CANT_BEGIN_HANDLER, CantBegin);
// MJ: TIMED_OUT_HANDLER and BAD_PACKET_HANDLER are not overridden because the SDK explicitly
// recommends not to do that as follows (see BAD_PACKET_handler and TIMED_OUT_handler):
//
// "Although this function is still exported for backward compatibility issue, user
// shouldn't have any reason to provide any custom TIMED_OUT_handler because the default
// ArmDll32's TIMED_OUT_handler should be enough. Any incorrect behavior in such custom
// error handler function could affect ArmDll32's performance."
#undef SETUP_ERROR_HANDLER
// Connect to hardware
if (ARM_SUCCESS != ArmConnect(0, 0)) {
CMN_LOG_CLASS_INIT_ERROR << "Startup: Unable to connect to ArmDll32" << std::endl;
ArmEnd();
cmnThrow("Startup: Unable to connect to ArmDll32");
} else {
OnDeviceConnection();
}
// Get device product information
const size_t len = 256;
char buf[len];
// Product name
if (ARM_SUCCESS == ArmGetProductName(buf, len)) {
DigitizerInfo.ProductName = buf;
} else {
DigitizerInfo.ProductName = "Failed to fetch";
}
// Model name
if (ARM_SUCCESS == ArmGetModelName(buf, len)) {
DigitizerInfo.ModelName = buf;
} else {
DigitizerInfo.ModelName = "Failed to fetch";
}
// Serial number
if (ARM_SUCCESS == ArmGetSerialNumber(buf, len)) {
DigitizerInfo.SerialNumber = buf;
} else {
DigitizerInfo.SerialNumber = "Failed to fetch";
}
// Driver and firmware version
char buf2[len];
if (ARM_SUCCESS == ArmGetVersion(buf, buf2, len)) {
DigitizerInfo.DriverVersion = buf;
DigitizerInfo.FirmwareVersion = buf2;
} else {
DigitizerInfo.DriverVersion = "Failed to fetch";
DigitizerInfo.FirmwareVersion = "Failed to fetch";
}
// Num of DoF
int dof;
if (ARM_SUCCESS == ArmGetNumDOF(&dof)) {
// -1 if DoF cannot be determined (e.g., pre-G2 versions of the MicroScribe)
DigitizerInfo.NumDoF = dof;
} else {
DigitizerInfo.NumDoF = -1;
}
CMN_LOG_CLASS_INIT_VERBOSE << "Startup: " << DigitizerInfo << std::endl;
// Set refresh rate
if (ARM_SUCCESS != ArmSetUpdateEx(ARM_FULL, (UINT) (this->Period * 1000))) { // second argument in msec
CMN_LOG_CLASS_INIT_ERROR << "Startup: Unable to set update for ArmDll32" << std::endl;
ArmDisconnect();
ArmEnd();
cmnThrow("Startup: Unable to set update for ArmDll32");
}
// Check device status (device can be disonncected after all getters succeeded)
if (ARM_SUCCESS == ArmGetDeviceStatus(&DeviceStatusVendor)) {
DeviceStatus(STATUS) = DeviceStatusVendor.status;
DeviceStatus(BAUD) = DeviceStatusVendor.Baud;
DeviceStatus(PORT_NUMBER) = DeviceStatusVendor.PortNumber;
if (!(DeviceStatus(STATUS) & ARM_CONNECTED)) {
OnDeviceDisconnection();
return;
}
}
}
