Add pybullet setVRCameraState and b3SetVRCameraStateCommandInit to set the VR camera root transform (position/orientation) and optional tracking object unique id (-1 for no tracking)

Fix robotcontrol.py script, getContactPointData -> getContactPoints
2017-01-05 17:41:58 -08:00
parent c940f0ec47
commit 83e103ac15
10 changed files with 229 additions and 41 deletions
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -2191,3 +2191,54 @@ void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData*
 		cl->getCachedVREvents(vrEventsData);
 	}
 }
+
+b3SharedMemoryCommandHandle	b3SetVRCameraStateCommandInit(b3PhysicsClientHandle physClient)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+	struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+	b3Assert(command);
+
+	command->m_type = CMD_SET_VR_CAMERA_STATE;
+	command->m_updateFlags = 0;
+
+	return (b3SharedMemoryCommandHandle)command;
+
+}
+
+int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double rootPos[3])
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE);
+    command->m_updateFlags |= VR_CAMERA_ROOT_POSITION;
+	command->m_vrCameraStateArguments.m_rootPosition[0] = rootPos[0];
+	command->m_vrCameraStateArguments.m_rootPosition[1] = rootPos[1];
+	command->m_vrCameraStateArguments.m_rootPosition[2] = rootPos[2];
+	return 0;
+
+}
+
+int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[4])
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE);
+    command->m_updateFlags |= VR_CAMERA_ROOT_ORIENTATION;
+	command->m_vrCameraStateArguments.m_rootOrientation[0] = rootOrn[0];
+	command->m_vrCameraStateArguments.m_rootOrientation[1] = rootOrn[1];
+	command->m_vrCameraStateArguments.m_rootOrientation[2] = rootOrn[2];
+	return 0;
+}
+
+int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE);
+    command->m_updateFlags |= VR_CAMERA_ROOT_TRACKING_OBJECT;
+	command->m_vrCameraStateArguments.m_trackingObjectUniqueId = objectUniqueId;
+	return 0;
+}
+
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -314,6 +314,12 @@ int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, dou
 b3SharedMemoryCommandHandle	b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient);
 void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* vrEventsData);

+b3SharedMemoryCommandHandle	b3SetVRCameraStateCommandInit(b3PhysicsClientHandle physClient);
+int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double rootPos[3]);
+int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[4]);
+int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId);
+
+


 #ifdef __cplusplus
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -44,11 +44,14 @@ bool gEnableRealTimeSimVR=false;
 bool gCreateDefaultRobotAssets = false;
 int gInternalSimFlags = 0;
 bool gResetSimulation = 0;
-int gHuskyId = -1;
-btTransform huskyTr = btTransform::getIdentity();
+int gVRTrackingObjectUniqueId = -1;
+btTransform gVRTrackingObjectTr = btTransform::getIdentity();

 int gCreateObjectSimVR = -1;
 int gEnableKukaControl = 0;
+btVector3 gVRTeleportPos1(0,0,0);
+btQuaternion gVRTeleportOrn(0, 0, 0,1);
+

 btScalar simTimeScalingFactor = 1;
 btScalar gRhsClamp = 1.f;
@@ -1369,6 +1372,32 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 				}
 #endif

+				case CMD_SET_VR_CAMERA_STATE:
+				{
+
+					if (clientCmd.m_updateFlags & 	VR_CAMERA_ROOT_POSITION)
+					{
+						gVRTeleportPos1[0] = clientCmd.m_vrCameraStateArguments.m_rootPosition[0];
+						gVRTeleportPos1[1] = clientCmd.m_vrCameraStateArguments.m_rootPosition[1];
+						gVRTeleportPos1[2] = clientCmd.m_vrCameraStateArguments.m_rootPosition[2];
+					}
+					if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_ORIENTATION)
+					{
+						gVRTeleportOrn[0] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[0];
+						gVRTeleportOrn[1] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[1];
+						gVRTeleportOrn[2] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[2];
+						gVRTeleportOrn[3] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[3];
+					}
+
+					if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_TRACKING_OBJECT)
+					{
+						gVRTrackingObjectUniqueId = clientCmd.m_vrCameraStateArguments.m_trackingObjectUniqueId;
+					}
+
+					serverStatusOut.m_type  = CMD_CLIENT_COMMAND_COMPLETED;
+					hasStatus = true;
+					break;
+				}
 				case CMD_REQUEST_VR_EVENTS_DATA:
 				{
 					serverStatusOut.m_sendVREvents.m_numVRControllerEvents = 0;
@@ -4287,12 +4316,12 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec,	const
 			gSubStep = m_data->m_physicsDeltaTime;
 		}
 		
-		if (gHuskyId >= 0)
+		if (gVRTrackingObjectUniqueId >= 0)
 		{
-			InternalBodyHandle* bodyHandle = m_data->getHandle(gHuskyId);
+			InternalBodyHandle* bodyHandle = m_data->getHandle(gVRTrackingObjectUniqueId);
 			if (bodyHandle && bodyHandle->m_multiBody)
 			{
-				huskyTr = bodyHandle->m_multiBody->getBaseWorldTransform();
+				gVRTrackingObjectTr  = bodyHandle->m_multiBody->getBaseWorldTransform();
 			}
 		}

@@ -4595,8 +4624,6 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
 		//loadUrdf("knight.urdf", btVector3(-1.2, 0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());

 		loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		gHuskyId = bodyId;
-		b3Printf("huskyId = %d", gHuskyId);
 		m_data->m_huskyId = bodyId;

 		m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
--- a/examples/SharedMemory/PhysicsServerExample.cpp
+++ b/examples/SharedMemory/PhysicsServerExample.cpp
@@ -26,9 +26,14 @@

 //@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
 extern btVector3 gLastPickPos;
-btVector3 gVRTeleportPos1(0,0,0);
+
+btVector3 gVRTeleportPosLocal(0,0,0);
+btQuaternion gVRTeleportOrnLocal(0,0,0,1);
+
+extern btVector3 gVRTeleportPos1;
+extern btQuaternion gVRTeleportOrn;
 btScalar gVRTeleportRotZ = 0;
-btQuaternion gVRTeleportOrn(0, 0, 0,1);
+
 extern btVector3 gVRGripperPos;
 extern btQuaternion gVRGripperOrn;
 extern btVector3 gVRController2Pos;
@@ -57,9 +62,9 @@ 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, "--camPosX= %f\n", gVRTeleportPosLocal[0]);
+		fprintf(f, "--camPosY= %f\n", gVRTeleportPosLocal[1]);
+		fprintf(f, "--camPosZ= %f\n", gVRTeleportPosLocal[2]);
 		fprintf(f, "--camRotZ= %f\n", gVRTeleportRotZ);
 		fclose(f);
 	}
@@ -109,9 +114,9 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void
 			if (message->at(1) == 16)
 			{
 				gVRTeleportRotZ= getParamf(-3.1415, 3.1415, message->at(2));
-				gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
+				gVRTeleportOrnLocal = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
 				saveCurrentSettingsVR();
-//				b3Printf("gVRTeleportOrn rotZ = %f\n", gVRTeleportRotZ);
+//				b3Printf("gVRTeleportOrnLocal rotZ = %f\n", gVRTeleportRotZ);
 			}

 			if (message->at(1) == 32)
@@ -123,9 +128,9 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void
 			{
 				if (message->at(1) == i)
 				{
-					gVRTeleportPos1[i] = getParamf(-2, 2, message->at(2));
+					gVRTeleportPosLocal[i] = getParamf(-2, 2, message->at(2));
 					saveCurrentSettingsVR();
-//					b3Printf("gVRTeleportPos[%d] =  %f\n", i,gVRTeleportPos1[i]);
+//					b3Printf("gVRTeleportPos[%d] =  %f\n", i,gVRTeleportPosLocal[i]);

 				}
 			}
@@ -1040,19 +1045,19 @@ public:
 			setSharedMemoryKey(shmemKey);
 		}

-		if (args.GetCmdLineArgument("camPosX", gVRTeleportPos1[0]))
+		if (args.GetCmdLineArgument("camPosX", gVRTeleportPosLocal[0]))
 		{
-			printf("camPosX=%f\n", gVRTeleportPos1[0]);
+			printf("camPosX=%f\n", gVRTeleportPosLocal[0]);
 		}

-		if (args.GetCmdLineArgument("camPosY", gVRTeleportPos1[1]))
+		if (args.GetCmdLineArgument("camPosY", gVRTeleportPosLocal[1]))
 		{
-			printf("camPosY=%f\n", gVRTeleportPos1[1]);
+			printf("camPosY=%f\n", gVRTeleportPosLocal[1]);
 		}

-		if (args.GetCmdLineArgument("camPosZ", gVRTeleportPos1[2]))
+		if (args.GetCmdLineArgument("camPosZ", gVRTeleportPosLocal[2]))
 		{
-			printf("camPosZ=%f\n", gVRTeleportPos1[2]);
+			printf("camPosZ=%f\n", gVRTeleportPosLocal[2]);
 		}

 		float camRotZ = 0.f;
@@ -1060,7 +1065,7 @@ public:
 		{
 			printf("camRotZ = %f\n", camRotZ);
 			btQuaternion ornZ(btVector3(0, 0, 1), camRotZ);
-			gVRTeleportOrn = ornZ;
+			gVRTeleportOrnLocal = ornZ;
 		}

 		if (args.CheckCmdLineFlag("robotassets"))
@@ -1474,8 +1479,8 @@ extern int gDroppedSimulationSteps;
 extern int gNumSteps;
 extern double gDtInSec;
 extern double gSubStep;
-extern int gHuskyId;
-extern btTransform huskyTr;
+extern int gVRTrackingObjectUniqueId;
+extern btTransform gVRTrackingObjectTr;

  struct LineSegment
                {
@@ -1607,15 +1612,22 @@ void PhysicsServerExample::drawUserDebugLines()

 void PhysicsServerExample::renderScene()
 {
+	btTransform vrTrans;
+	gVRTeleportPos1 = gVRTeleportPosLocal;
+	gVRTeleportOrn = gVRTeleportOrnLocal;

-#if 0
 	///little VR test to follow/drive Husky vehicle
-	if (gHuskyId >= 0)
+	if (gVRTrackingObjectUniqueId >= 0)
 	{
-		gVRTeleportPos1 = huskyTr.getOrigin();
-		gVRTeleportOrn = huskyTr.getRotation();
+		btTransform vrTrans;
+		vrTrans.setOrigin(gVRTeleportPosLocal);
+		vrTrans.setRotation(gVRTeleportOrnLocal);
+			
+		vrTrans = vrTrans * gVRTrackingObjectTr;
+
+		gVRTeleportPos1 = vrTrans.getOrigin();
+		gVRTeleportOrn = vrTrans.getRotation();
 	}
-#endif
 		

 	B3_PROFILE("PhysicsServerExample::RenderScene");
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -594,6 +594,26 @@ struct UserDebugDrawResultArgs
 	int m_debugItemUniqueId;
 };

+struct SendVREvents
+{
+	int m_numVRControllerEvents;
+	b3VRControllerEvent m_controllerEvents[MAX_VR_CONTROLLERS];
+};
+
+enum eVRCameraEnums
+{
+	VR_CAMERA_ROOT_POSITION=1,
+	VR_CAMERA_ROOT_ORIENTATION=2,
+	VR_CAMERA_ROOT_TRACKING_OBJECT=4
+};
+
+struct VRCameraState
+{
+	double m_rootPosition[3];
+	double m_rootOrientation[4];
+	int m_trackingObjectUniqueId;
+};
+

 struct SharedMemoryCommand
 {
@@ -635,6 +655,7 @@ struct SharedMemoryCommand
 		struct UserDebugDrawArgs m_userDebugDrawArgs;
 		struct RequestRaycastIntersections m_requestRaycastIntersections;
        struct LoadBunnyArgs m_loadBunnyArguments;
+		struct VRCameraState m_vrCameraStateArguments;
    };
 };

@@ -657,11 +678,8 @@ struct SendOverlappingObjectsArgs
 	int m_numRemainingOverlappingObjects;
 };

-struct SendVREvents
-{
-	int m_numVRControllerEvents;
-	b3VRControllerEvent m_controllerEvents[MAX_VR_CONTROLLERS];
-};
+
+



--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/examples/SharedMemory/SharedMemoryPublic.h
@@ -46,6 +46,8 @@ enum EnumSharedMemoryClientCommand
    CMD_SET_SHADOW,
 	CMD_USER_DEBUG_DRAW,
 	CMD_REQUEST_VR_EVENTS_DATA,
+	CMD_SET_VR_CAMERA_STATE,
+
    //don't go beyond this command!
    CMD_MAX_CLIENT_COMMANDS,