added rotation matrix to example 4

Author: Amy Blank

examples/.gitignore

@@ -0,0 +1,12 @@
+# Built libbarrett examples
+ex01_initialize_wam
+ex02_hold_position
+ex03_simple_move
+ex04_display_basic_info
+ex05_systems_intro
+ex06_realtime_move
+ex07_realtime_data_logging
+ex08_teach_and_play
+ex09_torque_control
+ex10_haptics
+ex11_master_master

examples/ex04_display_basic_info.cpp

@@ -96,6 +96,10 @@ int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam)
 	mvprintw(line++,0, "         Tool Position (m): ");
 	mvprintw(line++,0, "   Tool Orientation (quat): ");
 	line++;
+	mvprintw(line++,0, "                            ");
+	mvprintw(line++,0, "    Tool Orientation (mat): ");
+	mvprintw(line++,0, "                            ");
+	line++;
 
 	if (fts != NULL) {
 		mvprintw(line++,0, "F/T Sensor");
@@ -137,6 +141,7 @@ int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam)
 	jt_type jt;
 	cp_type cp;
 	Eigen::Quaterniond to;
+	Eigen::Matrix3d R;
 	math::Matrix<6,DOF> J;
 
 	cf_type cf;
@@ -182,6 +187,11 @@ int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam)
 		to = wam.getToolOrientation();  // We work only with unit quaternions. No saturation necessary.
     	mvprintw(line++,wamX, "%+7.4f %+7.4fi %+7.4fj %+7.4fk", to.w(), to.x(), to.y(), to.z());
 
+    R = to.toRotationMatrix(); // convert to a 3x3 rotation matrix
+    	line++;
+    	mvprintw(line++,wamX, "[%+7.4f %+7.4f %+7.4f]", R(0,0), R(0,1), R(0,2));
+    	mvprintw(line++,wamX, "[%+7.4f %+7.4f %+7.4f]", R(1,0), R(1,1), R(1,2));
+    	mvprintw(line++,wamX, "[%+7.4f %+7.4f %+7.4f]", R(2,0), R(2,1), R(2,2));
 
 		// FTS
 		if (fts != NULL) {