bool nmrRegistrationRigid(vctDynamicConstVectorBase<_vectorOwnerType, vct3> &dataSet1,
vctDynamicConstVectorBase<_vectorOwnerType, vct3> &dataSet2,
vctFrm3 &transform, double *fre)
{
size_t npoints = dataSet1.size();
size_t i;
if (npoints != dataSet2.size()) {
CMN_LOG_RUN_WARNING << "nmrRegistrationPairedPoint: incompatible sizes: " << npoints << ", "
<< dataSet2.size() << std::endl;
return false;
}
else if (npoints <= 0) {
CMN_LOG_RUN_WARNING << "nmrRegistrationPairedPoint called for " << npoints << " points." << std::endl;
return false;
}
// Compute averages
vct3 avg1 = dataSet1.SumOfElements();
avg1.Divide(static_cast<double>(npoints));
vct3 avg2 = dataSet2.SumOfElements();
avg2.Divide(static_cast<double>(npoints));
// Compute the sum of the outer products of (dataSet1-avg1) and (dataSet2-avg2)
vctDouble3x3 H, sumH;
sumH.SetAll(0.0);
for (i = 0; i < npoints; i++) {
H.OuterProductOf(dataSet1[i]-avg1, dataSet2[i]-avg2);
sumH.Add(H);
}
// Now, compute SVD of sumH
vctDouble3x3 U, Vt;
vctDouble3 S;
nmrSVD(sumH, U, S, Vt);
// Compute X = V*U' = (U*V')'
vctDouble3x3 X = (U*Vt).Transpose();
double det = vctDeterminant<3>::Compute(X);
// If determinant is not 1, apply correction from Umeyama(1991)
if (fabs(det-1.0) > 1e-6) {
vctDouble3x3 Fix(0.0);
Fix.Diagonal() = vct3(1.0, 1.0, -1.0);
X = (U*Fix*Vt).Transpose();
det = vctDeterminant<3>::Compute(X);
if (fabs(det-1.0) > 1e-6) {
CMN_LOG_RUN_WARNING << "nmrRegistrationPairedPoint: registration failed!!" << std::endl;
return false;
}
}
vctMatRot3 R;
R.Assign(X);
transform = vctFrm3(R, avg2-R*avg1);
// Now, compute residual error if fre is not null
if (fre) {
double err2 = 0.0;
for (i = 0; i < npoints; i++)
err2 += (dataSet2[i] - transform*dataSet1[i]).NormSquare();
*fre = sqrt(err2/npoints);
}
return true;
}
void RegistrationBehavior::OnStart(void)
{
// adjust scaling
//this->VisibleObject1->Actor->SetScale(0.25);
// adjust position offsets
vct6 bounds;
this->VisibleObject1->Lock();
bounds.Assign(this->VisibleObject1->Actor->GetBounds());
this->VisibleObject1->Unlock();
vct3 offset;
for (unsigned int i = 0; i < offset.size(); i++) {
offset[i] = (bounds[2*i] + bounds[(2*i)+1]) / -2.0;
}
this->VisibleObject1->Actor->AddPosition(offset.Pointer());
// adjust orientation offsets
double thetaX = -60.0 * cmnPI_180;
double thetaY = 0.0 * cmnPI_180;
double thetaZ = 0.0 * cmnPI_180;
vctRot3 rotateX(
1.0, 0.0, 0.0,
0.0, cos(thetaX), -sin(thetaX),
0.0, sin(thetaX), cos(thetaX));
vctRot3 rotateY(
cos(thetaY), 0.0, sin(thetaY),
0.0, 1.0, 0.0,
-sin(thetaY), 0.0, cos(thetaY));
vctRot3 rotateZ(
cos(thetaZ), -sin(thetaZ), 0.0,
sin(thetaZ), cos(thetaZ), 0.0,
0.0, 0.0, 1.0);
vctRot3 rotateZYX;
rotateZYX = rotateZ * rotateY * rotateX;
this->VisibleObject1->SetOrientation(rotateZYX);
this->ModelFiducials->SetPosition(vct3(0.0));
this->ModelFiducials->Show();
this->Position.Assign(0.0, 0.0, -500.0);
this->Widget3D->SetPosition(this->Position);
this->Widget3D->SetSize(150.0);
this->Widget3D->Show();
}
void mtsMedtronicStealthlink::Tool::Assign(const MNavStealthLink::DataItem & item_in)
{
if (typeid(*&item_in) == typeid(const MNavStealthLink::Instrument) ){
const MNavStealthLink::Instrument & tool_in = dynamic_cast<const MNavStealthLink::Instrument & >(item_in);
frameConversion(this->MarkerPosition.Position(),tool_in.localizer_T_instrument);
this->MarkerPosition.SetValid(tool_in.visibility == MNavStealthLink::Instrument::VISIBLE
|| tool_in.visibility == MNavStealthLink::Instrument::ALMOST_BLOCKED);
this->TooltipPosition.SetValid(tool_in.isCalibrated && this->MarkerPosition.Valid());
if (this->TooltipPosition.Valid()){
this->TooltipPosition.Position() = vctFrm3(this->MarkerPosition.Position().Rotation(),
vct3(tool_in.localizerPosition.tip.x,tool_in.localizerPosition.tip.y,tool_in.localizerPosition.tip.z));
//this->TooltipPosition.Position() = this->MarkerPosition.Position();
}
}else if(typeid(*&item_in) == typeid(const MNavStealthLink::Frame)){
const MNavStealthLink::Frame & frame_in = dynamic_cast<const MNavStealthLink::Frame & >(item_in);
frameConversion(this->MarkerPosition.Position(),frame_in.frame_T_localizer);
//Trek expects localizer_T_frame
this->MarkerPosition.Position() = this->MarkerPosition.Position().Inverse();
this->MarkerPosition.SetValid(frame_in.visibility == MNavStealthLink::Frame::VISIBLE
|| frame_in.visibility == MNavStealthLink::Frame::ALMOST_BLOCKED);
}
}
int main(int argc, char ** argv)
{
// configuration
const double periodIO = 0.5 * cmn_ms;
const double periodKinematics = 1.0 * cmn_ms;
const double periodTeleop = 1.0 * cmn_ms;
const double periodUDP = 20.0 * cmn_ms;
// log configuration
cmnLogger::SetMask(CMN_LOG_ALLOW_ALL);
cmnLogger::SetMaskDefaultLog(CMN_LOG_ALLOW_ALL);
cmnLogger::SetMaskFunction(CMN_LOG_ALLOW_ALL);
cmnLogger::SetMaskClassMatching("mtsIntuitiveResearchKit", CMN_LOG_ALLOW_ALL);
cmnLogger::AddChannel(std::cerr, CMN_LOG_ALLOW_ERRORS_AND_WARNINGS);
// parse options
cmnCommandLineOptions options;
int firewirePort = 0;
std::string gcmip = "-1";
std::string jsonMainConfigFile;
std::string jsonCollectionConfigFile;
typedef std::map<std::string, std::string> ConfigFilesType;
ConfigFilesType configFiles;
std::string masterName, slaveName;
options.AddOptionOneValue("j", "json-config",
"json configuration file",
cmnCommandLineOptions::REQUIRED_OPTION, &jsonMainConfigFile);
options.AddOptionOneValue("f", "firewire",
"firewire port number(s)",
cmnCommandLineOptions::OPTIONAL_OPTION, &firewirePort);
options.AddOptionOneValue("g", "gcmip",
"global component manager IP address",
cmnCommandLineOptions::OPTIONAL_OPTION, &gcmip);
options.AddOptionOneValue("c", "collection-config",
"json configuration file for data collection",
cmnCommandLineOptions::OPTIONAL_OPTION, &jsonCollectionConfigFile);
// check that all required options have been provided
std::string errorMessage;
if (!options.Parse(argc, argv, errorMessage)) {
std::cerr << "Error: " << errorMessage << std::endl;
options.PrintUsage(std::cerr);
return -1;
}
std::string arguments;
options.PrintParsedArguments(arguments);
std::cout << "Options provided:" << std::endl << arguments << std::endl;
// make sure the json config file exists and can be parsed
fileExists("JSON configuration", jsonMainConfigFile);
std::ifstream jsonStream;
jsonStream.open(jsonMainConfigFile.c_str());
Json::Value jsonConfig;
Json::Reader jsonReader;
if (!jsonReader.parse(jsonStream, jsonConfig)) {
std::cerr << "Error: failed to parse configuration\n"
<< jsonReader.getFormattedErrorMessages();
return -1;
}
// start initializing the cisstMultiTask manager
std::cout << "FirewirePort: " << firewirePort << std::endl;
std::string processname = "dvTeleop";
mtsManagerLocal * componentManager = 0;
if (gcmip != "-1") {
try {
componentManager = mtsManagerLocal::GetInstance(gcmip, processname);
} catch(...) {
std::cerr << "Failed to get GCM instance." << std::endl;
return -1;
}
} else {
componentManager = mtsManagerLocal::GetInstance();
}
// create a Qt application and tab to hold all widgets
mtsQtApplication * qtAppTask = new mtsQtApplication("QtApplication", argc, argv);
qtAppTask->Configure();
componentManager->AddComponent(qtAppTask);
// console
mtsIntuitiveResearchKitConsole * console = new mtsIntuitiveResearchKitConsole("console");
componentManager->AddComponent(console);
mtsIntuitiveResearchKitConsoleQtWidget * consoleGUI = new mtsIntuitiveResearchKitConsoleQtWidget("consoleGUI");
componentManager->AddComponent(consoleGUI);
// connect consoleGUI to console
componentManager->Connect("console", "Main", "consoleGUI", "Main");
// organize all widgets in a tab widget
QTabWidget * tabWidget = new QTabWidget;
tabWidget->addTab(consoleGUI, "Main");
// IO is shared accross components
mtsRobotIO1394 * io = new mtsRobotIO1394("io", periodIO, firewirePort);
// find name of button event used to detect if operator is present
std::string operatorPresentComponent = jsonConfig["operator-present"]["component"].asString();
std::string operatorPresentInterface = jsonConfig["operator-present"]["interface"].asString();
//set defaults
if (operatorPresentComponent == "") {
operatorPresentComponent = "io";
}
if (operatorPresentInterface == "") {
operatorPresentInterface = "COAG";
}
std::cout << "Using \"" << operatorPresentComponent << "::" << operatorPresentInterface
<< "\" to detect if operator is present" << std::endl;
// setup io defined in the json configuration file
const Json::Value pairs = jsonConfig["pairs"];
for (unsigned int index = 0; index < pairs.size(); ++index) {
// master
Json::Value jsonMaster = pairs[index]["master"];
std::string armName = jsonMaster["name"].asString();
std::string ioFile = jsonMaster["io"].asString();
fileExists(armName + " IO", ioFile);
io->Configure(ioFile);
// slave
Json::Value jsonSlave = pairs[index]["slave"];
armName = jsonSlave["name"].asString();
ioFile = jsonSlave["io"].asString();
fileExists(armName + " IO", ioFile);
io->Configure(ioFile);
}
componentManager->AddComponent(io);
// connect ioGUIMaster to io
mtsRobotIO1394QtWidgetFactory * robotWidgetFactory = new mtsRobotIO1394QtWidgetFactory("robotWidgetFactory");
componentManager->AddComponent(robotWidgetFactory);
componentManager->Connect("robotWidgetFactory", "RobotConfiguration", "io", "Configuration");
robotWidgetFactory->Configure();
// add all IO GUI to tab
mtsRobotIO1394QtWidgetFactory::WidgetListType::const_iterator iterator;
for (iterator = robotWidgetFactory->Widgets().begin();
iterator != robotWidgetFactory->Widgets().end();
++iterator) {
tabWidget->addTab(*iterator, (*iterator)->GetName().c_str());
}
tabWidget->addTab(robotWidgetFactory->ButtonsWidget(), "Buttons");
// setup arms defined in the json configuration file
for (unsigned int index = 0; index < pairs.size(); ++index) {
Json::Value jsonMaster = pairs[index]["master"];
std::string masterName = jsonMaster["name"].asString();
std::string masterPIDFile = jsonMaster["pid"].asString();
std::string masterKinematicFile = jsonMaster["kinematic"].asString();
std::string masterUDPIP = jsonMaster["UDP-IP"].asString();
short masterUDPPort = jsonMaster["UDP-port"].asInt();
fileExists(masterName + " PID", masterPIDFile);
fileExists(masterName + " kinematic", masterKinematicFile);
mtsIntuitiveResearchKitConsole::Arm * mtm
= new mtsIntuitiveResearchKitConsole::Arm(masterName, io->GetName());
mtm->ConfigurePID(masterPIDFile);
mtm->ConfigureArm(mtsIntuitiveResearchKitConsole::Arm::ARM_MTM,
masterKinematicFile, periodKinematics);
console->AddArm(mtm);
if (masterUDPIP != "" || masterUDPPort != 0) {
if (masterUDPIP != "" && masterUDPPort != 0) {
std::cout << "Adding UDPStream component for master " << masterName << " to " << masterUDPIP << ":" << masterUDPPort << std::endl;
mtsIntuitiveResearchKitUDPStreamer * streamer =
new mtsIntuitiveResearchKitUDPStreamer(masterName + "UDP", periodUDP, masterUDPIP, masterUDPPort);
componentManager->AddComponent(streamer);
// connect to mtm interface to get cartesian position
componentManager->Connect(streamer->GetName(), "Robot", mtm->Name(), "Robot");
// connect to io to get clutch events
componentManager->Connect(streamer->GetName(), "Clutch", "io", "CLUTCH");
// connect to io to get coag events
componentManager->Connect(streamer->GetName(), "Coag", "io", "COAG");
} else {
std::cerr << "Error for master arm " << masterName << ", you can provided UDP-IP w/o UDP-port" << std::endl;
exit(-1);
}
}
Json::Value jsonSlave = pairs[index]["slave"];
std::string slaveName = jsonSlave["name"].asString();
std::string slavePIDFile = jsonSlave["pid"].asString();
std::string slaveKinematicFile = jsonSlave["kinematic"].asString();
fileExists(slaveName + " PID", slavePIDFile);
fileExists(slaveName + " kinematic", slaveKinematicFile);
mtsIntuitiveResearchKitConsole::Arm * psm
= new mtsIntuitiveResearchKitConsole::Arm(slaveName, io->GetName());
psm->ConfigurePID(slavePIDFile);
psm->ConfigureArm(mtsIntuitiveResearchKitConsole::Arm::ARM_PSM,
slaveKinematicFile, periodKinematics);
console->AddArm(psm);
// PID Master GUI
std::string masterPIDName = masterName + " PID";
mtsPIDQtWidget * pidMasterGUI = new mtsPIDQtWidget(masterPIDName, 8);
pidMasterGUI->Configure();
componentManager->AddComponent(pidMasterGUI);
componentManager->Connect(pidMasterGUI->GetName(), "Controller", mtm->PIDComponentName(), "Controller");
tabWidget->addTab(pidMasterGUI, masterPIDName.c_str());
// PID Slave GUI
std::string slavePIDName = slaveName + " PID";
mtsPIDQtWidget * pidSlaveGUI = new mtsPIDQtWidget(slavePIDName, 7);
pidSlaveGUI->Configure();
componentManager->AddComponent(pidSlaveGUI);
componentManager->Connect(pidSlaveGUI->GetName(), "Controller", psm->PIDComponentName(), "Controller");
tabWidget->addTab(pidSlaveGUI, slavePIDName.c_str());
// Master GUI
mtsIntuitiveResearchKitArmQtWidget * masterGUI = new mtsIntuitiveResearchKitArmQtWidget(mtm->Name() + "GUI");
masterGUI->Configure();
componentManager->AddComponent(masterGUI);
tabWidget->addTab(masterGUI, mtm->Name().c_str());
// connect masterGUI to master
componentManager->Connect(masterGUI->GetName(), "Manipulator", mtm->Name(), "Robot");
// Slave GUI
mtsIntuitiveResearchKitArmQtWidget * slaveGUI = new mtsIntuitiveResearchKitArmQtWidget(psm->Name() + "GUI");
slaveGUI->Configure();
componentManager->AddComponent(slaveGUI);
tabWidget->addTab(slaveGUI, psm->Name().c_str());
// connect slaveGUI to slave
componentManager->Connect(slaveGUI->GetName(), "Manipulator", psm->Name(), "Robot");
// Teleoperation
std::string teleName = masterName + "-" + slaveName;
mtsTeleOperationQtWidget * teleGUI = new mtsTeleOperationQtWidget(teleName + "GUI");
teleGUI->Configure();
componentManager->AddComponent(teleGUI);
tabWidget->addTab(teleGUI, teleName.c_str());
mtsTeleOperation * tele = new mtsTeleOperation(teleName, periodTeleop);
// Default orientation between master and slave
vctMatRot3 master2slave;
master2slave.From(vctAxAnRot3(vct3(0.0, 0.0, 1.0), 180.0 * cmnPI_180));
tele->SetRegistrationRotation(master2slave);
componentManager->AddComponent(tele);
// connect teleGUI to tele
componentManager->Connect(teleGUI->GetName(), "TeleOperation", tele->GetName(), "Setting");
componentManager->Connect(tele->GetName(), "Master", mtm->Name(), "Robot");
componentManager->Connect(tele->GetName(), "Slave", psm->Name(), "Robot");
componentManager->Connect(tele->GetName(), "Clutch", "io", "CLUTCH");
componentManager->Connect(tele->GetName(), "OperatorPresent", operatorPresentComponent, operatorPresentInterface);
console->AddTeleOperation(tele->GetName());
}
// configure data collection if needed
if (options.IsSet("collection-config")) {
// make sure the json config file exists
fileExists("JSON data collection configuration", jsonCollectionConfigFile);
mtsCollectorFactory * collectorFactory = new mtsCollectorFactory("collectors");
collectorFactory->Configure(jsonCollectionConfigFile);
componentManager->AddComponent(collectorFactory);
collectorFactory->Connect();
mtsCollectorQtWidget * collectorQtWidget = new mtsCollectorQtWidget();
tabWidget->addTab(collectorQtWidget, "Collection");
mtsCollectorQtFactory * collectorQtFactory = new mtsCollectorQtFactory("collectorsQt");
collectorQtFactory->SetFactory("collectors");
componentManager->AddComponent(collectorQtFactory);
collectorQtFactory->Connect();
collectorQtFactory->ConnectToWidget(collectorQtWidget);
}
// show all widgets
tabWidget->show();
//-------------- create the components ------------------
io->CreateAndWait(2.0 * cmn_s); // this will also create the pids as they are in same thread
io->StartAndWait(2.0 * cmn_s);
// start all other components
componentManager->CreateAllAndWait(2.0 * cmn_s);
componentManager->StartAllAndWait(2.0 * cmn_s);
// QtApplication will run in main thread and return control
// when exited.
componentManager->KillAllAndWait(2.0 * cmn_s);
componentManager->Cleanup();
// delete dvgc robot
delete io;
delete robotWidgetFactory;
// stop all logs
cmnLogger::Kill();
return 0;
}
void mtsIntuitiveResearchKitArm::Init(void)
{
mCounter = 0;
IsGoalSet = false;
SetState(mtsIntuitiveResearchKitArmTypes::DVRK_UNINITIALIZED);
// initialize trajectory data
JointGet.SetSize(NumberOfJoints());
JointVelocityGet.SetSize(NumberOfJoints());
JointSet.SetSize(NumberOfJoints());
JointSetParam.Goal().SetSize(NumberOfAxes());
JointTrajectory.Velocity.SetSize(NumberOfJoints());
JointTrajectory.Acceleration.SetSize(NumberOfJoints());
JointTrajectory.Start.SetSize(NumberOfJoints());
JointTrajectory.Goal.SetSize(NumberOfJoints());
JointTrajectory.GoalError.SetSize(NumberOfJoints());
JointTrajectory.GoalTolerance.SetSize(NumberOfJoints());
JointTrajectory.EndTime = 0.0;
PotsToEncodersTolerance.SetSize(NumberOfAxes());
// initialize velocity
CartesianVelocityGetParam.SetVelocityLinear(vct3(0.0));
CartesianVelocityGetParam.SetVelocityAngular(vct3(0.0));
CartesianVelocityGetParam.SetTimestamp(0.0);
CartesianVelocityGetParam.SetValid(false);
// base frame, mostly for cases where no base frame is set by user
BaseFrame = vctFrm4x4::Identity();
BaseFrameValid = true;
// cartesian position are timestamped using timestamps provided by PID
CartesianGetParam.SetAutomaticTimestamp(false);
CartesianGetDesiredParam.SetAutomaticTimestamp(false);
this->StateTable.AddData(CartesianGetLocalParam, "CartesianPositionLocal");
this->StateTable.AddData(CartesianGetLocalDesiredParam, "CartesianPositionLocalDesired");
this->StateTable.AddData(CartesianGetParam, "CartesianPosition");
this->StateTable.AddData(CartesianGetDesiredParam, "CartesianPositionDesired");
this->StateTable.AddData(BaseFrame, "BaseFrame");
this->StateTable.AddData(JointGetParam, "JointPosition");
this->StateTable.AddData(JointGetDesired, "JointPositionDesired");
this->StateTable.AddData(CartesianVelocityGetParam, "CartesianVelocityGetParam");
this->StateTable.AddData(StateJointParam, "StateJoint");
this->StateTable.AddData(StateJointDesiredParam, "StateJointDesired");
// PID
PIDInterface = AddInterfaceRequired("PID");
if (PIDInterface) {
PIDInterface->AddFunction("Enable", PID.Enable);
PIDInterface->AddFunction("GetPositionJoint", PID.GetPositionJoint);
PIDInterface->AddFunction("GetPositionJointDesired", PID.GetPositionJointDesired);
PIDInterface->AddFunction("GetStateJoint", PID.GetStateJoint);
PIDInterface->AddFunction("GetStateJointDesired", PID.GetStateJointDesired);
PIDInterface->AddFunction("SetPositionJoint", PID.SetPositionJoint);
PIDInterface->AddFunction("SetCheckJointLimit", PID.SetCheckJointLimit);
PIDInterface->AddFunction("GetVelocityJoint", PID.GetVelocityJoint);
PIDInterface->AddFunction("EnableTorqueMode", PID.EnableTorqueMode);
PIDInterface->AddFunction("SetTorqueJoint", PID.SetTorqueJoint);
PIDInterface->AddFunction("SetTorqueOffset", PID.SetTorqueOffset);
PIDInterface->AddFunction("EnableTrackingError", PID.EnableTrackingError);
PIDInterface->AddFunction("SetTrackingErrorTolerances", PID.SetTrackingErrorTolerance);
PIDInterface->AddEventHandlerWrite(&mtsIntuitiveResearchKitArm::JointLimitEventHandler, this, "JointLimit");
PIDInterface->AddEventHandlerWrite(&mtsIntuitiveResearchKitArm::ErrorEventHandler, this, "Error");
}
// Robot IO
IOInterface = AddInterfaceRequired("RobotIO");
if (IOInterface) {
IOInterface->AddFunction("GetSerialNumber", RobotIO.GetSerialNumber);
IOInterface->AddFunction("EnablePower", RobotIO.EnablePower);
IOInterface->AddFunction("DisablePower", RobotIO.DisablePower);
IOInterface->AddFunction("GetActuatorAmpStatus", RobotIO.GetActuatorAmpStatus);
IOInterface->AddFunction("GetBrakeAmpStatus", RobotIO.GetBrakeAmpStatus);
IOInterface->AddFunction("BiasEncoder", RobotIO.BiasEncoder);
IOInterface->AddFunction("ResetSingleEncoder", RobotIO.ResetSingleEncoder);
IOInterface->AddFunction("GetAnalogInputPosSI", RobotIO.GetAnalogInputPosSI);
IOInterface->AddFunction("SetActuatorCurrent", RobotIO.SetActuatorCurrent);
IOInterface->AddFunction("UsePotsForSafetyCheck", RobotIO.UsePotsForSafetyCheck);
IOInterface->AddFunction("SetPotsToEncodersTolerance", RobotIO.SetPotsToEncodersTolerance);
IOInterface->AddFunction("BrakeRelease", RobotIO.BrakeRelease);
IOInterface->AddFunction("BrakeEngage", RobotIO.BrakeEngage);
}
// Setup Joints
SUJInterface = AddInterfaceRequired("BaseFrame", MTS_OPTIONAL);
if (SUJInterface) {
SUJInterface->AddEventHandlerWrite(&mtsIntuitiveResearchKitArm::SetBaseFrame, this, "BaseFrameDesired");
SUJInterface->AddEventHandlerWrite(&mtsIntuitiveResearchKitArm::ErrorEventHandler, this, "Error");
}
// Arm
RobotInterface = AddInterfaceProvided("Robot");
if (RobotInterface) {
// Get
RobotInterface->AddCommandReadState(this->StateTable, JointGetParam, "GetPositionJoint");
RobotInterface->AddCommandReadState(this->StateTable, JointGetDesired, "GetPositionJointDesired");
RobotInterface->AddCommandReadState(this->StateTable, StateJointParam, "GetStateJoint");
RobotInterface->AddCommandReadState(this->StateTable, StateJointDesiredParam, "GetStateJointDesired");
RobotInterface->AddCommandReadState(this->StateTable, CartesianGetLocalParam, "GetPositionCartesianLocal");
RobotInterface->AddCommandReadState(this->StateTable, CartesianGetLocalDesiredParam, "GetPositionCartesianLocalDesired");
RobotInterface->AddCommandReadState(this->StateTable, CartesianGetParam, "GetPositionCartesian");
RobotInterface->AddCommandReadState(this->StateTable, CartesianGetDesiredParam, "GetPositionCartesianDesired");
RobotInterface->AddCommandReadState(this->StateTable, BaseFrame, "GetBaseFrame");
RobotInterface->AddCommandReadState(this->StateTable, CartesianVelocityGetParam, "GetVelocityCartesian");
// Set
RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitArm::SetPositionJoint, this, "SetPositionJoint");
RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitArm::SetPositionGoalJoint, this, "SetPositionGoalJoint");
RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitArm::SetPositionCartesian, this, "SetPositionCartesian");
RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitArm::SetPositionGoalCartesian, this, "SetPositionGoalCartesian");
// Trajectory events
RobotInterface->AddEventWrite(JointTrajectory.GoalReachedEvent, "GoalReached", bool());
// Robot State
RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitArm::SetRobotControlState,
this, "SetRobotControlState", std::string(""));
RobotInterface->AddCommandRead(&mtsIntuitiveResearchKitArm::GetRobotControlState,
this, "GetRobotControlState", std::string(""));
// Human readable messages
RobotInterface->AddEventWrite(MessageEvents.Status, "Status", std::string(""));
RobotInterface->AddEventWrite(MessageEvents.Warning, "Warning", std::string(""));
RobotInterface->AddEventWrite(MessageEvents.Error, "Error", std::string(""));
RobotInterface->AddEventWrite(MessageEvents.RobotState, "RobotState", std::string(""));
// Stats
RobotInterface->AddCommandReadState(StateTable, StateTable.PeriodStats,
"GetPeriodStatistics");
}
}
void mtsIntuitiveResearchKitArm::GetRobotData(void)
{
// we can start reporting some joint values after the robot is powered
if (this->RobotState > mtsIntuitiveResearchKitArmTypes::DVRK_HOMING_POWERING) {
mtsExecutionResult executionResult;
executionResult = PID.GetPositionJoint(JointGetParam);
if (!executionResult.IsOK()) {
CMN_LOG_CLASS_RUN_ERROR << GetName() << ": GetRobotData: call to GetJointPosition failed \""
<< executionResult << "\"" << std::endl;
}
// assign to a more convenient vctDoubleVec
JointGet.Assign(JointGetParam.Position(), NumberOfJoints());
// desired joints
executionResult = PID.GetPositionJointDesired(JointGetDesired);
if (!executionResult.IsOK()) {
CMN_LOG_CLASS_RUN_ERROR << GetName() << ": GetRobotData: call to GetJointPositionDesired failed \""
<< executionResult << "\"" << std::endl;
}
// joint velocity
executionResult = PID.GetVelocityJoint(JointVelocityGetParam);
if (!executionResult.IsOK()) {
CMN_LOG_CLASS_RUN_ERROR << GetName() << ": GetRobotData: call to GetVelocityJoint failed \""
<< executionResult << "\"" << std::endl;
}
JointVelocityGet.Assign(JointVelocityGetParam.Velocity(), NumberOfJoints());
// joint state, not used internally but available to users
executionResult = PID.GetStateJoint(StateJointParam);
if (!executionResult.IsOK()) {
CMN_LOG_CLASS_RUN_ERROR << GetName() << ": GetRobotData: call to GetJointState failed \""
<< executionResult << "\"" << std::endl;
}
// desired joint state
executionResult = PID.GetStateJointDesired(StateJointDesiredParam);
if (!executionResult.IsOK()) {
CMN_LOG_CLASS_RUN_ERROR << GetName() << ": GetRobotData: call to GetJointStateDesired failed \""
<< executionResult << "\"" << std::endl;
}
// when the robot is ready, we can compute cartesian position
if (this->RobotState >= mtsIntuitiveResearchKitArmTypes::DVRK_READY) {
// update cartesian position
CartesianGetLocal = Manipulator.ForwardKinematics(JointGet, NumberOfJointsKinematics());
CartesianGet = BaseFrame * CartesianGetLocal;
// normalize
CartesianGetLocal.Rotation().NormalizedSelf();
CartesianGet.Rotation().NormalizedSelf();
// flags
CartesianGetLocalParam.SetTimestamp(JointGetParam.Timestamp());
CartesianGetLocalParam.SetValid(true);
CartesianGetParam.SetTimestamp(JointGetParam.Timestamp());
CartesianGetParam.SetValid(BaseFrameValid);
#if 0
// FIXME: bug
// update cartesian velocity (caveat, velocities are not updated)
const double dt = CartesianGetParam.Timestamp() - CartesianGetPreviousParam.Timestamp();
if (dt > 0.0) {
vct3 linearVelocity;
linearVelocity.DifferenceOf(CartesianGetParam.Position().Translation(),
CartesianGetPreviousParam.Position().Translation());
linearVelocity.Divide(dt);
CartesianVelocityGetParam.SetVelocityLinear(linearVelocity);
CartesianVelocityGetParam.SetVelocityAngular(vct3(0.0));
CartesianVelocityGetParam.SetTimestamp(CartesianGetParam.Timestamp());
CartesianVelocityGetParam.SetValid(true);
} else {
CartesianVelocityGetParam.SetValid(false);
}
#else
vct3 linearVelocity;
linearVelocity = (CartesianGet.Translation() - CartesianGetPrevious.Translation()).Divide(StateTable.Period);
CartesianVelocityGetParam.SetVelocityLinear(linearVelocity);
CartesianVelocityGetParam.SetVelocityAngular(vct3(0.0));
CartesianVelocityGetParam.SetTimestamp(CartesianGetParam.Timestamp());
CartesianVelocityGetParam.SetValid(true);
#endif
// update cartesian position desired based on joint desired
CartesianGetLocalDesired = Manipulator.ForwardKinematics(JointGetDesired);
CartesianGetDesired = BaseFrame * CartesianGetLocalDesired;
// normalize
CartesianGetLocalDesired.Rotation().NormalizedSelf();
CartesianGetDesired.Rotation().NormalizedSelf();
// flags
CartesianGetLocalDesiredParam.SetTimestamp(JointGetParam.Timestamp());
CartesianGetLocalDesiredParam.SetValid(true);
CartesianGetDesiredParam.SetTimestamp(JointGetParam.Timestamp());
CartesianGetDesiredParam.SetValid(BaseFrameValid);
} else {
// update cartesian position
CartesianGetLocal.Assign(vctFrm4x4::Identity());
CartesianGet.Assign(vctFrm4x4::Identity());
CartesianGetLocalParam.SetValid(false);
CartesianGetParam.SetValid(false);
// update cartesian position desired
CartesianGetLocalDesired.Assign(vctFrm4x4::Identity());
CartesianGetDesired.Assign(vctFrm4x4::Identity());
CartesianGetLocalDesiredParam.SetValid(false);
CartesianGetDesiredParam.SetValid(false);
}
CartesianGetLocalParam.Position().From(CartesianGetLocal);
CartesianGetParam.Position().From(CartesianGet);
CartesianGetLocalDesiredParam.Position().From(CartesianGetLocalDesired);
CartesianGetDesiredParam.Position().From(CartesianGetDesired);
} else {
// set joint to zeros
JointGet.Zeros();
PID.GetPositionJoint(JointGetParam); // get size right
JointGetParam.Position().Zeros();
JointGetParam.SetValid(false);
JointVelocityGet.Zeros();
JointVelocityGetParam.Velocity().Zeros();
JointVelocityGetParam.SetValid(false);
}
}
void CommandInitialize(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[])
{
// Expected Input:
// cmd
// class handle
// mesh
// V ~ 3D vertex positions (3 x Nv double)
// T ~ triangle vertex indices (3 x Nt integer)
// N ~ triangle normals (3 x Nt double)
// mesh noise model (optional)
// Tcov ~ triangle covariances (3 x 3 x Nt double)
//
std::stringstream ss;
// Get the class instance from the second input
mexInterface_AlgPDTree_MLP_Mesh& obj =
*convertMat2Ptr<mexInterface_AlgPDTree_MLP_Mesh>(prhs[1]);
// Check parameters
if (nlhs != 0 || nrhs < 5 || nrhs > 6 )
{
MEX_ERROR("Cmd Initialize: requires 0 outputs, 5-6 inputs.");
}
//--- Inputs ---//
MEX_DEBUG("Parsing Inputs...\n");
// Parse Mesh
//vctDynamicVector<vct3> V;
//vctDynamicVector<vctInt3> T;
//vctDynamicVector<vct3> Tn;
MEX_DEBUG(" ...V\n");
Parse3DVectorArray_Double(prhs[2], obj.V);
MEX_DEBUG(" ...T\n");
Parse3DVectorArray_Int(prhs[3], obj.T);
MEX_DEBUG(" ...N\n");
Parse3DVectorArray_Double(prhs[4], obj.Tn);
if (nrhs >= 6)
{
MEX_DEBUG(" ...Tcov\n");
ParseMatrixArray3x3_Double(prhs[5], obj.TriangleCov);
unsigned int numT = obj.T.size();
if (obj.TriangleCov.size() != numT)
{
MEX_ERROR("ERROR: number of triangle covariances does not equal number of triangles");
}
// compute eigenvalues of triangle covariances
obj.TriangleCovEig.SetSize(numT);
vct3x3 dummyMat;
for (unsigned int i = 0; i < numT; i++)
{
ComputeCovEigenDecomposition_NonIter(obj.TriangleCov[i], obj.TriangleCovEig[i], dummyMat);
}
}
else
{
obj.TriangleCov.SetSize(obj.T.size());
obj.TriangleCovEig.SetSize(obj.T.size());
obj.TriangleCov.SetAll(vct3x3(0.0));
obj.TriangleCovEig.SetAll(vct3(0.0));
}
MEX_DEBUG(" ...done\n");
//--- Processing ---//
// build mesh
MEX_DEBUG("Building mesh...\n");
cisstMesh *pMesh = new cisstMesh();
pMesh->LoadMesh(&obj.V, &obj.T, &obj.Tn);
if (pMesh->NumVertices() == 0)
{
MEX_ERROR("ERROR: Build mesh failed\n");
}
// set mesh noise model
pMesh->TriangleCov = obj.TriangleCov;
pMesh->TriangleCovEig = obj.TriangleCovEig;
// build covariance tree
int nThresh = 5; // Cov Tree Params
double diagThresh = 5.0; // ''
MEX_DEBUG("Building mesh covariance tree...\n");
obj.pTree = new PDTree_Mesh(*pMesh, nThresh, diagThresh);
//PDTree_Mesh *pPDTree = new PDTree_Mesh(*pMesh, nThresh, diagThresh);
if (nrhs >= 6)
{ // compute the node noise models
obj.pTree->ComputeNodeNoiseModels();
}
ss.str("");
ss << "Tree built: NNodes = " << obj.pTree->NumNodes() << " NDatums = "
<< obj.pTree->NumData() << " TreeDepth = " << obj.pTree->TreeDepth() << "\n";
MEX_DEBUG(ss.str());
// create & initialize algorithm
if (obj.pAlg)
{
delete obj.pAlg;
obj.pAlg = NULL;
}
obj.pAlg = new algPDTree_MLP_Mesh(obj.pTree);
}
