diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index a7eb18097..1ad7c41c1 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1332,7 +1332,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 +1367,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_uIndexm_initPoseArgs.m_initialStateQdot[info.m_uIndex] = jointVelocity; diff --git a/examples/SharedMemory/plugins/vrSyncPlugin/premake4.lua b/examples/SharedMemory/plugins/vrSyncPlugin/premake4.lua new file mode 100644 index 000000000..f23cc6c7e --- /dev/null +++ b/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", + } + + + diff --git a/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp b/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp new file mode 100644 index 000000000..ec14a5165 --- /dev/null +++ b/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 + +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;nm_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"); +} diff --git a/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.h b/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.h new file mode 100644 index 000000000..fcc60a610 --- /dev/null +++ b/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 diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index d729029a8..f7c004b1e 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -409,3 +409,6 @@ end include "udp" include "tcp" include "plugins/testPlugin" +include "plugins/vrSyncPlugin" + +