Merge branch 'master' of github.com:NASA-Tensegrity-Robotics-Toolkit/…

…NTRTsim

INSTALL

@@ -272,3 +272,4 @@ anything.
 	4)run setup.sh
 	5)run bin/build.sh
 
+

resources/YamlStructures/MountainGoat.yaml

@@ -0,0 +1,106 @@
+substructures:
+  spine:
+    path: MountainGoatSpine/MGSpine.yaml
+
+  front_right_leg/back_right_leg/front_left_leg/back_left_leg:
+    path: MountainGoatLimb/MGLeg.yaml
+  front_right_leg/back_right_leg:
+    rotation:
+      reference: [0, 0, 0]
+      axis: [0, 1, 0]
+      angle: 180
+
+  front_right_leg/front_left_leg:
+    translation: [22, 0, 0]
+  back_right_leg/back_left_leg:
+    translation: [66, 0, 0]
+
+bond_groups:
+  string:
+      front_right_leg/spine/node_node:
+        - [hip.inner_top, vertebra1.back2]
+        - [hip.inner_top, vertebra2.front2]
+        - [hip.inner_top, vertebra2.back1]
+        - [hip.inner_top, vertebra3.front1]
+
+        - [hip.inner_bottom, vertebra1.back1]
+        - [hip.inner_bottom, vertebra2.front2]
+        - [hip.inner_bottom, vertebra2.back1]
+        - [hip.inner_bottom, vertebra3.front2]
+
+        - [hip.center, vertebra2.back1]
+        - [hip.center, vertebra2.front2]
+        - [knee.top, vertebra2.front2]
+        - [knee.back, vertebra2.front2]
+        - [knee.front, vertebra2.back1]
+
+      front_left_leg/spine/node_node:
+        - [hip.inner_top, vertebra1.back2]
+        - [hip.inner_top, vertebra2.back2]
+        - [hip.inner_top, vertebra2.front1]
+        - [hip.inner_top, vertebra3.front1]
+
+        - [hip.inner_bottom, vertebra1.back1]
+        - [hip.inner_bottom, vertebra2.back2]
+        - [hip.inner_bottom, vertebra2.front1]
+        - [hip.inner_bottom, vertebra3.front2]
+
+        - [hip.center, vertebra2.back2]
+        - [hip.center, vertebra2.front1]
+        - [knee.top, vertebra2.front1]
+        - [knee.front, vertebra2.front1]
+        - [knee.back, vertebra2.back2]
+
+      back_right_leg/spine/node_node:
+        - [hip.inner_top, vertebra5.back2]
+        - [hip.inner_top, vertebra6.back1]
+        - [hip.inner_top, vertebra6.front2]
+        - [hip.inner_top, vertebra7.front1]
+
+        - [hip.inner_bottom, vertebra5.back1]
+        - [hip.inner_bottom, vertebra6.back1]
+        - [hip.inner_bottom, vertebra6.front2]
+        - [hip.inner_bottom, vertebra7.front2]
+
+        - [hip.center, vertebra6.back1]
+        - [hip.center, vertebra6.front2]
+        - [knee.top, vertebra4.back1]
+        - [knee.back, vertebra6.front2]
+        - [knee.front, vertebra6.back1]
+
+      back_left_leg/spine/node_node:
+        - [hip.inner_top, vertebra6.back2]
+        - [hip.inner_top, vertebra6.front1]
+        - [hip.inner_top, vertebra5.back2]
+        - [hip.inner_top, vertebra7.front1]
+
+        - [hip.inner_bottom, vertebra6.back2]
+        - [hip.inner_bottom, vertebra6.front1]
+        - [hip.inner_bottom, vertebra5.back1]
+        - [hip.inner_bottom, vertebra7.front2]
+
+        - [hip.center, vertebra6.back2]
+        - [hip.center, vertebra6.front1]
+        - [knee.top, vertebra4.back2]
+        - [knee.back, vertebra6.back2]
+        - [knee.front, vertebra6.front1]
+
+builders:
+  rod:
+    class: tgRodInfo
+    parameters:
+      density: 0.014
+      radius: 0.5
+      friction: 0.5
+  string:
+    class: tgBasicActuatorInfo
+    parameters:
+      stiffness: 2000
+      damping: 20
+      pretension: 1000
+      # motor_friction: 10
+      max_tension: 7000
+      target_velocity: 12
+
+
+

resources/YamlStructures/MountainGoatLimb/MGHip.yaml

@@ -0,0 +1,11 @@
+nodes:
+  center: [0, 0, 0]
+  outer_bottom: [0, -10, -10]
+  inner_bottom: [0, -10, 10]
+  inner_top: [0, 10, 10]
+
+pair_groups:
+  rod:
+    - [center, outer_bottom]
+    - [center, inner_bottom]
+    - [center, inner_top]

resources/YamlStructures/MountainGoatLimb/MGKnee.yaml

@@ -0,0 +1,15 @@
+nodes:
+  base_bottom: [0, 0, 0]
+  base_top: [0, 5, 0]
+  center: [0, 15, 0]
+  back: [10, 15, 0]
+  front: [-10, 15, 0]
+  top: [0, 25, 0]
+
+pair_groups:
+  rod:
+    - [base_bottom, base_top]
+    - [center, base_top]
+    - [center, back]
+    - [center, front]
+    - [center, top]

resources/YamlStructures/MountainGoatLimb/MGLeg.yaml

@@ -0,0 +1,20 @@
+substructures:
+  hip:
+    path: MGHip.yaml
+    translation: [0, 26, -11]
+  knee:
+    path: MGKnee.yaml
+    translation: [0, 0, -11]
+
+bond_groups:
+  string:
+      hip/knee/node_node:
+        - [inner_bottom, top]
+        - [inner_bottom, front]
+        - [inner_bottom, back]
+
+        - [outer_bottom, top]
+        - [outer_bottom, front]
+        - [outer_bottom, back]
+
+        - [inner_top, top]

resources/YamlStructures/MountainGoatSpine/MGSpine.yaml

@@ -0,0 +1,38 @@
+substructures:
+  vertebra1/vertebra2/vertebra3/vertebra4/vertebra5/vertebra6/vertebra7:
+    path: MGVertebra.yaml
+    translation: [11, 0, 0]
+    offset: [11, 0, 0]
+  vertebra1/vertebra3/vertebra5/vertebra7:
+    rotation:
+      axis: [1, 0, 0]
+      angle: 90
+
+bond_groups:
+  string:
+      vertebra1/vertebra2/vertebra3/vertebra4/vertebra5/vertebra6/vertebra7/node_node:
+        - [back1, front1]
+        - [back1, front2]
+        - [back2, front1]
+        - [back2, front2]
+      vertebra1/vertebra3/vertebra5/vertebra7/node_node:
+        - [back1, front2]
+        - [back2, front1]
+      vertebra2/vertebra4/vertebra6/node_node:
+        - [back1, front2]
+        - [back2, front1]
+      vertebra1/vertebra2/node_node:
+        - [front1, front1]
+        - [front2, front2]
+        - [front1, front2]
+        - [front2, front1]
+      vertebra6/vertebra7/node_node:
+        - [back1, back1]
+        - [back1, back2]
+        - [back2, back1]
+        - [back2, back2]
+      vertebra2/vertebra3/vertebra4/vertebra5/vertebra6/vertebra7/node_node:
+        - [front1, front1]
+        - [front1, front2]
+        - [front2, front1]
+        - [front2, front2]

resources/YamlStructures/MountainGoatSpine/MGVertebra.yaml

@@ -0,0 +1,13 @@
+nodes:
+  center: [0, 26, 0]
+  back1: [10, 26, 10]
+  back2: [10, 26, -10]
+  front1: [-10, 26, -10]
+  front2: [-10, 26, 10]
+
+pair_groups:
+  rod:
+    - [center, back1]
+    - [center, back2]
+    - [center, front1]
+    - [center, front2]

src/dev/ezhu/STLParser/AppSTLParser.cpp

@@ -2,7 +2,8 @@
 #include <fstream>
 #include <stdlib.h>
 #include <stdio.h>
-#include <string.h>
+#include <string>
+#include <math.h>
 
 using namespace std;
 
@@ -22,7 +23,7 @@ int main(int argc, char* argv[]) {
 		string filename_out = argv[2];
 		//string  out_path = "/home/edward/NTRTsim/src/dev/ezhu/STLParser/";
 		// Check for valid file extension
-		if (strstr(filename_in.c_str(), ".stl") == NULL) {
+		if (filename_in.find(".stl") == string::npos) {
 			cout << "Incorrect filetype, application for ASCII STL files only" << endl;
 			exit(EXIT_FAILURE);
 		}
@@ -55,28 +56,90 @@ int main(int argc, char* argv[]) {
 		}
 		// file_out << "Test 1 2 3" << endl;
 		string key = "vertex";
+		string line_out;
+		int vertex_index = 0;
+		int vertex_count = 0;
+		int triangle_count = 0;
+		int element_index = 0;
+		int line_index = 0;
 		// Parse triangle verticies from .stl file
-		for (int i = 0; i < 20; i++) {
+		//for (int i = 0; i < 204; i++) {
+		while (1) {
 			if (file_in.good()) {
-				string line;
-				getline(file_in, line);
+				string line_in;
+				getline(file_in, line_in);
+				line_index += 1;
+				cout << "Lines processed: " << line_index << endl;
 				//cout << line << endl;
-				if (strstr(line.c_str(), key.c_str()) != NULL) {
-					cout << line << endl;
+				size_t found_key = line_in.find(key);
+				// Look for "vertex" tag
+				if (found_key != string::npos) {
+					//cout << line_in << endl;
+					// Coordinates are space delimited
+					size_t found_char_last = line_in.find_first_of(" ", found_key);
+					size_t found_char_curr;
+					while (found_char_last != string::npos) {
+						found_char_curr = line_in.find_first_of(" ", found_char_last + 1);
+						string element;
+						if (found_char_curr == string::npos) {
+							element = line_in.substr(found_char_last + 1, line_in.size()-found_char_last);
+							//cout << element << endl;
+						}
+						else {
+							element = line_in.substr(found_char_last + 1, found_char_curr - 1 - found_char_last);
+							//cout << element << endl;
+						}
+						size_t found_E = element.find_first_of("E");
+						string coeff = element.substr(0, found_E);
+						string exp = element.substr(found_E + 1, element.size() - found_E);
+						//cout << coeff << " " << exp << endl;
+						double coeff_num = atof(coeff.c_str());
+						double exp_num = atof(exp.c_str());
+						//cout << exp_num << endl;
+						double num = coeff_num * pow(10.0, exp_num);
+						//cout << num << endl;
+
+						switch (element_index) {
+							case 0:
+								file_out << "[" << num << ", ";
+								element_index += 1;
+								break;
+							case 1:
+								file_out << num << ", ";
+								element_index += 1;
+								break;
+							case 2:
+								file_out << num << "] ";
+								element_index = 0;
+								break;
+						}
+						found_char_last = found_char_curr;
+					}
+					vertex_index += 1;
+					vertex_count += 1;
+					if (vertex_index >= 3) {
+						file_out << endl;
+						vertex_index = 0;
+						triangle_count += 1;
+					}
 				}
+
 			}
 			else {
 				// Check for read failures or end of file
-				if (file_in.fail()) {
-					cout << "Read failed" << endl;
+				if (file_in.eof()) {
+					cout << "Reached end of file, closing files" << endl;
+					cout << "Number of verticies extracted: " << vertex_count << endl;
+					cout << "Number of triangles: " << triangle_count << endl;
+					break;
 				}
-				else if (file_in.eof()) {
-					cout << "Reached end of file, closing input file" << endl;
+				else if (file_in.fail()) {
+					cout << "Read failed" << endl;
+					break;
 				}
+
 			}
 		}
-
-
 		file_in.close();
 		file_out.close();
 	}

src/dev/ezhu/STLParser/README.md

@@ -2,9 +2,13 @@ STL PARSER
 ------------
 An application for parsing ASCII STL files for easy import of triangle meshes into the Bullet Physics Engine
 
+Each line of the output file will consist of 3 verticies defining a triangle. These verticies will be in the following format: 
+
+    [x1, y1, z1] [x2, y2, z2] [x3, y3, z3]
+
 USAGE
 -----------
-$ ./appSTLParser [path/filename of .stl to be converted] [path/filename of .txt to be created with parsed info]
+    $ ./appSTLParser [path/filename of .stl to be converted] [path/filename of .txt to be created with parsed info]
 
 Author
 -----------