save default VR camera tuning, requires MIDI controller

tweak some values in VR demo

data/gripper/wsg50_one_motor_gripper_free_base.sdf

@@ -303,6 +303,10 @@
     </joint>
     
     <link name='finger_right'>
+      <contact>
+        <lateral_friction value="1.0"/>
+        <spinning_friction value="1.5"/>
+      </contact>
       <pose frame=''>0.042 0 0.145 0 0 1.5708</pose>
       <inertial>
         <mass>0.2</mass>
@@ -343,6 +347,10 @@
     </joint>
     
     <link name='finger_left'>
+      <contact>
+        <lateral_friction value="1.0"/>
+        <spinning_friction value="1.5"/>
+      </contact>
       <pose frame=''>-0.042 0 0.145 0 0 4.71239</pose>
       <inertial>
         <mass>0.2</mass>

data/gripper/wsg50_one_motor_gripper_new_free_base.sdf

@@ -300,6 +300,10 @@
     </joint>
     
     <link name='finger_right'>
+      <contact>
+        <lateral_friction>1.0</lateral_friction>
+        <spinning_friction>1.5</spinning_friction>
+      </contact>
       <pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
       <inertial>
         <mass>0.2</mass>
@@ -340,6 +344,10 @@
     </joint>
     
     <link name='finger_left'>
+      <contact>
+        <lateral_friction>1.0</lateral_friction>
+        <spinning_friction>1.5</spinning_friction>
+      </contact>
       <pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
       <inertial>
         <mass>0.2</mass>

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -48,7 +48,7 @@ int gHuskyId = -1;
 btTransform huskyTr = btTransform::getIdentity();
 
 int gCreateObjectSimVR = -1;
-int gEnableKukaControl = 1;
+int gEnableKukaControl = 0;
 
 btScalar simTimeScalingFactor = 1;
 btScalar gRhsClamp = 1.f;
@@ -4016,7 +4016,7 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
 }
 
 
-btVector3 gVRGripperPos(0,0,0.2);
+btVector3 gVRGripperPos(0.6, 0.4, 0.7);
 btQuaternion gVRGripperOrn(0,0,0,1);
 btVector3 gVRController2Pos(0,0,0.2);
 btQuaternion gVRController2Orn(0,0,0,1);
@@ -4156,7 +4156,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
 		{
 			int bodyId = 0;
 
-			if (loadUrdf("pr2_gripper.urdf", btVector3(0, 0, 0.1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()))
+			if (loadUrdf("pr2_gripper.urdf", btVector3(-0.2, 0.15, 0.9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()))
 			{
 				InteralBodyData* parentBody = m_data->getHandle(bodyId);
 				if (parentBody->m_multiBody)
@@ -4186,6 +4186,30 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
 
 		loadUrdf("kuka_iiwa/model_vr_limits.urdf", btVector3(1.4, -0.2, 0.6), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 		m_data->m_KukaId = bodyId;
+		InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
+		if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs() == 7)
+		{
+			btScalar q[7];
+			q[0] = 0;// -SIMD_HALF_PI;
+			q[1] = 0;
+			q[2] = 0;
+			q[3] = SIMD_HALF_PI;
+			q[4] = 0;
+			q[5] = -SIMD_HALF_PI*0.66;
+			q[6] = 0;
+
+			for (int i = 0; i < 7; i++)
+			{
+				kukaBody->m_multiBody->setJointPos(i, q[i]);
+			}
+			btAlignedObjectArray<btQuaternion> scratch_q;
+			btAlignedObjectArray<btVector3> scratch_m;
+			kukaBody->m_multiBody->forwardKinematics(scratch_q, scratch_m);
+			int nLinks = kukaBody->m_multiBody->getNumLinks();
+			scratch_q.resize(nLinks + 1);
+			scratch_m.resize(nLinks + 1);
+			kukaBody->m_multiBody->updateCollisionObjectWorldTransforms(scratch_q, scratch_m);
+		}
 		loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 		loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .8), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 		loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
@@ -4194,7 +4218,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
 		// Load one motor gripper for kuka
 		loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
 		m_data->m_gripperId = bodyId + 1;
-		InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
+		
 		InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId);
 
 		// Reset the default gripper motor maximum torque for damping to 0
@@ -4236,6 +4260,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
 		m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute1);
 		m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute2);
 
+		kukaBody = m_data->getHandle(m_data->m_KukaId);
 		if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs()==7)
 		{
 			gripperBody->m_multiBody->setHasSelfCollision(0);
@@ -4289,7 +4314,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
 
 		// Table area
 		loadUrdf("table/table.urdf", objectWorldTr[0].getOrigin(), objectWorldTr[0].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		loadUrdf("tray/tray_textured.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+		//loadUrdf("tray/tray_textured.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 		//loadUrdf("cup_small.urdf", objectWorldTr[2].getOrigin(), objectWorldTr[2].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 		//loadUrdf("pitcher_small.urdf", objectWorldTr[3].getOrigin(), objectWorldTr[3].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 		loadUrdf("teddy_vhacd.urdf", objectWorldTr[4].getOrigin(), objectWorldTr[4].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
@@ -4330,9 +4355,10 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
 		if (motor)
 		{
 			btScalar posTarget = (-0.048)*btMin(btScalar(0.75), gVRGripper2Analog) / 0.75;
-			motor->setPositionTarget(posTarget, .2);
+			motor->setPositionTarget(posTarget, .8);
 			motor->setVelocityTarget(0.0, .5);
-			motor->setMaxAppliedImpulse(5.0);
+			motor->setMaxAppliedImpulse(1.0);
+
 		}
 	}
 

examples/SharedMemory/PhysicsServerExample.cpp

@@ -27,6 +27,7 @@
 //@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
 extern btVector3 gLastPickPos;
 btVector3 gVRTeleportPos1(0,0,0);
+btScalar gVRTeleportRotZ = 0;
 btQuaternion gVRTeleportOrn(0, 0, 0,1);
 extern btVector3 gVRGripperPos;
 extern btQuaternion gVRGripperOrn;
@@ -46,10 +47,47 @@ extern btScalar simTimeScalingFactor;
 
 extern bool gVRGripperClosed;
 
-#if B3_USE_MIDI
+const char* startFileNameVR = "0_VRDemoSettings.txt";
 
 #include <vector>
 
+//remember the settings (you don't want to re-tune again and again...)
+static void saveCurrentSettingsVR()
+{
+	FILE* f = fopen(startFileNameVR, "w");
+	if (f)
+	{
+		fprintf(f, "--camPosX= %f\n", gVRTeleportPos1[0]);
+		fprintf(f, "--camPosY= %f\n", gVRTeleportPos1[1]);
+		fprintf(f, "--camPosZ= %f\n", gVRTeleportPos1[2]);
+		fprintf(f, "--camRotZ= %f\n", gVRTeleportRotZ);
+		fclose(f);
+	}
+};
+
+static void loadCurrentSettingsVR(b3CommandLineArgs& args)
+{
+	int currentEntry = 0;
+	FILE* f = fopen(startFileNameVR, "r");
+	if (f)
+	{
+		char oneline[1024];
+		char* argv[] = { 0,&oneline[0] };
+
+		while (fgets(oneline, 1024, f) != NULL)
+		{
+			char *pos;
+			if ((pos = strchr(oneline, '\n')) != NULL)
+				*pos = '\0';
+			args.addArgs(2, argv);
+		}
+		fclose(f);
+	}
+
+};
+#if B3_USE_MIDI
+
+
 static float getParamf(float rangeMin, float rangeMax, int midiVal)
 {
 	float v = rangeMin + (rangeMax - rangeMin)* (float(midiVal / 127.));
@@ -70,9 +108,10 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void
 		{
 			if (message->at(1) == 16)
 			{
-				float rotZ = getParamf(-3.1415, 3.1415, message->at(2));
+				gVRTeleportRotZ= getParamf(-3.1415, 3.1415, message->at(2));
-				gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), rotZ);
+				gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
-				b3Printf("gVRTeleportOrn rotZ = %f\n", rotZ);
+				saveCurrentSettingsVR();
+				b3Printf("gVRTeleportOrn rotZ = %f\n", gVRTeleportRotZ);
 			}
 
 			if (message->at(1) == 32)
@@ -85,6 +124,7 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void
 				if (message->at(1) == i)
 				{
 					gVRTeleportPos1[i] = getParamf(-2, 2, message->at(2));
+					saveCurrentSettingsVR();
 					b3Printf("gVRTeleportPos[%d] =  %f\n", i,gVRTeleportPos1[i]);
 
 				}
@@ -927,7 +967,7 @@ public:
 	virtual void	processCommandLineArgs(int argc, char* argv[])
 	{
 		b3CommandLineArgs args(argc,argv);
-
+		loadCurrentSettingsVR(args);
 		if (args.GetCmdLineArgument("camPosX", gVRTeleportPos1[0]))
 		{
 			printf("camPosX=%f\n", gVRTeleportPos1[0]);
@@ -1665,8 +1705,10 @@ void	PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
 		return;
 
 	if (gGraspingController < 0)
+	{
 		gGraspingController = controllerId;
-
+		gEnableKukaControl = true;
+	}
 	if (controllerId != gGraspingController)
 	{
 		if (button == 1 && state == 0)