270a036cd7
This is in C++ and the sync runs at the simulation speed (240 Hz), so there is less lag than in Python.
Modify the pybullet/examples/vr_kuka_setup.py at the end to do this:
plugin = p.loadPlugin("e:/develop/bullet3/bin/pybullet_vrSyncPlugin_vs2010_x64_release.dll")
controllerId = 3
p.executePluginCommand(plugin ,"bla", [controllerId,pr2_cid],[50])
117 lines
3.5 KiB
C++
117 lines
3.5 KiB
C++
|
|
//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");
|
|
}
|