Showed that IK works in rst

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)

build/.lastload

@@ -0,0 +1 @@
+/home/cerdogan/Documents/MacGyver/3rdParty/rst/scenes/DesktopArm.rscene

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) 

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++) {

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)

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)

projects/TemplateTab/TemplateTab.cpp

@@ -45,10 +45,12 @@
 #include <GUI/RSTFrame.h>
 #include <iostream>
 using namespace std;
+#include <Eigen/Dense>
+using namespace Eigen;
 
 #include <Tabs/AllTabs.h>
 #include <RSTApp.h>
-
+#include <librst/Tools/IK.h>
 
 //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 <int> 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 <double, 3, Affine> 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;
+
+
+
 
 }
 

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

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