Merge pull request #1339 from erwincoumans/master

add experimental vrSyncPlugin, that syncs the position/orientation of…

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -649,6 +649,18 @@ B3_SHARED_API int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryC
 	return 0;
 }
 
+B3_SHARED_API	int b3RequestActualStateCommandComputeForwardKinematics(b3SharedMemoryCommandHandle commandHandle, int computeForwardKinematics)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command);
+	btAssert(command->m_type == CMD_REQUEST_ACTUAL_STATE);
+	if (computeForwardKinematics && command->m_type == CMD_REQUEST_ACTUAL_STATE)
+	{
+		command->m_updateFlags |= ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS;
+	}
+	return 0;
+}
+
 
 B3_SHARED_API int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState *state)
 {
@@ -1332,7 +1344,7 @@ B3_SHARED_API	int	b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle phys
 	command->m_updateFlags |=INIT_POSE_HAS_JOINT_STATE;
 	b3JointInfo info;
 	b3GetJointInfo(physClient, command->m_initPoseArgs.m_bodyUniqueId,jointIndex, &info);
-	btAssert((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0);
+	//btAssert((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0);
 	if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0)
 	{  
 		command->m_initPoseArgs.m_initialStateQ[info.m_qIndex] = jointPosition;
@@ -1367,7 +1379,7 @@ B3_SHARED_API	int	b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle phys
 	command->m_updateFlags |=INIT_POSE_HAS_JOINT_VELOCITY;
 	b3JointInfo info;
 	b3GetJointInfo(physClient, command->m_initPoseArgs.m_bodyUniqueId,jointIndex, &info);
-	btAssert((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_uIndex >=0);
+	//btAssert((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_uIndex >=0);
 	if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && (info.m_uIndex >=0) && (info.m_uIndex<MAX_DEGREE_OF_FREEDOM))
 	{
 		command->m_initPoseArgs.m_initialStateQdot[info.m_uIndex] = jointVelocity;

examples/SharedMemory/PhysicsClientC_API.h

@@ -439,6 +439,8 @@ B3_SHARED_API	int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle com
 
 B3_SHARED_API	b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);
 B3_SHARED_API	int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryCommandHandle commandHandle, int computeLinkVelocity);
+B3_SHARED_API	int b3RequestActualStateCommandComputeForwardKinematics(b3SharedMemoryCommandHandle commandHandle, int computeForwardKinematics);
+
 
 B3_SHARED_API	int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);
 B3_SHARED_API	int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *state);

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -1204,6 +1204,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
 			{
 				break;
 			}
+			case CMD_CALCULATED_JACOBIAN_COMPLETED:
+			{
+				break;
+			}
+			case CMD_CALCULATED_JACOBIAN_FAILED:
+			{
+				b3Warning("jacobian calculation failed");
+				break;
+			}
 			case CMD_CUSTOM_COMMAND_FAILED:
 			{
 				b3Warning("custom plugin command failed");

examples/SharedMemory/PhysicsDirect.cpp

@@ -949,7 +949,31 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
 		b3Warning("custom plugin command failed");
 		break;
 	}
-
+	case CMD_CLIENT_COMMAND_COMPLETED:
+		{
+			break;
+		}
+	case CMD_CALCULATED_JACOBIAN_COMPLETED:
+	{
+		break;
+	}
+	case CMD_CALCULATED_JACOBIAN_FAILED:
+	{
+		b3Warning("jacobian calculation failed");
+		break;
+	}
+	case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
+		{
+			break;
+		}
+	case CMD_DESIRED_STATE_RECEIVED_COMPLETED:
+		{
+			break;
+		}
+	case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
+		{
+			break;
+		}
 	default:
 	{
 		//b3Warning("Unknown server status type");

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -5205,6 +5205,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 							btAlignedObjectArray<btVector3> omega;
 							btAlignedObjectArray<btVector3> linVel;
 							
+							bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS)!=0);
+							if (computeForwardKinematics)
+							{
+								B3_PROFILE("compForwardKinematics");
+								btAlignedObjectArray<btQuaternion> world_to_local;
+								btAlignedObjectArray<btVector3> local_origin;
+								world_to_local.resize(mb->getNumLinks()+1);
+								local_origin.resize(mb->getNumLinks()+1);
+								mb->forwardKinematics(world_to_local,local_origin);
+							}
+
 							bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY)!=0);
 							if (computeLinkVelocities)
 							{

examples/SharedMemory/SharedMemoryPublic.h

@@ -5,7 +5,8 @@
 ///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
 ///my convention is year/month/day/rev
 
-#define SHARED_MEMORY_MAGIC_NUMBER 201708270
+#define SHARED_MEMORY_MAGIC_NUMBER 201709260
+//#define SHARED_MEMORY_MAGIC_NUMBER 201708270
 //#define SHARED_MEMORY_MAGIC_NUMBER 201707140
 //#define SHARED_MEMORY_MAGIC_NUMBER 201706015
 //#define SHARED_MEMORY_MAGIC_NUMBER 201706001
@@ -505,7 +506,8 @@ struct b3VisualShapeInformation
 
 enum eLinkStateFlags
 {
-	ACTUAL_STATE_COMPUTE_LINKVELOCITY=1
+	ACTUAL_STATE_COMPUTE_LINKVELOCITY=1,
+	ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS=2,
 };
 
 ///b3LinkState provides extra information such as the Cartesian world coordinates

examples/SharedMemory/plugins/vrSyncPlugin/premake4.lua

@@ -0,0 +1,42 @@
+		
+
+project ("pybullet_vrSyncPlugin")
+		language "C++"
+		kind "SharedLib"
+		
+		includedirs {".","../../../../src", "../../../../examples",
+		"../../../ThirdPartyLibs"}
+		defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
+	hasCL = findOpenCL("clew")
+
+	links{"BulletFileLoader", "Bullet3Common", "LinearMath"}
+
+
+	if os.is("MacOSX") then
+--		targetextension {"so"}
+		links{"Cocoa.framework","Python"}
+	end
+
+
+		files {
+			"vrSyncPlugin.cpp",
+			"../../PhysicsClient.cpp",
+			"../../PhysicsClient.h",
+			"../../PhysicsClientSharedMemory.cpp",
+			"../../PhysicsClientSharedMemory.h",
+			"../../PhysicsClientSharedMemory_C_API.cpp",
+			"../../PhysicsClientSharedMemory_C_API.h",
+			"../../PhysicsClientC_API.cpp",
+			"../../PhysicsClientC_API.h",
+			"../../Win32SharedMemory.cpp",
+			"../../Win32SharedMemory.h",
+			"../../PosixSharedMemory.cpp",
+			"../../PosixSharedMemory.h",
+			"../../../Utils/b3Clock.cpp",
+			"../../../Utils/b3Clock.h",
+			"../../../Utils/b3ResourcePath.cpp",
+			"../../../Utils/b3ResourcePath.h",
+				}
+	
+	
+	

examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp

@@ -0,0 +1,116 @@
+
+//vrSyncPlugin plugin, will query the vr controller events
+//and set change the user constraint to match the pose
+
+//in Python you can load and configure the plugin like this:
+//plugin = p.loadPlugin("e:/develop/bullet3/bin/pybullet_vrSyncPlugin_vs2010_x64_release.dll")
+//could also be plugin = p.loadPlugin("vrSyncPlugin.so") on Mac/Linux
+//controllerId = 3
+//p.executePluginCommand(plugin ,"bla", [controllerId,pr2_cid],[50])
+
+#include "vrSyncPlugin.h"
+#include "../../SharedMemoryPublic.h"
+#include "../b3PluginContext.h"
+#include <stdio.h>
+
+struct MyClass
+{
+	int m_testData;
+
+	int m_controllerId;
+	int m_constraintId;
+	float m_maxForce;
+	MyClass()
+		:m_testData(42),
+		m_controllerId(-1),
+		m_constraintId(-1),
+		m_maxForce(0)
+	{
+	}
+	virtual ~MyClass()
+	{
+	}
+};
+
+B3_SHARED_API int initPlugin(struct b3PluginContext* context)
+{
+	MyClass* obj = new MyClass();
+	context->m_userPointer = obj;
+
+	printf("hi vrSyncPlugin!\n");
+	return SHARED_MEMORY_MAGIC_NUMBER;
+}
+
+
+B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context)
+{
+	MyClass* obj = (MyClass* )context->m_userPointer;
+	if (obj->m_controllerId>=0)
+	{
+		b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(context->m_physClient);
+		int deviceTypeFilter = VR_DEVICE_CONTROLLER;
+		b3VREventsSetDeviceTypeFilter(commandHandle, deviceTypeFilter);
+
+		b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
+		int statusType = b3GetStatusType(statusHandle);
+		if (statusType == CMD_REQUEST_VR_EVENTS_DATA_COMPLETED)
+		{
+			struct b3VREventsData vrEvents;
+		
+			int i = 0;
+			b3GetVREventsData(context->m_physClient, &vrEvents);
+			if (vrEvents.m_numControllerEvents)
+			{
+				for (int n=0;n<vrEvents.m_numControllerEvents;n++)
+				{
+					b3VRControllerEvent& event = vrEvents.m_controllerEvents[n];
+					if (event.m_controllerId ==obj->m_controllerId)
+					{
+						b3SharedMemoryCommandHandle commandHandle;
+						int userConstraintUniqueId = obj->m_constraintId;
+						commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, userConstraintUniqueId);
+						double pos[4] = {event.m_pos[0],event.m_pos[1],event.m_pos[2],1};
+						b3InitChangeUserConstraintSetPivotInB(commandHandle, pos);
+						double orn[4] = {event.m_orn[0],event.m_orn[1],event.m_orn[2],event.m_orn[3]};
+						b3InitChangeUserConstraintSetFrameInB(commandHandle, orn);
+						b3InitChangeUserConstraintSetMaxForce(commandHandle, obj->m_maxForce);
+						
+						b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
+						//this is basically equivalent to doing this in Python/pybullet:
+						//p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=500)
+					}
+				}
+			}
+		}
+	}
+
+	return 0;
+}
+
+
+
+B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
+{
+	MyClass* obj = (MyClass*) context->m_userPointer;
+	if (arguments->m_numInts>=2 && arguments->m_numFloats >= 0)
+	{
+		obj->m_constraintId = arguments->m_ints[1];
+		printf("obj->m_constraintId=%d\n", obj->m_constraintId);
+		obj->m_maxForce = arguments->m_floats[0];
+		printf("obj->m_maxForce = %f\n", obj->m_maxForce);
+		obj->m_controllerId = arguments->m_ints[0];
+		printf("obj->m_controllerId=%d\n", obj->m_controllerId);
+
+	}
+	return 0;
+}
+
+
+B3_SHARED_API void exitPlugin(struct b3PluginContext* context)
+{
+	MyClass* obj = (MyClass*) context->m_userPointer;
+	delete obj;
+	context->m_userPointer = 0;
+
+	printf("bye vrSyncPlugin!\n");
+}

examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.h

@@ -0,0 +1,25 @@
+#ifndef TEST_PLUGIN_H
+#define TEST_PLUGIN_H
+
+#include "../b3PluginAPI.h"
+
+#ifdef __cplusplus
+extern "C" 
+{
+#endif
+
+//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
+B3_SHARED_API int initPlugin(struct b3PluginContext* context);
+B3_SHARED_API void exitPlugin(struct b3PluginContext* context);
+B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
+
+//preTickPluginCallback and postTickPluginCallback are optional.
+B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context);
+
+
+
+#ifdef __cplusplus
+};
+#endif
+
+#endif//#define TEST_PLUGIN_H

examples/SharedMemory/premake4.lua

@@ -409,3 +409,6 @@ end
 include "udp"
 include "tcp"
 include "plugins/testPlugin"
+include "plugins/vrSyncPlugin"
+
+

examples/pybullet/pybullet.c

@@ -3126,13 +3126,14 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args, PyObject*
 	int bodyUniqueId = -1;
 	int linkIndex = -1;
 	int computeLinkVelocity = 0;
+	int computeForwardKinematics = 0;
 
 	int i;
 	b3PhysicsClientHandle sm = 0;
 
 	int physicsClientId = 0;
-	static char* kwlist[] = {"bodyUniqueId", "linkIndex", "computeLinkVelocity", "physicsClientId", NULL};
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ii", kwlist, &bodyUniqueId, &linkIndex,&computeLinkVelocity, &physicsClientId))
+	static char* kwlist[] = {"bodyUniqueId", "linkIndex", "computeLinkVelocity", "computeForwardKinematics", "physicsClientId", NULL};
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|iii", kwlist, &bodyUniqueId, &linkIndex,&computeLinkVelocity,&computeForwardKinematics,&physicsClientId))
 	{
 		return NULL;
 	}
@@ -3168,6 +3169,11 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args, PyObject*
 				b3RequestActualStateCommandComputeLinkVelocity(cmd_handle,computeLinkVelocity);
 			}
 
+			if (computeForwardKinematics)
+			{
+				b3RequestActualStateCommandComputeForwardKinematics(cmd_handle,computeForwardKinematics);
+			}
+
 			status_handle =
 				b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
 
@@ -3367,6 +3373,8 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj
 	double textSize = 1.f;
 	double lifeTime = 0.f;
 	int physicsClientId = 0;
+	int debugItemUniqueId = -1;
+
 	b3PhysicsClientHandle sm = 0;
 	static char* kwlist[] = {"text", "textPosition", "textColorRGB", "textSize", "lifeTime", "textOrientation", "parentObjectUniqueId", "parentLinkIndex", "physicsClientId", NULL};
 
@@ -3419,15 +3427,14 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj
 
 	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
 	statusType = b3GetStatusType(statusHandle);
+	
 	if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED)
 	{
-		int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
-		PyObject* item = PyInt_FromLong(debugItemUniqueId);
-		return item;
+		debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
 	}
 
-	PyErr_SetString(SpamError, "Error in addUserDebugText.");
-	return NULL;
+	PyObject* item = PyInt_FromLong(debugItemUniqueId);
+	return item;
 }
 
 static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObject* keywds)
@@ -3449,6 +3456,7 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj
 	double lineWidth = 1.f;
 	double lifeTime = 0.f;
 	int physicsClientId = 0;
+	int debugItemUniqueId = -1;
 	b3PhysicsClientHandle sm = 0;
 	static char* kwlist[] = {"lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", "parentObjectUniqueId", "parentLinkIndex", "physicsClientId", NULL};
 
@@ -3492,13 +3500,10 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj
 	statusType = b3GetStatusType(statusHandle);
 	if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED)
 	{
-		int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
-		PyObject* item = PyInt_FromLong(debugItemUniqueId);
-		return item;
+		debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
 	}
-
-	PyErr_SetString(SpamError, "Error in addUserDebugLine.");
-	return NULL;
+	PyObject* item = PyInt_FromLong(debugItemUniqueId);
+	return item;
 }
 
 static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, PyObject* keywds)