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); }