diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index a7eb18097..1270a8021 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/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_uIndexm_initPoseArgs.m_initialStateQdot[info.m_uIndex] = jointVelocity; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 4b686cec0..d2b2c8421 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/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); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 1ea576f65..94c73f5f2 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/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"); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 2de1acabd..76d09dfe7 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/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"); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 970967b40..012169251 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5205,6 +5205,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btAlignedObjectArray omega; btAlignedObjectArray linVel; + bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS)!=0); + if (computeForwardKinematics) + { + B3_PROFILE("compForwardKinematics"); + btAlignedObjectArray world_to_local; + btAlignedObjectArray 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) { diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 2186f3a78..90dbfe8b9 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/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 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" + + diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index aa6c4c63c..1c3758e47 100644 --- a/examples/pybullet/pybullet.c +++ b/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)