diff --git a/CMakeLists.txt b/CMakeLists.txt index 6af0114..40836dc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,7 @@ cmake_minimum_required (VERSION 2.6) project (rst_all) +link_directories(/usr/lib) add_subdirectory(librst) add_subdirectory(projects/EmptyTab) add_subdirectory(projects/TemplateTab) diff --git a/build/.lastload b/build/.lastload new file mode 100644 index 0000000..2dd6c55 --- /dev/null +++ b/build/.lastload @@ -0,0 +1 @@ +/home/cerdogan/Documents/MacGyver/3rdParty/rst/scenes/DesktopArm.rscene diff --git a/build/projects/EmptyTab/EmptyTab b/build/projects/EmptyTab/EmptyTab new file mode 100755 index 0000000..a6a5948 Binary files /dev/null and b/build/projects/EmptyTab/EmptyTab differ diff --git a/build/projects/TemplateTab/TemplateTab b/build/projects/TemplateTab/TemplateTab new file mode 100755 index 0000000..a95fc8a Binary files /dev/null and b/build/projects/TemplateTab/TemplateTab differ diff --git a/librst/CMakeLists.txt b/librst/CMakeLists.txt index cf3a4ff..24687d0 100644 --- a/librst/CMakeLists.txt +++ b/librst/CMakeLists.txt @@ -70,6 +70,8 @@ set ( HEADERS ../ ) +link_directories(/usr/lib/) + set (wxWidgets_USE_LIBS base core gl) find_package (wxWidgets) if (wxWidgets_FOUND) diff --git a/librst/Tools/.IK.cpp.swp b/librst/Tools/.IK.cpp.swp new file mode 100644 index 0000000..ec29f15 Binary files /dev/null and b/librst/Tools/.IK.cpp.swp differ diff --git a/librst/Tools/.Robot.h.swp b/librst/Tools/.Robot.h.swp new file mode 100644 index 0000000..d854b6e Binary files /dev/null and b/librst/Tools/.Robot.h.swp differ diff --git a/librst/Tools/Robot.cpp b/librst/Tools/Robot.cpp index 1db864d..c18883d 100644 --- a/librst/Tools/Robot.cpp +++ b/librst/Tools/Robot.cpp @@ -108,7 +108,7 @@ int Robot::findLink(string name){ void Robot::setConf(const VectorXd &conf, bool updateCollisionModel) { if(conf.size() != links.size()) { - cout << "Error: Configuration size does not match number of links" << endl; + cout << "Error: Configuration size does not match number of links: " << links.size() << endl; } int j = 0; for(unsigned int i = 0; i < links.size(); i++) { diff --git a/projects/EmptyTab/CMakeLists.txt b/projects/EmptyTab/CMakeLists.txt index 3de043a..1075afb 100644 --- a/projects/EmptyTab/CMakeLists.txt +++ b/projects/EmptyTab/CMakeLists.txt @@ -12,7 +12,7 @@ find_package (wxWidgets) if (wxWidgets_FOUND) include (${wxWidgets_USE_FILE}) add_executable (EmptyTab ${SRC_TAB_SIMULATION}) - target_link_libraries (EmptyTab rst ${wxWidgets_LIBRARIES}) + target_link_libraries (EmptyTab rst ${wxWidgets_LIBRARIES} GLU) else (wxWidgets_FOUND) message ("wxWidgets not found!") endif (wxWidgets_FOUND) diff --git a/projects/TemplateTab/.TemplateTab.cpp.swp b/projects/TemplateTab/.TemplateTab.cpp.swp new file mode 100644 index 0000000..ee9da3c Binary files /dev/null and b/projects/TemplateTab/.TemplateTab.cpp.swp differ diff --git a/projects/TemplateTab/CMakeLists.txt b/projects/TemplateTab/CMakeLists.txt index 6ba73cd..bbd2fe7 100644 --- a/projects/TemplateTab/CMakeLists.txt +++ b/projects/TemplateTab/CMakeLists.txt @@ -3,7 +3,7 @@ project (TemplateTab) include_directories(../../librst ../../) -set (CMAKE_CXX_FLAGS "-L../../librst") +set (CMAKE_CXX_FLAGS "-L../../librst -g") set ( SRC_TAB_SIMULATION TemplateTab TemplateTabApp ) @@ -12,7 +12,7 @@ find_package (wxWidgets) if (wxWidgets_FOUND) include (${wxWidgets_USE_FILE}) add_executable (TemplateTab ${SRC_TAB_SIMULATION}) - target_link_libraries (TemplateTab rst ${wxWidgets_LIBRARIES}) + target_link_libraries (TemplateTab rst ${wxWidgets_LIBRARIES} GLU) else (wxWidgets_FOUND) message ("wxWidgets not found!") endif (wxWidgets_FOUND) diff --git a/projects/TemplateTab/TemplateTab.cpp b/projects/TemplateTab/TemplateTab.cpp index 100211e..32475dc 100644 --- a/projects/TemplateTab/TemplateTab.cpp +++ b/projects/TemplateTab/TemplateTab.cpp @@ -45,10 +45,12 @@ #include #include using namespace std; +#include +using namespace Eigen; #include #include - +#include //Give each slider a number so we recognize them (also indicates order of select on tabbing) enum sliderNames { @@ -58,6 +60,7 @@ enum sliderNames { //Add a handler for slider changes BEGIN_EVENT_TABLE(TemplateTab, wxPanel) EVT_COMMAND (wxID_ANY, wxEVT_RST_SLIDER_CHANGE, TemplateTab::OnSlider) + EVT_COMMAND (wxID_ANY, wxEVT_COMMAND_BUTTON_CLICKED, TemplateTab::OnButton) END_EVENT_TABLE () // Class constructor for the tab: Each tab will be a subclass of RSTTab @@ -68,38 +71,40 @@ TemplateTab::TemplateTab(wxWindow *parent, const wxWindowID id, RSTTab(parent, id, pos, size, style) { sizerFull = new wxBoxSizer(wxHORIZONTAL); - // Create Static boxes - these are the outlines you see on the inspector tab - a nice way to organize things - wxStaticBox* ss1Box = new wxStaticBox(this, -1, wxT("Sample Box 1")); - wxStaticBox* ss2Box = new wxStaticBox(this, -1, wxT("Sampel Box 2")); - - // Create sizers for these static boxes - wxStaticBoxSizer* ss1BoxS = new wxStaticBoxSizer(ss1Box, wxVERTICAL); - wxStaticBoxSizer* ss2BoxS = new wxStaticBoxSizer(ss2Box, wxVERTICAL); - - // Add 2 static text fields (these can be re-written by the handler) - sampleText1 = new wxStaticText(this, -1, wxT("Sample text 1"), - wxDefaultPosition, wxDefaultSize, wxALIGN_CENTRE); - sampleText2 = new wxStaticText(this, -1, wxT("Sample text 2"), - wxDefaultPosition, wxDefaultSize, wxALIGN_CENTRE); - - // Create RST-style sliders - sampleRSTSlider1 = new RSTSlider("SS1", -180, 180, 2000, 0, 1000, 2000, - this, SAMPLE_RST_SLIDER1); - sampleRSTSlider2 = new RSTSlider("SS2", -180, 180, 2000, 0, 1000, 2000, - this, SAMPLE_RST_SLIDER2); - - // Add the boxes to their respective sizers - sizerFull->Add(ss1BoxS, 1, wxEXPAND | wxALL, 6); - sizerFull->Add(ss2BoxS, 1, wxEXPAND | wxALL, 6); - SetSizer(sizerFull); - - // Add content to box1 (1st sample text and slider) - ss1BoxS->Add(sampleText1, 1, wxEXPAND | wxALL, 6); - ss1BoxS->Add(sampleRSTSlider1, 1, wxEXPAND | wxALL, 6); - - // Add content to box2 (2nd sample text and slider) - ss2BoxS->Add(sampleRSTSlider2, 1, wxEXPAND | wxALL, 6); - ss2BoxS->Add(sampleText2, 1, wxEXPAND | wxALL, 6); + wxStaticBox* algoBox = new wxStaticBox(this, -1, wxT("Algorithms")); + wxStaticBoxSizer* algoBoxSizer = new wxStaticBoxSizer(algoBox, wxVERTICAL); + algoBoxSizer->Add(new wxButton(this, 19123, wxT("Show")), 0, wxALL, 4); + + sizerFull->Add(algoBoxSizer, 0, wxALL | wxEXPAND, 5); +} + +void TemplateTab::OnButton(wxCommandEvent& evt) { + + + // Create the IK structure + vector idx; + for(size_t i = 0; i < 7; i++) idx.push_back(i+1); + IK ik (world, 0, 7); + static bool doik = false; + static Eigen::Transform pose; + static Eigen::VectorXd angles (7); + if(!doik){ + pose = world->robots[0]->links[7]->absPose; + world->robots[0]->getConf(angles); + cout << "recording angles: " << angles.transpose() / M_PI * 180.0 << endl; + cout << "recording pose: \n" << pose.matrix() << endl; + } + else { + cout << "attempting pose: \n" << pose.matrix() << endl; + ik.calculate(pose, 11*M_PI/12, angles); + cout << "angles: " << angles.transpose() / M_PI * 180.0 << endl; + if(angles(1) != 0.0) world->robots[0]->setConf(idx, angles, false); + world->robots[0]->Draw(); + } + doik = !doik; + + + } diff --git a/projects/TemplateTab/TemplateTab.h b/projects/TemplateTab/TemplateTab.h index bca4d32..0afc302 100644 --- a/projects/TemplateTab/TemplateTab.h +++ b/projects/TemplateTab/TemplateTab.h @@ -65,6 +65,7 @@ class TemplateTab : public RSTTab RSTSlider* sampleRSTSlider2; void OnSlider(wxCommandEvent &evt); + void OnButton(wxCommandEvent& evt); void RSTStateChange(); // Thread specific diff --git a/scenes/DesktopArm.rscene b/scenes/DesktopArm.rscene index b808d14..07f041d 100644 --- a/scenes/DesktopArm.rscene +++ b/scenes/DesktopArm.rscene @@ -2,7 +2,7 @@ ##### ROBOT 1 ##### -> ROBOT SCHUNKARM schunk/SchunkArmHand.rsdh +> ROBOT SCHUNKARM schunk/SchunkArm.rsdh > POSITION -0.132 1.316 0.721 > ORIENTATION 90 -0 0