From eb7cf3829326cc57531012b3c43e292cee99d26a Mon Sep 17 00:00:00 2001
From: Erwin Coumans <erwin.coumans@gmail.com>
Date: Tue, 2 May 2017 22:33:42 -0700
Subject: [PATCH] Add option to --useKitchen=1 to VRGloveSimulatorMain

---
 data/plane_with_collision_audio.urdf          |  2 +-
 .../RobotSimulator/VRGloveSimulatorMain.cpp   | 32 +++++++++++--------
 2 files changed, 19 insertions(+), 15 deletions(-)

diff --git a/data/plane_with_collision_audio.urdf b/data/plane_with_collision_audio.urdf
index b3a189628..732361eb5 100644
--- a/data/plane_with_collision_audio.urdf
+++ b/data/plane_with_collision_audio.urdf
@@ -2,7 +2,7 @@
 <robot name="plane">
   <link name="planeLink">
   	<audio_source filename="wav/cardboardbox0.wav">
-  		<collision_force_threshold>0.4</collision_force_threshold>
+  		<collision_force_threshold>0.04</collision_force_threshold>
 			<attack_rate>0.01</attack_rate>
 			<decay_rate>0.0001</decay_rate>
 			<sustain_level>0.5</sustain_level>
diff --git a/examples/RobotSimulator/VRGloveSimulatorMain.cpp b/examples/RobotSimulator/VRGloveSimulatorMain.cpp
index 11a8b13b3..6e89f2d0d 100644
--- a/examples/RobotSimulator/VRGloveSimulatorMain.cpp
+++ b/examples/RobotSimulator/VRGloveSimulatorMain.cpp
@@ -40,13 +40,12 @@ int main(int argc, char* argv[])
 	args.GetCmdLineArgument("disableGui",disableGui);
 	int disableShadows = 0;
 	args.GetCmdLineArgument("disableShadows",disableShadows);
+	int useKitchen = 0;
+	args.GetCmdLineArgument("useKitchen",useKitchen);
 	
 	int deviceTypeFilter =	VR_DEVICE_GENERIC_TRACKER;
 	args.GetCmdLineArgument("deviceTypeFilter",deviceTypeFilter);
 
-	int wireFrame = 0;
-	args.GetCmdLineArgument("wireFrame",wireFrame);
-
 	printf("port=%s, speed=%d, connection mode=%s\n", port.c_str(), baud,mode.c_str());	
 	
 	
@@ -82,11 +81,6 @@ int main(int argc, char* argv[])
 		sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);
 	}
 
-	if (wireFrame)
-	{
-		sim->configureDebugVisualizer( COV_ENABLE_WIREFRAME, 1);
-	}
-	
 
 	sim->setTimeOut(12345);
 	//syncBodies is only needed when connecting to an existing physics server that has already some bodies
@@ -102,8 +96,14 @@ int main(int argc, char* argv[])
 	sim->setGravity(b3MakeVector3(0,0,-9.8));
 	sim->setContactBreakingThreshold(0.001);
 
-	sim->loadURDF("plane_with_collision_audio.urdf");
-
+	if (useKitchen)
+	{
+		b3RobotSimulatorLoadFileResults res;
+		sim->loadSDF("kitchens/1.sdf",res);
+	} else
+	{
+		sim->loadURDF("plane_with_collision_audio.urdf");
+	}
 	
 	int handUid = -1;
 
@@ -156,6 +156,7 @@ int main(int argc, char* argv[])
 	jointInfo.m_parentFrame[5] = handOrn[2];
 	jointInfo.m_parentFrame[6] = handOrn[3];
 
+	sim->resetBasePositionAndOrientation(handUid,handStartPosWorld,handStartOrnWorld);
 	int handConstraintId = sim->createConstraint(handUid,-1,-1,-1,&jointInfo);
 	double maxFingerForce = 10;
 	double maxArmForce  = 1000;
@@ -182,7 +183,7 @@ int main(int argc, char* argv[])
 	sim->loadURDF("cube_small.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3( 0.950000,-0.100000,0.700000),b3Quaternion(0.000000,0.000000,0.707107,0.707107)));
 	sim->loadURDF("sphere_small.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.850000,-0.400000,0.700000),b3Quaternion(0.000000,0.000000,0.707107,0.707107)));
 
-
+	
 	b3Clock clock;
 	double startTime = clock.getTimeInSeconds();
 	double simWallClockSeconds = 20.;
@@ -200,7 +201,7 @@ int main(int argc, char* argv[])
 		my_serial.setBytesize(serial::sevenbits);
 		my_serial.setParity(serial::parity_odd);
 		my_serial.setStopbits(serial::stopbits_two);
-		my_serial.setTimeout(serial::Timeout::simpleTimeout(10));
+		my_serial.setTimeout(serial::Timeout::simpleTimeout(0.01));
 		my_serial.open();
 	} catch(...)
 	{
@@ -216,10 +217,13 @@ int main(int argc, char* argv[])
 
 
 	my_serial.flush();
+
+	
 	while (sim->canSubmitCommand())
 	{
+		clock.usleep(1);
+
 		b3VREventsData vrEvents;
-	
 
 		sim->getVREvents(&vrEvents, deviceTypeFilter);
 		//instead of iterating over all vr events, we just take the most up-to-date one
@@ -258,7 +262,7 @@ int main(int argc, char* argv[])
 		}
 		if (result.length())
 		{
-			//my_serial.flush();
+			my_serial.flush();
 			int res = result.find("\n");
 			while (res<0)
 			{