Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
yunfeibai
2017-09-27 15:20:11 -07:00
parent 113e103bc2 8e08a9ff16
commit 85be3b43dd
75 changed files with 6253 additions and 767 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -171,6 +171,7 @@ SET(BulletExampleBrowser_SRCS
../SharedMemory/PhysicsServerCommandProcessor.h
../SharedMemory/SharedMemoryCommands.h
../SharedMemory/SharedMemoryPublic.h
../SharedMemory/b3PluginManager.cpp
../RobotSimulator/b3RobotSimulatorClientAPI.cpp
../RobotSimulator/b3RobotSimulatorClientAPI.h
../BasicDemo/BasicExample.cpp

1
examples/ExampleBrowser/premake4.lua
View File

@@ -109,6 +109,7 @@ project "App_BulletExampleBrowser"
"../SharedMemory/PhysicsLoopBackC_API.h",
"../SharedMemory/PhysicsServerCommandProcessor.cpp",
"../SharedMemory/PhysicsServerCommandProcessor.h",
"../SharedMemory/b3PluginManager.cpp",
"../SharedMemory/TinyRendererVisualShapeConverter.cpp",
"../SharedMemory/TinyRendererVisualShapeConverter.h",
"../SharedMemory/SharedMemoryCommands.h",

8
examples/OpenGLWindow/GLInstancingRenderer.cpp
View File

@@ -971,10 +971,10 @@ int GLInstancingRenderer::registerTexture(const unsigned char* texels, int width
void GLInstancingRenderer::replaceTexture(int shapeIndex, int textureId)
{
if (shapeIndex >=0 && shapeIndex < m_data->m_textureHandles.size())
if ((shapeIndex >=0) && (shapeIndex < m_graphicsInstances.size()))
{
b3GraphicsInstance* gfxObj = m_graphicsInstances[shapeIndex];
if (textureId>=0)
if (textureId>=0 && textureId < m_data->m_textureHandles.size())
{
gfxObj->m_textureIndex = textureId;
gfxObj->m_flags |= eGfxHasTexture;
@@ -985,7 +985,7 @@ void GLInstancingRenderer::replaceTexture(int shapeIndex, int textureId)
void GLInstancingRenderer::updateTexture(int textureIndex, const unsigned char* texels, bool flipPixelsY)
{
if (textureIndex>=0)
if ((textureIndex>=0) && (textureIndex < m_data->m_textureHandles.size()))
{
glActiveTexture(GL_TEXTURE0);
b3Assert(glGetError() ==GL_NO_ERROR);
@@ -1027,7 +1027,7 @@ void GLInstancingRenderer::activateTexture(int textureIndex)
{
glActiveTexture(GL_TEXTURE0);
if (textureIndex>=0)
if (textureIndex>=0 && textureIndex < m_data->m_textureHandles.size())
{
glBindTexture(GL_TEXTURE_2D,m_data->m_textureHandles[textureIndex].m_glTexture);
} else

2
examples/RobotSimulator/CMakeLists.txt
View File

@@ -42,6 +42,8 @@ SET(RobotSimulator_SRCS
../../examples/SharedMemory/PhysicsDirectC_API.h
../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
../../examples/SharedMemory/PhysicsServerCommandProcessor.h
../../examples/SharedMemory/b3PluginManager.cpp
../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
../../examples/SharedMemory/PhysicsClientSharedMemory.h
../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp

3
examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp
View File

@@ -988,7 +988,8 @@ bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex,
if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED)
{
b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian);
int dofCount;
b3GetStatusJacobian(statusHandle, &dofCount, linearJacobian, angularJacobian);
return true;
}
return false;

2
examples/RobotSimulator/premake4.lua
View File

@@ -20,6 +20,8 @@ myfiles =
"../../examples/SharedMemory/PhysicsDirectC_API.h",
"../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp",
"../../examples/SharedMemory/PhysicsServerCommandProcessor.h",
"../../examples/SharedMemory/b3PluginManager.cpp",
"../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
"../../examples/SharedMemory/PhysicsClientSharedMemory.h",
"../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp",

2
examples/SharedMemory/CMakeLists.txt
View File

@@ -1,5 +1,4 @@
SET(SharedMemory_SRCS
IKTrajectoryHelper.cpp
IKTrajectoryHelper.h
@@ -41,6 +40,7 @@ SET(SharedMemory_SRCS
TinyRendererVisualShapeConverter.h
SharedMemoryCommands.h
SharedMemoryPublic.h
b3PluginManager.cpp
../TinyRenderer/geometry.cpp
../TinyRenderer/model.cpp
../TinyRenderer/tgaimage.cpp

687
examples/SharedMemory/PhysicsClientC_API.cpp
View File

File diff suppressed because it is too large Load Diff

525
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -10,6 +10,14 @@ B3_DECLARE_HANDLE(b3PhysicsClientHandle);
B3_DECLARE_HANDLE(b3SharedMemoryCommandHandle);
B3_DECLARE_HANDLE(b3SharedMemoryStatusHandle);
#ifdef _WIN32
#define B3_SHARED_API __declspec(dllexport)
#elif defined (__GNUC__)
#define B3_SHARED_API __attribute__((visibility("default")))
#else
#define B3_SHARED_API
#endif
///There are several connection methods, see following header files:
#include "PhysicsClientSharedMemory_C_API.h"
@@ -33,30 +41,42 @@ extern "C" {
///b3DisconnectSharedMemory will disconnect the client from the server and cleanup memory.
void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient);
///There can only be 1 outstanding command. Check if a command can be send.
int b3CanSubmitCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3CanSubmitCommand(b3PhysicsClientHandle physClient);
///blocking submit command and wait for status
b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle);
B3_SHARED_API b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle);
///In general it is better to use b3SubmitClientCommandAndWaitStatus. b3SubmitClientCommand is a non-blocking submit
///command, which requires checking for the status manually, using b3ProcessServerStatus. Also, before sending the
///next command, make sure to check if you can send a command using 'b3CanSubmitCommand'.
int b3SubmitClientCommand(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle);
B3_SHARED_API int b3SubmitClientCommand(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle);
///non-blocking check status
b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient);
/// Get the physics server return status type. See EnumSharedMemoryServerStatus in SharedMemoryPublic.h for error codes.
int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity);
///Plugin system, load and unload a plugin, execute a command
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateCustomCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3CustomCommandLoadPlugin(b3SharedMemoryCommandHandle commandHandle, const char* pluginPath);
B3_SHARED_API int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle statusHandle);
int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API void b3CustomCommandUnloadPlugin(b3SharedMemoryCommandHandle commandHandle, int pluginUniqueId);
B3_SHARED_API void b3CustomCommandExecutePluginCommand(b3SharedMemoryCommandHandle commandHandle, int pluginUniqueId, const char* textArguments);
B3_SHARED_API void b3CustomCommandExecuteAddIntArgument(b3SharedMemoryCommandHandle commandHandle, int intVal);
B3_SHARED_API void b3CustomCommandExecuteAddFloatArgument(b3SharedMemoryCommandHandle commandHandle, float floatVal);
int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
B3_SHARED_API int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity);
B3_SHARED_API int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* numDegreeOfFreedomQ,
int* numDegreeOfFreedomU,
@@ -66,315 +86,320 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
const double* jointReactionForces[]);
b3SharedMemoryCommandHandle b3RequestCollisionInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[3], double aabbMax[3]);
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestCollisionInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
B3_SHARED_API int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[/*3*/], double aabbMax[/*3*/]);
///If you re-connected to an existing server, or server changed otherwise, sync the body info and user constraints etc.
b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3InitRemoveBodyCommand(b3PhysicsClientHandle physClient, int bodyUniqueId);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveBodyCommand(b3PhysicsClientHandle physClient, int bodyUniqueId);
///return the total number of bodies in the simulation
int b3GetNumBodies(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3GetNumBodies(b3PhysicsClientHandle physClient);
/// return the body unique id, given the index in range [0 , b3GetNumBodies() )
int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex);
B3_SHARED_API int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex);
///given a body unique id, return the body information. See b3BodyInfo in SharedMemoryPublic.h
int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info);
B3_SHARED_API int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info);
///give a unique body index (after loading the body) return the number of joints.
int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
B3_SHARED_API int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId);
///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
///given a body unique id and link index, return the dynamics information. See b3DynamicsInfo in SharedMemoryPublic.h
int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info);
B3_SHARED_API int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info);
b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient);
int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass);
int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction);
int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);
int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);
int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution);
int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double linearDamping);
int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double angularDamping);
int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double contactStiffness, double contactDamping);
int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, int frictionAnchor);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass);
B3_SHARED_API int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction);
B3_SHARED_API int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);
B3_SHARED_API int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);
B3_SHARED_API int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution);
B3_SHARED_API int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double linearDamping);
B3_SHARED_API int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double angularDamping);
B3_SHARED_API int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double contactStiffness, double contactDamping);
B3_SHARED_API int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, int frictionAnchor);
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
///return a unique id for the user constraint, after successful creation, or -1 for an invalid constraint id
int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle);
///change parameters of an existing user constraint
b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[3]);
int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]);
int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce);
int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio);
int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
B3_SHARED_API int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[/*3*/]);
B3_SHARED_API int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[/*4*/]);
B3_SHARED_API int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce);
B3_SHARED_API int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio);
B3_SHARED_API int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink);
B3_SHARED_API int b3InitChangeUserConstraintSetRelativePositionTarget(b3SharedMemoryCommandHandle commandHandle, double relativePositionTarget);
B3_SHARED_API int b3InitChangeUserConstraintSetERP(b3SharedMemoryCommandHandle commandHandle, double erp);
b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
int b3GetNumUserConstraints(b3PhysicsClientHandle physClient);
int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info);
B3_SHARED_API int b3GetNumUserConstraints(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info);
/// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() )
int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex);
B3_SHARED_API int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex);
///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h
b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode);
///Get the pointers to the physics debug line information, after b3InitRequestDebugLinesCommand returns
///status CMD_DEBUG_LINES_COMPLETED
void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines);
B3_SHARED_API void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines);
///configure the 3D OpenGL debug visualizer (enable/disable GUI widgets, shadows, position camera etc)
b3SharedMemoryCommandHandle b3InitConfigureOpenGLVisualizer(b3PhysicsClientHandle physClient);
void b3ConfigureOpenGLVisualizerSetVisualizationFlags(b3SharedMemoryCommandHandle commandHandle, int flag, int enabled);
void b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, float cameraDistance, float cameraPitch, float cameraYaw, const float cameraTargetPosition[3]);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitConfigureOpenGLVisualizer(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3ConfigureOpenGLVisualizerSetVisualizationFlags(b3SharedMemoryCommandHandle commandHandle, int flag, int enabled);
B3_SHARED_API void b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, float cameraDistance, float cameraPitch, float cameraYaw, const float cameraTargetPosition[/*3*/]);
b3SharedMemoryCommandHandle b3InitRequestOpenGLVisualizerCameraCommand(b3PhysicsClientHandle physClient);
int b3GetStatusOpenGLVisualizerCamera(b3SharedMemoryStatusHandle statusHandle, struct b3OpenGLVisualizerCameraInfo* camera);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestOpenGLVisualizerCameraCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3GetStatusOpenGLVisualizerCamera(b3SharedMemoryStatusHandle statusHandle, struct b3OpenGLVisualizerCameraInfo* camera);
/// Add/remove user-specific debug lines and debug text messages
b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[3], double toXYZ[3], double colorRGB[3], double lineWidth, double lifeTime);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[/*3*/], double toXYZ[/*3*/], double colorRGB[/*3*/], double lineWidth, double lifeTime);
b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime);
void b3UserDebugTextSetOptionFlags(b3SharedMemoryCommandHandle commandHandle, int optionFlags);
void b3UserDebugTextSetOrientation(b3SharedMemoryCommandHandle commandHandle, double orientation[4]);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[/*3*/], double colorRGB[/*3*/], double textSize, double lifeTime);
B3_SHARED_API void b3UserDebugTextSetOptionFlags(b3SharedMemoryCommandHandle commandHandle, int optionFlags);
B3_SHARED_API void b3UserDebugTextSetOrientation(b3SharedMemoryCommandHandle commandHandle, double orientation[/*4*/]);
void b3UserDebugItemSetParentObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex);
B3_SHARED_API void b3UserDebugItemSetParentObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex);
b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle physClient, const char* txt, double rangeMin, double rangeMax, double startValue);
b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3PhysicsClientHandle physClient, int debugItemUniqueId);
int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle physClient, const char* txt, double rangeMin, double rangeMax, double startValue);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3PhysicsClientHandle physClient, int debugItemUniqueId);
B3_SHARED_API int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue);
b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId);
b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsClientHandle physClient);
void b3SetDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex, double objectColorRGB[3]);
void b3RemoveDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3SetDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex, double objectColorRGB[/*3*/]);
B3_SHARED_API void b3RemoveDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex);
///All debug items unique Ids are positive: a negative unique Id means failure.
int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle);
///request an image from a simulated camera, using a software renderer.
b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient);
void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]);
void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height );
void b3RequestCameraImageSetLightDirection(b3SharedMemoryCommandHandle commandHandle, const float lightDirection[3]);
void b3RequestCameraImageSetLightColor(b3SharedMemoryCommandHandle commandHandle, const float lightColor[3]);
void b3RequestCameraImageSetLightDistance(b3SharedMemoryCommandHandle commandHandle, float lightDistance);
void b3RequestCameraImageSetLightAmbientCoeff(b3SharedMemoryCommandHandle commandHandle, float lightAmbientCoeff);
void b3RequestCameraImageSetLightDiffuseCoeff(b3SharedMemoryCommandHandle commandHandle, float lightDiffuseCoeff);
void b3RequestCameraImageSetLightSpecularCoeff(b3SharedMemoryCommandHandle commandHandle, float lightSpecularCoeff);
void b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle commandHandle, int hasShadow);
void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer);
void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHandle, float viewMatrix[/*16*/], float projectionMatrix[/*16*/]);
B3_SHARED_API void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle commandHandle, int width, int height );
B3_SHARED_API void b3RequestCameraImageSetLightDirection(b3SharedMemoryCommandHandle commandHandle, const float lightDirection[/*3*/]);
B3_SHARED_API void b3RequestCameraImageSetLightColor(b3SharedMemoryCommandHandle commandHandle, const float lightColor[/*3*/]);
B3_SHARED_API void b3RequestCameraImageSetLightDistance(b3SharedMemoryCommandHandle commandHandle, float lightDistance);
B3_SHARED_API void b3RequestCameraImageSetLightAmbientCoeff(b3SharedMemoryCommandHandle commandHandle, float lightAmbientCoeff);
B3_SHARED_API void b3RequestCameraImageSetLightDiffuseCoeff(b3SharedMemoryCommandHandle commandHandle, float lightDiffuseCoeff);
B3_SHARED_API void b3RequestCameraImageSetLightSpecularCoeff(b3SharedMemoryCommandHandle commandHandle, float lightSpecularCoeff);
B3_SHARED_API void b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle commandHandle, int hasShadow);
B3_SHARED_API void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer);
B3_SHARED_API void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData);
///compute a view matrix, helper function for b3RequestCameraImageSetCameraMatrices
void b3ComputeViewMatrixFromPositions(const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3], float viewMatrix[16]);
void b3ComputeViewMatrixFromYawPitchRoll(const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis, float viewMatrix[16]);
void b3ComputePositionFromViewMatrix(const float viewMatrix[16], float cameraPosition[3], float cameraTargetPosition[3], float cameraUp[3]);
B3_SHARED_API void b3ComputeViewMatrixFromPositions(const float cameraPosition[/*3*/], const float cameraTargetPosition[/*3*/], const float cameraUp[/*3*/], float viewMatrix[/*16*/]);
B3_SHARED_API void b3ComputeViewMatrixFromYawPitchRoll(const float cameraTargetPosition[/*3*/], float distance, float yaw, float pitch, float roll, int upAxis, float viewMatrix[/*16*/]);
B3_SHARED_API void b3ComputePositionFromViewMatrix(const float viewMatrix[/*16*/], float cameraPosition[/*3*/], float cameraTargetPosition[/*3*/], float cameraUp[/*3*/]);
///compute a projection matrix, helper function for b3RequestCameraImageSetCameraMatrices
void b3ComputeProjectionMatrix(float left, float right, float bottom, float top, float nearVal, float farVal, float projectionMatrix[16]);
void b3ComputeProjectionMatrixFOV(float fov, float aspect, float nearVal, float farVal, float projectionMatrix[16]);
B3_SHARED_API void b3ComputeProjectionMatrix(float left, float right, float bottom, float top, float nearVal, float farVal, float projectionMatrix[/*16*/]);
B3_SHARED_API void b3ComputeProjectionMatrixFOV(float fov, float aspect, float nearVal, float farVal, float projectionMatrix[/*16*/]);
/* obsolete, please use b3ComputeViewProjectionMatrices */
void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle command, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]);
B3_SHARED_API void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, const float cameraPosition[/*3*/], const float cameraTargetPosition[/*3*/], const float cameraUp[/*3*/]);
/* obsolete, please use b3ComputeViewProjectionMatrices */
void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis);
B3_SHARED_API void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[/*3*/], float distance, float yaw, float pitch, float roll, int upAxis);
/* obsolete, please use b3ComputeViewProjectionMatrices */
void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle command, float left, float right, float bottom, float top, float nearVal, float farVal);
B3_SHARED_API void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float left, float right, float bottom, float top, float nearVal, float farVal);
/* obsolete, please use b3ComputeViewProjectionMatrices */
void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle command, float fov, float aspect, float nearVal, float farVal);
B3_SHARED_API void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float fov, float aspect, float nearVal, float farVal);
///request an contact point information
b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient);
void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
void b3SetContactFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA);
void b3SetContactFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
B3_SHARED_API void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
B3_SHARED_API void b3SetContactFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA);
B3_SHARED_API void b3SetContactFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
B3_SHARED_API void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData);
///compute the closest points between two bodies
b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient);
void b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA);
void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance);
void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
B3_SHARED_API void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA);
B3_SHARED_API void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
B3_SHARED_API void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
B3_SHARED_API void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance);
B3_SHARED_API void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
///get all the bodies that touch a given axis aligned bounding box specified in world space (min and max coordinates)
b3SharedMemoryCommandHandle b3InitAABBOverlapQuery(b3PhysicsClientHandle physClient, const double aabbMin[3],const double aabbMax[3]);
void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, struct b3AABBOverlapData* data);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitAABBOverlapQuery(b3PhysicsClientHandle physClient, const double aabbMin[/*3*/],const double aabbMax[/*3*/]);
B3_SHARED_API void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, struct b3AABBOverlapData* data);
//request visual shape information
b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA);
void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA);
B3_SHARED_API void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo);
b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename);
int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename);
B3_SHARED_API int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle);
b3SharedMemoryCommandHandle b3CreateChangeTextureCommandInit(b3PhysicsClientHandle physClient, int textureUniqueId, int width, int height, const char* rgbPixels);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateChangeTextureCommandInit(b3PhysicsClientHandle physClient, int textureUniqueId, int width, int height, const char* rgbPixels);
b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId);
void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, double rgbaColor[4]);
void b3UpdateVisualShapeSpecularColor(b3SharedMemoryCommandHandle commandHandle, double specularColor[3]);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId);
B3_SHARED_API void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, double rgbaColor[/*4*/]);
B3_SHARED_API void b3UpdateVisualShapeSpecularColor(b3SharedMemoryCommandHandle commandHandle, double specularColor[/*3*/]);
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);
int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
int b3PhysicsParamSetDefaultNonContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultNonContactERP);
int b3PhysicsParamSetDefaultFrictionERP(b3SharedMemoryCommandHandle commandHandle, double frictionERP);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);
B3_SHARED_API int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
B3_SHARED_API int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
B3_SHARED_API int b3PhysicsParamSetDefaultNonContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultNonContactERP);
B3_SHARED_API int b3PhysicsParamSetDefaultFrictionERP(b3SharedMemoryCommandHandle commandHandle, double frictionERP);
int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode);
B3_SHARED_API int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
B3_SHARED_API int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode);
int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse);
int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold);
int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold);
int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHandle, int maxNumCmdPer1ms);
int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching);
int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold);
B3_SHARED_API int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse);
B3_SHARED_API int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold);
B3_SHARED_API int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold);
B3_SHARED_API int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHandle, int maxNumCmdPer1ms);
B3_SHARED_API int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching);
B3_SHARED_API int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold);
//b3PhysicsParamSetInternalSimFlags is for internal/temporary/easter-egg/experimental demo purposes
//Use at own risk: magic things may or my not happen when calling this API
int b3PhysicsParamSetInternalSimFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
B3_SHARED_API int b3PhysicsParamSetInternalSimFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient);
///Load a robot from a URDF file. Status type will CMD_URDF_LOADING_COMPLETED.
///Access the robot from the unique body index, through b3GetStatusBodyIndex(statusHandle);
b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName);
int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
int b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling);
B3_SHARED_API int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
B3_SHARED_API int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
B3_SHARED_API int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
B3_SHARED_API int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
B3_SHARED_API int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
B3_SHARED_API int b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling);
b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
B3_SHARED_API b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,
double* jointForces);
b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian);
B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle,
int* dofCount,
double* linearJacobian,
double* angularJacobian);
///compute the joint positions to move the end effector to a desired target using inverse kinematics
b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3]);
void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4]);
void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff);
int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/]);
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double targetOrientation[/*4*/]);
B3_SHARED_API void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
B3_SHARED_API void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double targetOrientation[/*4*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff);
B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,
double* jointPositions);
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
B3_SHARED_API int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
B3_SHARED_API int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling);
b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
B3_SHARED_API b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
///Set joint motor control variables such as desired position/angle, desired velocity,
///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
b3SharedMemoryCommandHandle b3JointControlCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode);
B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode);
///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD
int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value);
int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
B3_SHARED_API int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value);
B3_SHARED_API int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
B3_SHARED_API int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
///Only use when controlMode is CONTROL_MODE_VELOCITY
int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */
int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
B3_SHARED_API int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */
B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
///Only use if when controlMode is CONTROL_MODE_TORQUE,
int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
B3_SHARED_API int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
///the creation of collision shapes and rigid bodies etc is likely going to change,
///but good to have a b3CreateBoxShapeCommandInit for now
b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient);
int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius);
int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3]);
int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,double radius, double height);
int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,double radius, double height);
int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, double planeNormal[3], double planeConstant);
int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char* fileName, double meshScale[3]);
void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius);
B3_SHARED_API int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[/*3*/]);
B3_SHARED_API int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,double radius, double height);
B3_SHARED_API int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,double radius, double height);
B3_SHARED_API int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, double planeNormal[/*3*/], double planeConstant);
B3_SHARED_API int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char* fileName, double meshScale[/*3*/]);
B3_SHARED_API void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags);
void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[3], double childOrientation[4]);
B3_SHARED_API void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[/*3*/], double childOrientation[/*4*/]);
int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient);
int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient);
int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[3], double baseOrientation[4] , double baseInertialFramePosition[3], double baseInertialFrameOrientation[4]);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[/*3*/], double baseOrientation[/*4*/] , double baseInertialFramePosition[/*3*/], double baseInertialFrameOrientation[/*4*/]);
int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandle, double linkMass, double linkCollisionShapeIndex,
B3_SHARED_API int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandle, double linkMass, double linkCollisionShapeIndex,
double linkVisualShapeIndex,
double linkPosition[3],
double linkOrientation[4],
double linkInertialFramePosition[3],
double linkInertialFrameOrientation[4],
double linkPosition[/*3*/],
double linkOrientation[/*4*/],
double linkInertialFramePosition[/*3*/],
double linkInertialFrameOrientation[/*4*/],
int linkParentIndex,
int linkJointType,
double linkJointAxis[3]);
double linkJointAxis[/*3*/]);
//useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet
void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle);
B3_SHARED_API void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle);
//int b3CreateMultiBodyAddLink(b3SharedMemoryCommandHandle commandHandle, int jointType, int parentLinkIndex, double linkMass, int linkCollisionShapeUnique, int linkVisualShapeUniqueId);
@@ -382,119 +407,121 @@ void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandH
///create a box of size (1,1,1) at world origin (0,0,0) at orientation quat (0,0,0,1)
///after that, you can optionally adjust the initial position, orientation and size
b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient);
int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ);
int b3CreateBoxCommandSetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandHandle, int collisionShapeType);
int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, double red,double green,double blue, double alpha);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
B3_SHARED_API int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
B3_SHARED_API int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ);
B3_SHARED_API int b3CreateBoxCommandSetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
B3_SHARED_API int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandHandle, int collisionShapeType);
B3_SHARED_API int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, double red,double green,double blue, double alpha);
///b3CreatePoseCommandInit will initialize (teleport) the pose of a body/robot. You can individually set the base position,
///base orientation and joint angles. This will set all velocities of base and joints to zero.
///This is not a robot control command using actuators/joint motors, but manual repositioning the robot.
b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[3]);
int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[3]);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
B3_SHARED_API int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
B3_SHARED_API int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
B3_SHARED_API int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[/*3*/]);
B3_SHARED_API int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[/*3*/]);
int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions);
int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition);
B3_SHARED_API int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions);
B3_SHARED_API int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition);
int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* jointVelocities);
int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointVelocity);
B3_SHARED_API int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* jointVelocities);
B3_SHARED_API int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointVelocity);
///We are currently not reading the sensor information from the URDF file, and programmatically assign sensors.
///This is rather inconsistent, to mix programmatical creation with loading from file.
b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
B3_SHARED_API int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable);
///b3CreateSensorEnableIMUForLink is not implemented yet.
///For now, if the IMU is located in the root link, use the root world transform to mimic an IMU.
int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable);
B3_SHARED_API int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable);
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);
int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryCommandHandle commandHandle, int computeLinkVelocity);
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);
int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);
int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *state);
b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
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);
B3_SHARED_API b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ);
b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
B3_SHARED_API b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY,
double rayToWorldZ);
b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle physClient, double rayFromWorldX,
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ);
b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient);
void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[/*3*/], const double rayToWorld[/*3*/]);
void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo);
B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo);
/// Apply external force at the body (or link) center of mass, in world space/Cartesian coordinates.
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient);
void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags);
void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags);
B3_SHARED_API b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[/*3*/], const double position[/*3*/], int flag);
B3_SHARED_API void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[/*3*/], int flag);
///experiments of robots interacting with non-rigid objects (such as btSoftBody)
b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient);
int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale);
int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale);
B3_SHARED_API int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
B3_SHARED_API int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin);
b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient);
void b3VREventsSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter);
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3VREventsSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter);
void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* vrEventsData);
B3_SHARED_API 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);
int b3SetVRCameraTrackingObjectFlag(b3SharedMemoryCommandHandle commandHandle, int flag);
B3_SHARED_API b3SharedMemoryCommandHandle b3SetVRCameraStateCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double rootPos[/*3*/]);
B3_SHARED_API int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[/*4*/]);
B3_SHARED_API int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId);
B3_SHARED_API int b3SetVRCameraTrackingObjectFlag(b3SharedMemoryCommandHandle commandHandle, int flag);
b3SharedMemoryCommandHandle b3RequestKeyboardEventsCommandInit(b3PhysicsClientHandle physClient);
void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3KeyboardEventsData* keyboardEventsData);
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestKeyboardEventsCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3KeyboardEventsData* keyboardEventsData);
b3SharedMemoryCommandHandle b3RequestMouseEventsCommandInit(b3PhysicsClientHandle physClient);
void b3GetMouseEventsData(b3PhysicsClientHandle physClient, struct b3MouseEventsData* mouseEventsData);
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestMouseEventsCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3GetMouseEventsData(b3PhysicsClientHandle physClient, struct b3MouseEventsData* mouseEventsData);
b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient);
int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName);
int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId);
int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof);
int b3StateLoggingSetLinkIndexA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA);
int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
int b3StateLoggingSetBodyAUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyAUniqueId);
int b3StateLoggingSetBodyBUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyBUniqueId);
int b3StateLoggingSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter);
int b3StateLoggingSetLogFlags(b3SharedMemoryCommandHandle commandHandle, int logFlags);
B3_SHARED_API b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName);
B3_SHARED_API int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId);
B3_SHARED_API int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof);
B3_SHARED_API int b3StateLoggingSetLinkIndexA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA);
B3_SHARED_API int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
B3_SHARED_API int b3StateLoggingSetBodyAUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyAUniqueId);
B3_SHARED_API int b3StateLoggingSetBodyBUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyBUniqueId);
B3_SHARED_API int b3StateLoggingSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter);
B3_SHARED_API int b3StateLoggingSetLogFlags(b3SharedMemoryCommandHandle commandHandle, int logFlags);
int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle);
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId);
B3_SHARED_API int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid);
b3SharedMemoryCommandHandle b3ProfileTimingCommandInit(b3PhysicsClientHandle physClient, const char* name);
void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle commandHandle, int duration);
B3_SHARED_API b3SharedMemoryCommandHandle b3ProfileTimingCommandInit(b3PhysicsClientHandle physClient, const char* name);
B3_SHARED_API void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle commandHandle, int duration);
void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds);
double b3GetTimeOut(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds);
B3_SHARED_API double b3GetTimeOut(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, char* path);
B3_SHARED_API b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, char* path);
void b3MultiplyTransforms(const double posA[3], const double ornA[4], const double posB[3], const double ornB[4], double outPos[3], double outOrn[4]);
void b3InvertTransform(const double pos[3], const double orn[4], double outPos[3], double outOrn[4]);
B3_SHARED_API void b3MultiplyTransforms(const double posA[/*3*/], const double ornA[/*4*/], const double posB[/*3*/], const double ornB[/*4*/], double outPos[/*3*/], double outOrn[/*4*/]);
B3_SHARED_API void b3InvertTransform(const double pos[/*3*/], const double orn[/*4*/], double outPos[/*3*/], double outOrn[/*4*/]);
#ifdef __cplusplus
}

20
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -78,7 +78,7 @@ struct PhysicsClientSharedMemoryInternalData {
m_hasLastServerStatus(false),
m_sharedMemoryKey(SHARED_MEMORY_KEY),
m_verboseOutput(false),
m_timeOutInSeconds(5)
m_timeOutInSeconds(30)
{}
void processServerStatus();
@@ -1200,6 +1200,24 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("Request getCollisionInfo failed");
break;
}
case CMD_CUSTOM_COMMAND_COMPLETED:
{
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");
break;
}
default: {
b3Error("Unknown server status %d\n", serverCmd.m_type);

2
examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp
View File

@@ -2,7 +2,7 @@
#include "PhysicsClientSharedMemory.h"
b3PhysicsClientHandle b3ConnectSharedMemory(int key)
B3_SHARED_API b3PhysicsClientHandle b3ConnectSharedMemory(int key)
{
PhysicsClientSharedMemory* cl = new PhysicsClientSharedMemory();
cl->setSharedMemoryKey(key);

2
examples/SharedMemory/PhysicsClientSharedMemory_C_API.h
View File

@@ -7,7 +7,7 @@
extern "C" {
#endif
b3PhysicsClientHandle b3ConnectSharedMemory(int key);
B3_SHARED_API b3PhysicsClientHandle b3ConnectSharedMemory(int key);
#ifdef __cplusplus
}

2
examples/SharedMemory/PhysicsClientTCP_C_API.cpp
View File

@@ -4,7 +4,7 @@
#include "PhysicsDirect.h"
#include <stdio.h>
b3PhysicsClientHandle b3ConnectPhysicsTCP(const char* hostName, int port)
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsTCP(const char* hostName, int port)
{
TcpNetworkedPhysicsProcessor* tcp = new TcpNetworkedPhysicsProcessor(hostName, port);

2
examples/SharedMemory/PhysicsClientTCP_C_API.h
View File

@@ -9,7 +9,7 @@ extern "C" {
///send physics commands using TCP networking
b3PhysicsClientHandle b3ConnectPhysicsTCP(const char* hostName, int port);
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsTCP(const char* hostName, int port);
#ifdef __cplusplus

2
examples/SharedMemory/PhysicsClientUDP_C_API.cpp
View File

@@ -5,7 +5,7 @@
#include <stdio.h>
//think more about naming. The b3ConnectPhysicsLoopback
b3PhysicsClientHandle b3ConnectPhysicsUDP(const char* hostName, int port)
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsUDP(const char* hostName, int port)
{
UdpNetworkedPhysicsProcessor* udp = new UdpNetworkedPhysicsProcessor(hostName, port);

2
examples/SharedMemory/PhysicsClientUDP_C_API.h
View File

@@ -9,7 +9,7 @@ extern "C" {
///send physics commands using UDP networking
b3PhysicsClientHandle b3ConnectPhysicsUDP(const char* hostName, int port);
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsUDP(const char* hostName, int port);
#ifdef __cplusplus

34
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -940,6 +940,40 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
break;
}
case CMD_CUSTOM_COMMAND_COMPLETED:
{
break;
}
case CMD_CUSTOM_COMMAND_FAILED:
{
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");

2
examples/SharedMemory/PhysicsDirectC_API.cpp
View File

@@ -7,7 +7,7 @@
//think more about naming. The b3ConnectPhysicsLoopback
b3PhysicsClientHandle b3ConnectPhysicsDirect()
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDirect()
{
PhysicsServerCommandProcessor* sdk = new PhysicsServerCommandProcessor;

2
examples/SharedMemory/PhysicsDirectC_API.h
View File

@@ -9,7 +9,7 @@ extern "C" {
///think more about naming. Directly execute commands without transport (no shared memory, UDP, socket, grpc etc)
b3PhysicsClientHandle b3ConnectPhysicsDirect();
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDirect();
#ifdef __cplusplus

208
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1,5 +1,6 @@
#include "PhysicsServerCommandProcessor.h"
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
@@ -37,6 +38,8 @@
#include "LinearMath/btRandom.h"
#include "Bullet3Common/b3ResizablePool.h"
#include "../Utils/b3Clock.h"
#include "b3PluginManager.h"
#ifdef B3_ENABLE_TINY_AUDIO
#include "../TinyAudio/b3SoundEngine.h"
#endif
@@ -1053,13 +1056,13 @@ struct GenericRobotStateLogger : public InternalStateLogger
std::string m_fileName;
FILE* m_logFileHandle;
std::string m_structTypes;
btMultiBodyDynamicsWorld* m_dynamicsWorld;
const btMultiBodyDynamicsWorld* m_dynamicsWorld;
btAlignedObjectArray<int> m_bodyIdList;
bool m_filterObjectUniqueId;
int m_maxLogDof;
int m_logFlags;
GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld, int maxLogDof, int logFlags)
GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, const btMultiBodyDynamicsWorld* dynamicsWorld, int maxLogDof, int logFlags)
:m_loggingTimeStamp(0),
m_logFileHandle(0),
m_dynamicsWorld(dynamicsWorld),
@@ -1137,7 +1140,7 @@ struct GenericRobotStateLogger : public InternalStateLogger
{
for (int i=0;i<m_dynamicsWorld->getNumMultibodies();i++)
{
btMultiBody* mb = m_dynamicsWorld->getMultiBody(i);
const btMultiBody* mb = m_dynamicsWorld->getMultiBody(i);
int objectUniqueId = mb->getUserIndex2();
if (m_filterObjectUniqueId && m_bodyIdList.findLinearSearch2(objectUniqueId) < 0)
{
@@ -1430,6 +1433,8 @@ struct PhysicsServerCommandProcessorInternalData
b3ResizablePool< InternalBodyHandle > m_bodyHandles;
b3ResizablePool<InternalCollisionShapeHandle> m_userCollisionShapeHandles;
b3PluginManager m_pluginManager;
bool m_allowRealTimeSimulation;
@@ -1484,7 +1489,7 @@ struct PhysicsServerCommandProcessorInternalData
btAlignedObjectArray<int> m_sdfRecentLoadedBodies;
btAlignedObjectArray<InternalStateLogger*> m_stateLoggers;
int m_stateLoggersUniqueId;
int m_profileTimingLoggingUid;
@@ -1512,8 +1517,8 @@ struct PhysicsServerCommandProcessorInternalData
b3HashMap<b3HashString, char*> m_profileEvents;
PhysicsServerCommandProcessorInternalData()
:
PhysicsServerCommandProcessorInternalData(PhysicsCommandProcessorInterface* proc)
:m_pluginManager(proc),
m_allowRealTimeSimulation(false),
m_commandLogger(0),
m_logPlayback(0),
@@ -1537,6 +1542,16 @@ struct PhysicsServerCommandProcessorInternalData
m_pickedConstraint(0),
m_pickingMultiBodyPoint2Point(0)
{
{
//test to statically link a plugin
//#include "plugins/testPlugin/testplugin.h"
//register static plugins:
//m_pluginManager.registerStaticLinkedPlugin("path", initPlugin, exitPlugin, executePluginCommand);
}
m_vrControllerEvents.init();
m_bodyHandles.exitHandles();
@@ -1643,8 +1658,9 @@ void PhysicsServerCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiH
PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
:m_data(0)
{
m_data = new PhysicsServerCommandProcessorInternalData();
m_data = new PhysicsServerCommandProcessorInternalData(this);
createEmptyDynamicsWorld();
@@ -1666,13 +1682,23 @@ PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
delete m_data;
}
void preTickCallback(btDynamicsWorld *world, btScalar timeStep)
{
PhysicsServerCommandProcessor* proc = (PhysicsServerCommandProcessor*) world->getWorldUserInfo();
bool isPreTick = true;
proc->tickPlugins(timeStep, isPreTick);
}
void logCallback(btDynamicsWorld *world, btScalar timeStep)
{
//handle the logging and playing sounds
PhysicsServerCommandProcessor* proc = (PhysicsServerCommandProcessor*) world->getWorldUserInfo();
proc->processCollisionForces(timeStep);
proc->logObjectStates(timeStep);
bool isPreTick = false;
proc->tickPlugins(timeStep, isPreTick);
}
@@ -1781,6 +1807,12 @@ void PhysicsServerCommandProcessor::processCollisionForces(btScalar timeStep)
#endif//B3_ENABLE_TINY_AUDIO
}
void PhysicsServerCommandProcessor::tickPlugins(btScalar timeStep, bool isPreTick)
{
m_data->m_pluginManager.tickPlugins(timeStep, isPreTick);
}
void PhysicsServerCommandProcessor::logObjectStates(btScalar timeStep)
{
for (int i=0;i<m_data->m_stateLoggers.size();i++)
@@ -2144,7 +2176,11 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
{
m_data->m_guiHelper->createPhysicsDebugDrawer(m_data->m_dynamicsWorld);
}
m_data->m_dynamicsWorld->setInternalTickCallback(logCallback,this);
bool isPreTick=false;
m_data->m_dynamicsWorld->setInternalTickCallback(logCallback,this,isPreTick);
isPreTick = true;
m_data->m_dynamicsWorld->setInternalTickCallback(preTickCallback,this,isPreTick);
#ifdef B3_ENABLE_TINY_AUDIO
m_data->m_soundEngine.init(16,true);
@@ -2961,27 +2997,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
bool hasStatus = false;
{
///we ignore overflow of integer for now
{
//until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
//const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0];
#if 1
if (m_data->m_commandLogger)
{
m_data->m_commandLogger->logCommand(clientCmd);
}
#endif
//m_data->m_testBlock1->m_numProcessedClientCommands++;
//no timestamp yet
//int timeStamp = 0;
//catch uninitialized cases
serverStatusOut.m_type = CMD_INVALID_STATUS;
serverStatusOut.m_numDataStreamBytes = 0;
serverStatusOut.m_dataStream = 0;
@@ -2989,32 +3009,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
//consume the command
switch (clientCmd.m_type)
{
#if 0
case CMD_SEND_BULLET_DATA_STREAM:
{
if (m_data->m_verboseOutput)
{
b3Printf("Processed CMD_SEND_BULLET_DATA_STREAM length %d",clientCmd.m_dataStreamArguments.m_streamChunkLength);
}
btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld);
m_data->m_worldImporters.push_back(worldImporter);
bool completedOk = worldImporter->loadFileFromMemory(m_data->m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength);
if (completedOk)
{
SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
m_data->submitServerStatus(status);
} else
{
SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_FAILED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(status);
}
break;
}
#endif
case CMD_STATE_LOGGING:
{
BT_PROFILE("CMD_STATE_LOGGING");
@@ -5210,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)
{
@@ -6623,30 +6629,60 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (tree)
{
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
const int num_dofs = bodyHandle->m_multiBody->getNumDofs();
btInverseDynamics::vecx nu(num_dofs+baseDofs), qdot(num_dofs + baseDofs), q(num_dofs + baseDofs), joint_force(num_dofs + baseDofs);
for (int i = 0; i < num_dofs; i++)
const int numDofs = bodyHandle->m_multiBody->getNumDofs();
btInverseDynamics::vecx q(numDofs + baseDofs);
btInverseDynamics::vecx qdot(numDofs + baseDofs);
btInverseDynamics::vecx nu(numDofs + baseDofs);
btInverseDynamics::vecx joint_force(numDofs + baseDofs);
for (int i = 0; i < numDofs; i++)
{
q[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointPositionsQ[i];
qdot[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointVelocitiesQdot[i];
nu[i+baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointAccelerations[i];
nu[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointAccelerations[i];
}
// Set the gravity to correspond to the world gravity
btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
if (-1 != tree->setGravityInWorldFrame(id_grav) &&
-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{
serverCmd.m_jacobianResultArgs.m_dofCount = num_dofs;
serverCmd.m_jacobianResultArgs.m_dofCount = numDofs + baseDofs;
// Set jacobian value
tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3, num_dofs);
tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex+1, &jac_t);
btInverseDynamics::mat3x jac_t(3, numDofs + baseDofs);
btInverseDynamics::mat3x jac_r(3, numDofs + baseDofs);
// Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_t);
tree->getBodyJacobianRot(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_r);
// Update the translational jacobian based on the desired local point.
// v_pt = v_frame + w x pt
// v_pt = J_t * qd + (J_r * qd) x pt
// v_pt = J_t * qd - pt x (J_r * qd)
// v_pt = J_t * qd - pt_x * J_r * qd)
// v_pt = (J_t - pt_x * J_r) * qd
// J_t_new = J_t - pt_x * J_r
btInverseDynamics::vec3 localPosition;
for (int i = 0; i < 3; ++i) {
localPosition(i) = clientCmd.m_calculateJacobianArguments.m_localPosition[i];
}
// Only calculate if the localPosition is non-zero.
if (btInverseDynamics::maxAbs(localPosition) > 0.0) {
btInverseDynamics::mat33 skewCrossProduct;
btInverseDynamics::skew(localPosition, &skewCrossProduct);
btInverseDynamics::mat3x jac_l(3, numDofs + baseDofs);
btInverseDynamics::mul(skewCrossProduct, jac_r, &jac_l);
btInverseDynamics::mat3x jac_t_new(3, numDofs + baseDofs);
btInverseDynamics::sub(jac_t, jac_l, &jac_t_new);
jac_t = jac_t_new;
}
// Fill in the result into the shared memory.
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < num_dofs; ++j)
for (int j = 0; j < (numDofs + baseDofs); ++j)
{
serverCmd.m_jacobianResultArgs.m_linearJacobian[i*num_dofs+j] = jac_t(i,j);
int element = (numDofs + baseDofs) * i + j;
serverCmd.m_jacobianResultArgs.m_linearJacobian[element] = jac_t(i,j);
serverCmd.m_jacobianResultArgs.m_angularJacobian[element] = jac_r(i,j);
}
}
serverCmd.m_type = CMD_CALCULATED_JACOBIAN_COMPLETED;
@@ -7242,6 +7278,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
userConstraintPtr->m_mbConstraint->setGearRatio(clientCmd.m_userConstraintArguments.m_gearRatio);
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET)
{
userConstraintPtr->m_mbConstraint->setRelativePositionTarget(clientCmd.m_userConstraintArguments.m_relativePositionTarget);
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP)
{
userConstraintPtr->m_mbConstraint->setErp(clientCmd.m_userConstraintArguments.m_erp);
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK)
{
userConstraintPtr->m_mbConstraint->setGearAuxLink(clientCmd.m_userConstraintArguments.m_gearAuxLink);
@@ -7370,7 +7415,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3, numDofs);
btInverseDynamics::mat3x jac_r(3,numDofs);
// Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
// Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t);
tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r);
for (int i = 0; i < 3; ++i)
@@ -7968,6 +8013,39 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
break;
}
case CMD_CUSTOM_COMMAND:
{
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_CUSTOM_COMMAND_FAILED;
serverCmd.m_customCommandResultArgs.m_pluginUniqueId = -1;
if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN)
{
//pluginPath could be registered or load from disk
int pluginUniqueId = m_data->m_pluginManager.loadPlugin(clientCmd.m_customCommandArgs.m_pluginPath);
if (pluginUniqueId>=0)
{
serverCmd.m_customCommandResultArgs.m_pluginUniqueId = pluginUniqueId;
serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED;
}
}
if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN)
{
m_data->m_pluginManager.unloadPlugin(clientCmd.m_customCommandArgs.m_pluginUniqueId);
serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED;
}
if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND)
{
int result = m_data->m_pluginManager.executePluginCommand(clientCmd.m_customCommandArgs.m_pluginUniqueId, &clientCmd.m_customCommandArgs.m_arguments);
serverCmd.m_customCommandResultArgs.m_executeCommandResult = result;
serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED;
}
hasStatus = true;
break;
}
default:
{
BT_PROFILE("CMD_UNKNOWN");
@@ -8186,12 +8264,6 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
}
btVector3 gVRGripperPos(0.7, 0.3, 0.7);
btQuaternion gVRGripperOrn(0,0,0,1);
btVector3 gVRController2Pos(0,0,0.2);
btQuaternion gVRController2Orn(0,0,0,1);
btScalar gVRGripper2Analog = 0;
btScalar gVRGripperAnalog = 0;
@@ -8214,6 +8286,8 @@ bool PhysicsServerCommandProcessor::isRealTimeSimulationEnabled() const
void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents)
{
m_data->m_vrControllerEvents.addNewVREvents(vrControllerEvents,numVRControllerEvents);
for (int i=0;i<m_data->m_stateLoggers.size();i++)
{
if (m_data->m_stateLoggers[i]->m_loggingType==STATE_LOGGING_VR_CONTROLLERS)

1
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -91,6 +91,7 @@ public:
virtual void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
//logging of object states (position etc)
void tickPlugins(btScalar timeStep, bool isPreTick);
void logObjectStates(btScalar timeStep);
void processCollisionForces(btScalar timeStep);

26
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -109,6 +109,26 @@ struct b3SearchPathfArgs
char m_path[MAX_FILENAME_LENGTH];
};
enum CustomCommandEnum
{
CMD_CUSTOM_COMMAND_LOAD_PLUGIN=1,
CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN=2,
CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND=4,
};
struct b3CustomCommand
{
int m_pluginUniqueId;
b3PluginArguments m_arguments;
char m_pluginPath[MAX_FILENAME_LENGTH];
};
struct b3CustomCommandResultArgs
{
int m_pluginUniqueId;
int m_executeCommandResult;
};
struct BulletDataStreamArgs
{
@@ -658,7 +678,9 @@ enum EnumUserConstraintFlags
USER_CONSTRAINT_CHANGE_MAX_FORCE=32,
USER_CONSTRAINT_REQUEST_INFO=64,
USER_CONSTRAINT_CHANGE_GEAR_RATIO=128,
USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK=256,
USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK=256,
USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET=512,
USER_CONSTRAINT_CHANGE_ERP=1024,
};
@@ -968,6 +990,7 @@ struct SharedMemoryCommand
struct b3RequestCollisionInfoArgs m_requestCollisionInfoArgs;
struct b3ChangeTextureArgs m_changeTextureArgs;
struct b3SearchPathfArgs m_searchPathArgs;
struct b3CustomCommand m_customCommandArgs;
};
};
@@ -1039,6 +1062,7 @@ struct SharedMemoryStatus
struct b3SendCollisionInfoArgs m_sendCollisionInfoArgs;
struct SendMouseEvents m_sendMouseEvents;
struct b3LoadTextureResultArgs m_loadTextureResultArguments;
struct b3CustomCommandResultArgs m_customCommandResultArgs;
};
};

10
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
View File

@@ -83,7 +83,7 @@ public:
};
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[])
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[])
{
InProcessPhysicsClientSharedMemoryMainThread* cl = new InProcessPhysicsClientSharedMemoryMainThread(argc, argv, 1);
cl->setSharedMemoryKey(SHARED_MEMORY_KEY+1);
@@ -91,7 +91,7 @@ b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int arg
return (b3PhysicsClientHandle ) cl;
}
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(int argc, char* argv[])
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(int argc, char* argv[])
{
InProcessPhysicsClientSharedMemoryMainThread* cl = new InProcessPhysicsClientSharedMemoryMainThread(argc, argv, 0);
cl->setSharedMemoryKey(SHARED_MEMORY_KEY+1);
@@ -133,7 +133,7 @@ public:
};
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[])
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[])
{
InProcessPhysicsClientSharedMemory* cl = new InProcessPhysicsClientSharedMemory(argc, argv, 1);
@@ -141,7 +141,7 @@ b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* a
cl->connect();
return (b3PhysicsClientHandle ) cl;
}
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectSharedMemory(int argc, char* argv[])
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectSharedMemory(int argc, char* argv[])
{
InProcessPhysicsClientSharedMemory* cl = new InProcessPhysicsClientSharedMemory(argc, argv, 0);
@@ -248,7 +248,7 @@ int b3InProcessMouseButtonCallback(b3PhysicsClientHandle clientHandle, int butto
}
b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(void* guiHelperPtr)
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(void* guiHelperPtr)
{
GUIHelperInterface* guiHelper = (GUIHelperInterface*) guiHelperPtr;
InProcessPhysicsClientExistingExampleBrowser* cl = new InProcessPhysicsClientExistingExampleBrowser(guiHelper);

10
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h
View File

@@ -10,13 +10,13 @@ extern "C" {
///think more about naming. The b3ConnectPhysicsLoopback
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[]);
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectSharedMemory(int argc, char* argv[]);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[]);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectSharedMemory(int argc, char* argv[]);
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]);
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(int argc, char* argv[]);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(int argc, char* argv[]);
b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(void* guiHelperPtr);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(void* guiHelperPtr);
///ignore the following APIs, they are for internal use for example browser
void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle);

25
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -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
@@ -72,6 +73,7 @@ enum EnumSharedMemoryClientCommand
CMD_REQUEST_MOUSE_EVENTS_DATA,
CMD_CHANGE_TEXTURE,
CMD_SET_ADDITIONAL_SEARCH_PATH,
CMD_CUSTOM_COMMAND,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,
@@ -167,6 +169,9 @@ enum EnumSharedMemoryServerStatus
CMD_REQUEST_COLLISION_INFO_FAILED,
CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED,
CMD_CHANGE_TEXTURE_COMMAND_FAILED,
CMD_CUSTOM_COMMAND_COMPLETED,
CMD_CUSTOM_COMMAND_FAILED,
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
CMD_MAX_SERVER_COMMANDS
};
@@ -242,6 +247,8 @@ struct b3UserConstraint
int m_userConstraintUniqueId;
double m_gearRatio;
int m_gearAuxLink;
double m_relativePositionTarget;
double m_erp;
};
@@ -501,7 +508,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
@@ -616,6 +624,19 @@ enum eStateLoggingFlags
STATE_LOG_JOINT_TORQUES = STATE_LOG_JOINT_MOTOR_TORQUES+STATE_LOG_JOINT_USER_TORQUES,
};
#define B3_MAX_PLUGIN_ARG_SIZE 128
#define B3_MAX_PLUGIN_ARG_TEXT_LEN 1024
struct b3PluginArguments
{
char m_text[B3_MAX_PLUGIN_ARG_TEXT_LEN];
int m_numInts;
int m_ints[B3_MAX_PLUGIN_ARG_SIZE];
int m_numFloats;
int m_floats[B3_MAX_PLUGIN_ARG_SIZE];
};
#endif//SHARED_MEMORY_PUBLIC_H

255
examples/SharedMemory/b3PluginManager.cpp Normal file
View File

@@ -0,0 +1,255 @@
#include "b3PluginManager.h"
#include "Bullet3Common/b3HashMap.h"
#include "Bullet3Common/b3ResizablePool.h"
#include "PhysicsClientC_API.h"
#include "PhysicsDirect.h"
#include "plugins/b3PluginContext.h"
#ifdef _WIN32
#define WIN32_LEAN_AND_MEAN
#define VC_EXTRALEAN
#include <windows.h>
typedef HMODULE B3_DYNLIB_HANDLE;
#define B3_DYNLIB_OPEN LoadLibrary
#define B3_DYNLIB_CLOSE FreeLibrary
#define B3_DYNLIB_IMPORT GetProcAddress
#else
#include <dlfcn.h>
typedef void* B3_DYNLIB_HANDLE;
#define B3_DYNLIB_OPEN(path) dlopen(path, RTLD_NOW | RTLD_GLOBAL)
#define B3_DYNLIB_CLOSE dlclose
#define B3_DYNLIB_IMPORT dlsym
#endif
struct b3Plugin
{
B3_DYNLIB_HANDLE m_pluginHandle;
bool m_ownsPluginHandle;
std::string m_pluginPath;
int m_pluginUniqueId;
PFN_INIT m_initFunc;
PFN_EXIT m_exitFunc;
PFN_EXECUTE m_executeCommandFunc;
PFN_TICK m_preTickFunc;
PFN_TICK m_postTickFunc;
void* m_userPointer;
b3Plugin()
:m_pluginHandle(0),
m_ownsPluginHandle(false),
m_pluginUniqueId(-1),
m_initFunc(0),
m_exitFunc(0),
m_executeCommandFunc(0),
m_preTickFunc(0),
m_postTickFunc(0),
m_userPointer(0)
{
}
void clear()
{
if (m_ownsPluginHandle)
{
B3_DYNLIB_CLOSE(m_pluginHandle);
}
m_pluginHandle = 0;
m_initFunc = 0;
m_exitFunc = 0;
m_executeCommandFunc = 0;
m_preTickFunc = 0;
m_postTickFunc = 0;
m_userPointer = 0;
}
};
typedef b3PoolBodyHandle<b3Plugin> b3PluginHandle;
struct b3PluginManagerInternalData
{
b3ResizablePool<b3PluginHandle> m_plugins;
b3HashMap<b3HashString, b3PluginHandle> m_pluginMap;
PhysicsDirect* m_physicsDirect;
};
b3PluginManager::b3PluginManager(class PhysicsCommandProcessorInterface* physSdk)
{
m_data = new b3PluginManagerInternalData;
m_data->m_physicsDirect = new PhysicsDirect(physSdk,false);
}
b3PluginManager::~b3PluginManager()
{
while (m_data->m_pluginMap.size())
{
b3PluginHandle* plugin = m_data->m_pluginMap.getAtIndex(0);
unloadPlugin(plugin->m_pluginUniqueId);
}
delete m_data->m_physicsDirect;
m_data->m_pluginMap.clear();
m_data->m_plugins.exitHandles();
delete m_data;
}
int b3PluginManager::loadPlugin(const char* pluginPath)
{
int pluginUniqueId = -1;
b3Plugin* pluginOrg = m_data->m_pluginMap.find(pluginPath);
if (pluginOrg)
{
//already loaded
pluginUniqueId = pluginOrg->m_pluginUniqueId;
}
else
{
pluginUniqueId = m_data->m_plugins.allocHandle();
b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId);
plugin->m_pluginUniqueId = pluginUniqueId;
B3_DYNLIB_HANDLE pluginHandle = B3_DYNLIB_OPEN(pluginPath);
bool ok = false;
if (pluginHandle)
{
plugin->m_initFunc = (PFN_INIT)B3_DYNLIB_IMPORT(pluginHandle, "initPlugin");
plugin->m_exitFunc = (PFN_EXIT)B3_DYNLIB_IMPORT(pluginHandle, "exitPlugin");
plugin->m_executeCommandFunc = (PFN_EXECUTE)B3_DYNLIB_IMPORT(pluginHandle, "executePluginCommand");
plugin->m_preTickFunc = (PFN_TICK)B3_DYNLIB_IMPORT(pluginHandle, "preTickPluginCallback");
plugin->m_postTickFunc = (PFN_TICK)B3_DYNLIB_IMPORT(pluginHandle, "postTickPluginCallback");
if (plugin->m_initFunc && plugin->m_exitFunc && plugin->m_executeCommandFunc)
{
b3PluginContext context;
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
int version = plugin->m_initFunc(&context);
//keep the user pointer persistent
plugin->m_userPointer = context.m_userPointer;
if (version == SHARED_MEMORY_MAGIC_NUMBER)
{
ok = true;
plugin->m_ownsPluginHandle = true;
plugin->m_pluginHandle = pluginHandle;
plugin->m_pluginPath = pluginPath;
m_data->m_pluginMap.insert(pluginPath, *plugin);
}
else
{
int expect = SHARED_MEMORY_MAGIC_NUMBER;
b3Warning("Warning: plugin is wrong version: expected %d, got %d\n", expect, version);
}
}
else
{
b3Warning("Loaded plugin but couldn't bind functions");
}
if (!ok)
{
B3_DYNLIB_CLOSE(pluginHandle);
}
}
else
{
b3Warning("Warning: couldn't load plugin %s\n", pluginPath);
}
if (!ok)
{
m_data->m_plugins.freeHandle(pluginUniqueId);
pluginUniqueId = -1;
}
}
return pluginUniqueId;
}
void b3PluginManager::unloadPlugin(int pluginUniqueId)
{
b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId);
if (plugin)
{
b3PluginContext context;
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
plugin->m_exitFunc(&context);
m_data->m_pluginMap.remove(plugin->m_pluginPath.c_str());
m_data->m_plugins.freeHandle(pluginUniqueId);
}
}
void b3PluginManager::tickPlugins(double timeStep, bool isPreTick)
{
for (int i=0;i<m_data->m_pluginMap.size();i++)
{
b3PluginHandle* plugin = m_data->m_pluginMap.getAtIndex(i);
PFN_TICK tick = isPreTick? plugin->m_preTickFunc : plugin->m_postTickFunc;
if (tick)
{
b3PluginContext context;
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
int result = tick(&context);
plugin->m_userPointer = context.m_userPointer;
}
}
}
int b3PluginManager::executePluginCommand(int pluginUniqueId, const b3PluginArguments* arguments)
{
int result = -1;
b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId);
if (plugin)
{
b3PluginContext context;
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
result = plugin->m_executeCommandFunc(&context, arguments);
plugin->m_userPointer = context.m_userPointer;
}
return result;
}
int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc,PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc)
{
b3Plugin orgPlugin;
int pluginUniqueId = m_data->m_plugins.allocHandle();
b3PluginHandle* pluginHandle = m_data->m_plugins.getHandle(pluginUniqueId);
pluginHandle->m_pluginHandle = 0;
pluginHandle->m_ownsPluginHandle =false;
pluginHandle->m_pluginUniqueId = pluginUniqueId;
pluginHandle->m_executeCommandFunc = executeCommandFunc;
pluginHandle->m_exitFunc = exitFunc;
pluginHandle->m_initFunc = initFunc;
pluginHandle->m_preTickFunc = preTickFunc;
pluginHandle->m_postTickFunc = postTickFunc;
pluginHandle->m_pluginHandle = 0;
pluginHandle->m_pluginPath = pluginPath;
pluginHandle->m_userPointer = 0;
m_data->m_pluginMap.insert(pluginPath, *pluginHandle);
{
b3PluginContext context;
context.m_userPointer = 0;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
int result = pluginHandle->m_initFunc(&context);
pluginHandle->m_userPointer = context.m_userPointer;
}
return pluginUniqueId;
}

23
examples/SharedMemory/b3PluginManager.h Normal file
View File

@@ -0,0 +1,23 @@
#ifndef B3_PLUGIN_MANAGER_H
#define B3_PLUGIN_MANAGER_H
#include "plugins/b3PluginAPI.h"
class b3PluginManager
{
struct b3PluginManagerInternalData* m_data;
public:
b3PluginManager(class PhysicsCommandProcessorInterface* physSdk);
virtual ~b3PluginManager();
int loadPlugin(const char* pluginPath);
void unloadPlugin(int pluginUniqueId);
int executePluginCommand(int pluginUniqueId, const struct b3PluginArguments* arguments);
void tickPlugins(double timeStep, bool isPreTick);
int registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc,PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc);
};
#endif //B3_PLUGIN_MANAGER_H

37
examples/SharedMemory/plugins/b3PluginAPI.h Normal file
View File

@@ -0,0 +1,37 @@
#ifndef B3_PLUGIN_API_H
#define B3_PLUGIN_API_H
#ifdef _WIN32
#define B3_SHARED_API __declspec(dllexport)
#elif defined (__GNUC__)
#define B3_SHARED_API __attribute__((visibility("default")))
#else
#define B3_SHARED_API
#endif
#if defined(_WIN32)
#define B3_API_ENTRY
#define B3_API_CALL __cdecl
#define B3_CALLBACK __cdecl
#else
#define B3_API_ENTRY
#define B3_API_CALL
#define B3_CALLBACK
#endif
#ifdef __cplusplus
extern "C" {
#endif
/* Plugin API */
typedef B3_API_ENTRY int (B3_API_CALL * PFN_INIT)(struct b3PluginContext* context);
typedef B3_API_ENTRY void (B3_API_CALL * PFN_EXIT)(struct b3PluginContext* context);
typedef B3_API_ENTRY int (B3_API_CALL * PFN_EXECUTE)(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
typedef B3_API_ENTRY int (B3_API_CALL * PFN_TICK)(struct b3PluginContext* context);
#ifdef __cplusplus
}
#endif
#endif //B3_PLUGIN_API_H

21
examples/SharedMemory/plugins/b3PluginContext.h Normal file
View File

@@ -0,0 +1,21 @@
#ifndef B3_PLUGIN_CONTEXT_H
#define B3_PLUGIN_CONTEXT_H
#include "../PhysicsClientC_API.h"
struct b3PluginContext
{
b3PhysicsClientHandle m_physClient;
//plugin can modify the m_userPointer to store persistent object pointer (class or struct instance etc)
void* m_userPointer;
};
#endif //B3_PLUGIN_CONTEXT_H

42
examples/SharedMemory/plugins/testPlugin/premake4.lua Normal file
View File

@@ -0,0 +1,42 @@
project ("pybullet_testplugin")
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 {
"testplugin.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",
}

150
examples/SharedMemory/plugins/testPlugin/testplugin.cpp Normal file
View File

@@ -0,0 +1,150 @@
//test plugin, can load a URDF file, example usage on a Windows machine:
/*
import pybullet as p
p.connect(p.GUI)
pluginUid = p.loadPlugin("E:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll")
commandUid = 0
argument = "plane.urdf"
p.executePluginCommand(pluginUid,commandUid,argument)
p.unloadPlugin(pluginUid)
*/
#include "testplugin.h"
#include "../../SharedMemoryPublic.h"
#include "../b3PluginContext.h"
#include <stdio.h>
struct MyClass
{
int m_testData;
MyClass()
:m_testData(42)
{
}
virtual ~MyClass()
{
}
};
B3_SHARED_API int initPlugin(struct b3PluginContext* context)
{
MyClass* obj = new MyClass();
context->m_userPointer = obj;
printf("hi!\n");
return SHARED_MEMORY_MAGIC_NUMBER;
}
B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context)
{
MyClass* obj = (MyClass* )context->m_userPointer;
{
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)
{
//this is only for a test, normally you wouldn't print to the console at each simulation substep!
printf("got %d VR controller events!\n", vrEvents.m_numControllerEvents);
}
}
}
{
b3KeyboardEventsData keyboardEventsData;
b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(context->m_physClient);
b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
b3GetKeyboardEventsData(context->m_physClient, &keyboardEventsData);
if (keyboardEventsData.m_numKeyboardEvents)
{
//this is only for a test, normally you wouldn't print to the console at each simulation substep!
printf("got %d keyboard events\n", keyboardEventsData.m_numKeyboardEvents);
}
}
{
b3MouseEventsData mouseEventsData;
b3SharedMemoryCommandHandle commandHandle = b3RequestMouseEventsCommandInit(context->m_physClient);
b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
b3GetMouseEventsData(context->m_physClient, &mouseEventsData);
if (mouseEventsData.m_numMouseEvents)
{
//this is only for a test, normally you wouldn't print to the console at each simulation substep!
printf("got %d mouse events\n", mouseEventsData.m_numMouseEvents);
}
}
return 0;
}
B3_SHARED_API int postTickPluginCallback(struct b3PluginContext* context)
{
MyClass* obj = (MyClass* )context->m_userPointer;
obj->m_testData++;
return 0;
}
B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
printf("text argument:%s\n",arguments->m_text);
printf("int args: [");
for (int i=0;i<arguments->m_numInts;i++)
{
printf("%d", arguments->m_ints[i]);
if ((i+1)<arguments->m_numInts)
{
printf(",");
}
}
printf("]\nfloat args: [");
for (int i=0;i<arguments->m_numFloats;i++)
{
printf("%f", arguments->m_floats[i]);
if ((i+1)<arguments->m_numFloats)
{
printf(",");
}
}
printf("]\n");
MyClass* obj = (MyClass*) context->m_userPointer;
b3SharedMemoryStatusHandle statusHandle;
int statusType = -1;
int bodyUniqueId = -1;
b3SharedMemoryCommandHandle command =
b3LoadUrdfCommandInit(context->m_physClient, arguments->m_text);
statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, command);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_URDF_LOADING_COMPLETED)
{
bodyUniqueId = b3GetStatusBodyIndex(statusHandle);
}
return bodyUniqueId;
}
B3_SHARED_API void exitPlugin(struct b3PluginContext* context)
{
MyClass* obj = (MyClass*) context->m_userPointer;
delete obj;
context->m_userPointer = 0;
printf("bye!\n");
}

26
examples/SharedMemory/plugins/testPlugin/testplugin.h Normal file
View File

@@ -0,0 +1,26 @@
#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);
B3_SHARED_API int postTickPluginCallback(struct b3PluginContext* context);
#ifdef __cplusplus
};
#endif
#endif//#define TEST_PLUGIN_H

42
examples/SharedMemory/plugins/vrSyncPlugin/premake4.lua Normal file
View File

@@ -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",
}

209
examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp Normal file
View File

@@ -0,0 +1,209 @@
//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
#include "vrSyncPlugin.h"
#include "../../SharedMemoryPublic.h"
#include "../b3PluginContext.h"
#include <stdio.h>
struct MyClass
{
int m_testData;
int m_controllerId;
int m_constraintId;
int m_constraintId2;
int m_gripperId;
float m_maxForce;
float m_maxForce2;
MyClass()
:m_testData(42),
m_controllerId(-1),
m_constraintId(-1),
m_constraintId2(-1),
m_gripperId(-1),
m_maxForce(0),
m_maxForce2(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)
{
if (obj->m_constraintId>=0)
{
//this is basically equivalent to doing this in Python/pybullet:
//p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=...)
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);
}
// apply the analogue button to close the constraint, using a gear constraint with position target
if (obj->m_constraintId2>=0)
{
//this block is similar to
//p.changeConstraint(c,gearRatio=1, erp=..., relativePositionTarget=relPosTarget, maxForce=...)
//printf("obj->m_constraintId2=%d\n", obj->m_constraintId2);
b3SharedMemoryCommandHandle commandHandle;
commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, obj->m_constraintId2);
//0 -> open, 1 = closed
double openPos = 1.;
double relPosTarget = openPos - (event.m_analogAxis*openPos);
b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, relPosTarget);
b3InitChangeUserConstraintSetERP(commandHandle,1);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
}
//printf("event.m_analogAxis=%f\n", event.m_analogAxis);
// use the pr2_gripper motors to keep the gripper centered/symmetric around the center axis
if (obj->m_gripperId>=0)
{
//this block is similar to
//b = p.getJointState(pr2_gripper,2)[0]
//print("b = " + str(b))
//p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3)
//printf("obj->m_gripperId=%d\n", obj->m_gripperId);
{
b3SharedMemoryCommandHandle cmd_handle =
b3RequestActualStateCommandInit(context->m_physClient, obj->m_gripperId);
b3SharedMemoryStatusHandle status_handle =
b3SubmitClientCommandAndWaitStatus(context->m_physClient, cmd_handle);
int status_type = b3GetStatusType(status_handle);
if (status_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
//printf("status_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED\n");
b3JointSensorState sensorState;
if (b3GetJointState(context->m_physClient, status_handle, 2, &sensorState))
{
b3SharedMemoryCommandHandle commandHandle;
double targetPosition = sensorState.m_jointPosition;
//printf("targetPosition =%f\n", targetPosition);
if (1)
{
b3JointInfo info;
b3GetJointInfo(context->m_physClient, obj->m_gripperId, 0, &info);
commandHandle = b3JointControlCommandInit2(context->m_physClient, obj->m_gripperId, CONTROL_MODE_POSITION_VELOCITY_PD);
double kp = .1;
double targetVelocity = 0;
double kd = .6;
b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition);
b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex,targetVelocity);
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, obj->m_maxForce2);
b3SubmitClientCommandAndWaitStatus(context->m_physClient, cmd_handle);
}
} else
{
//printf("???\n");
}
} else
{
//printf("no\n");
}
}
}
}
}
}
}
}
return 0;
}
B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
MyClass* obj = (MyClass*) context->m_userPointer;
if (arguments->m_numInts>=4 && arguments->m_numFloats >= 2)
{
obj->m_constraintId = arguments->m_ints[1];
obj->m_constraintId2 = arguments->m_ints[2];
obj->m_gripperId = arguments->m_ints[3];
printf("obj->m_constraintId=%d\n", obj->m_constraintId);
obj->m_maxForce = arguments->m_floats[0];
obj->m_maxForce2 = arguments->m_floats[1];
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);
b3SharedMemoryCommandHandle command = b3InitSyncBodyInfoCommand(context->m_physClient);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, command);
int statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_SYNC_BODY_INFO_COMPLETED)
{
}
}
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");
}

25
examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.h Normal file
View File

@@ -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

8
examples/SharedMemory/premake4.lua
View File

@@ -7,7 +7,7 @@ else
kind "ConsoleApp"
end
includedirs {".","../../src", "../ThirdPartyLibs",}
includedirs {".","../../src", "../ThirdPartyLibs"}
links {
"Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "BussIK"
@@ -53,6 +53,8 @@ myfiles =
"SharedMemoryCommandProcessor.h",
"PhysicsServerCommandProcessor.cpp",
"PhysicsServerCommandProcessor.h",
"b3PluginManager.cpp",
"b3PluginManager.h",
"TinyRendererVisualShapeConverter.cpp",
"TinyRendererVisualShapeConverter.h",
"../TinyRenderer/geometry.cpp",
@@ -99,6 +101,7 @@ myfiles =
"../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../ThirdPartyLibs/stb_image/stb_image.cpp",
}
files {
@@ -405,4 +408,7 @@ end
include "udp"
include "tcp"
include "plugins/testPlugin"
include "plugins/vrSyncPlugin"

3
examples/SharedMemory/tcp/premake4.lua
View File

@@ -88,6 +88,9 @@ myfiles =
"../SharedMemoryPublic.h",
"../PhysicsServerCommandProcessor.cpp",
"../PhysicsServerCommandProcessor.h",
"../b3PluginManager.cpp",
"../PhysicsDirect.cpp",
"../PhysicsClient.cpp",
"../TinyRendererVisualShapeConverter.cpp",
"../TinyRendererVisualShapeConverter.h",
"../../TinyRenderer/geometry.cpp",

3
examples/SharedMemory/udp/premake4.lua
View File

@@ -79,6 +79,9 @@ myfiles =
"../SharedMemoryPublic.h",
"../PhysicsServerCommandProcessor.cpp",
"../PhysicsServerCommandProcessor.h",
"../b3PluginManager.cpp",
"../PhysicsDirect.cpp",
"../PhysicsClient.cpp",
"../TinyRendererVisualShapeConverter.cpp",
"../TinyRendererVisualShapeConverter.h",
"../../TinyRenderer/geometry.cpp",

3
examples/StandaloneMain/hellovr_opengl_main.cpp
View File

@@ -1808,6 +1808,8 @@ void CMainApplication::RenderStereoTargets()
m_app->m_instancingRenderer->getActiveCamera()->setCameraUpVector(mat[0],mat[1],mat[2]);
m_app->m_instancingRenderer->getActiveCamera()->setVRCamera(viewMatLeft.get(),m_mat4ProjectionLeft.get());
m_app->m_instancingRenderer->updateCamera(m_app->getUpAxis());
m_app->m_instancingRenderer->getActiveCamera()->setVRCamera(viewMatLeft.get(),m_mat4ProjectionLeft.get());
}
glBindFramebuffer( GL_FRAMEBUFFER, leftEyeDesc.m_nRenderFramebufferId );
@@ -1866,6 +1868,7 @@ void CMainApplication::RenderStereoTargets()
Matrix4 viewMatRight = m_mat4eyePosRight * m_mat4HMDPose * rotYtoZ;
m_app->m_instancingRenderer->getActiveCamera()->setVRCamera(viewMatRight.get(),m_mat4ProjectionRight.get());
m_app->m_instancingRenderer->updateCamera(m_app->getUpAxis());
m_app->m_instancingRenderer->getActiveCamera()->setVRCamera(viewMatRight.get(),m_mat4ProjectionRight.get());
}
glBindFramebuffer( GL_FRAMEBUFFER, rightEyeDesc.m_nRenderFramebufferId );

3
examples/pybullet/CMakeLists.txt
View File

@@ -43,6 +43,9 @@ SET(pybullet_SRCS
../../examples/SharedMemory/PhysicsDirectC_API.h
../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
../../examples/SharedMemory/PhysicsServerCommandProcessor.h
../../examples/SharedMemory/b3PluginManager.cpp
../../examples/SharedMemory/b3PluginManager.h
../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
../../examples/SharedMemory/PhysicsClientSharedMemory.h
../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp

3
examples/pybullet/examples/mimicJointConstraint.py
View File

@@ -23,6 +23,7 @@ p.changeConstraint(c,gearRatio=-1, maxForce=10000)
p.setRealTimeSimulation(1)
while(1):
p.setGravity(0,0,-10)
time.sleep(0.01)
#p.removeConstraint(c)

1
examples/pybullet/examples/quadruped.py
View File

@@ -187,6 +187,7 @@ t = 0.0
t_end = t + 15
ref_time = time.time()
while (t<t_end):
p.setGravity(0,0,-10)
if (useRealTime):
t = time.time()-ref_time
else:

10
examples/pybullet/examples/vr_kuka_setup.py
View File

@@ -1,6 +1,8 @@
import pybullet as p
import time
#p.connect(p.UDP,"192.168.86.100")
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
@@ -80,6 +82,12 @@ p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.setGravity(0.000000,0.000000,0.000000)
p.setGravity(0,0,-10)
p.stepSimulation()
##show this for 10 seconds
#now = time.time()
#while (time.time() < now+10):
# p.stepSimulation()
p.setRealTimeSimulation(1)
while (1):
p.setGravity(0,0,-10)
p.disconnect()

107
examples/pybullet/examples/vr_kuka_setup_vrSyncPlugin.py Normal file
View File

@@ -0,0 +1,107 @@
import pybullet as p
import time
#p.connect(p.UDP,"192.168.86.100")
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.resetSimulation()
#disable rendering during loading makes it much faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
#objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("pr2_gripper.urdf", 0.500000,0.300006,0.700000,-0.000000,-0.000000,-0.000031,1.000000)]
pr2_gripper = objects[0]
print ("pr2_gripper=")
print (pr2_gripper)
jointPositions=[ 0.550569, 0.000000, 0.549657, 0.000000 ]
for jointIndex in range (p.getNumJoints(pr2_gripper)):
p.resetJointState(pr2_gripper,jointIndex,jointPositions[jointIndex])
p.setJointMotorControl2(pr2_gripper,jointIndex,p.POSITION_CONTROL,targetPosition=0,force=0)
pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000])
print ("pr2_cid")
print (pr2_cid)
pr2_cid2 = p.createConstraint(pr2_gripper,0,pr2_gripper,2,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(pr2_cid2,gearRatio=1, erp=0.5, relativePositionTarget=0.5, maxForce=3)
objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)]
kuka = objects[0]
jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ]
for jointIndex in range (p.getNumJoints(kuka)):
p.resetJointState(kuka,jointIndex,jointPositions[jointIndex])
p.setJointMotorControl2(kuka,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0)
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.700000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.800000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.900000,0.000000,0.000000,0.000000,1.000000)]
objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf")
kuka_gripper = objects[0]
print ("kuka gripper=")
print(kuka_gripper)
p.resetBasePositionAndOrientation(kuka_gripper,[0.923103,-0.200000,1.250036],[-0.000000,0.964531,-0.000002,-0.263970])
jointPositions=[ 0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000 ]
for jointIndex in range (p.getNumJoints(kuka_gripper)):
p.resetJointState(kuka_gripper,jointIndex,jointPositions[jointIndex])
p.setJointMotorControl2(kuka_gripper,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0)
kuka_cid = p.createConstraint(kuka, 6, kuka_gripper,0,p.JOINT_FIXED, [0,0,0], [0,0,0.05],[0,0,0])
objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.000000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("teddy_vhacd.urdf", 1.050000,-0.500000,0.700000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("cube_small.urdf", 0.950000,-0.100000,0.700000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("sphere_small.urdf", 0.850000,-0.400000,0.700000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("duck_vhacd.urdf", 0.850000,-0.400000,0.900000,0.000000,0.000000,0.707107,0.707107)]
objects = p.loadSDF("kiva_shelf/model.sdf")
ob = objects[0]
p.resetBasePositionAndOrientation(ob,[0.000000,1.000000,1.204500],[0.000000,0.000000,0.000000,1.000000])
objects = [p.loadURDF("teddy_vhacd.urdf", -0.100000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("sphere_small.urdf", -0.100000,0.955006,1.169706,0.633232,-0.000000,-0.000000,0.773962)]
objects = [p.loadURDF("cube_small.urdf", 0.300000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("table_square/table_square.urdf", -1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
ob = objects[0]
jointPositions=[ 0.000000 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
objects = [p.loadURDF("husky/husky.urdf", 2.000000,-5.000000,1.000000,0.000000,0.000000,0.000000,1.000000)]
ob = objects[0]
jointPositions=[ 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.setGravity(0.000000,0.000000,0.000000)
p.setGravity(0,0,-10)
##show this for 10 seconds
#now = time.time()
#while (time.time() < now+10):
# p.stepSimulation()
p.setRealTimeSimulation(1)
plugin = p.loadPlugin("e:/develop/bullet3/bin/pybullet_vrSyncPlugin_vs2010_x64_release.dll")
controllerId = 3
p.executePluginCommand(plugin ,"bla", [controllerId,pr2_cid, pr2_cid2,pr2_gripper],[50,3])
while (1):
#b = p.getJointState(pr2_gripper,2)[0]
#print("b = " + str(b))
#p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3)
p.setGravity(0,0,-10)
p.disconnect()

2
examples/pybullet/gym/pybullet_envs/agents/__init__.py Normal file
View File

@@ -0,0 +1,2 @@

110
examples/pybullet/gym/pybullet_envs/agents/config_ppo.py Normal file
View File

@@ -0,0 +1,110 @@
"""The PPO training configuration file for minitaur environments."""
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import functools
from agents import ppo
from agents.scripts import networks
from pybullet_envs.bullet import minitaur_gym_env
from pybullet_envs.bullet import minitaur_env_randomizer
import pybullet_envs.bullet.minitaur_gym_env as minitaur_gym_env
import pybullet_envs
# pylint: disable=unused-variable
def default():
"""The default configurations."""
# General
algorithm = ppo.PPOAlgorithm
num_agents = 25
eval_episodes = 25
use_gpu = False
# Network
network = networks.ForwardGaussianPolicy
weight_summaries = dict(
all=r'.*', policy=r'.*/policy/.*', value=r'.*/value/.*')
policy_layers = 200, 100
value_layers = 200, 100
init_mean_factor = 0.2
init_logstd = -1
network_config = dict()
# Optimization
update_every = 25
policy_optimizer = 'AdamOptimizer'
value_optimizer = 'AdamOptimizer'
update_epochs_policy = 25
update_epochs_value = 25
value_lr = 1e-3
policy_lr = 1e-4
# Losses
discount = 0.99
kl_target = 1e-2
kl_cutoff_factor = 2
kl_cutoff_coef = 1000
kl_init_penalty = 1
return locals()
def pybullet_pendulum():
locals().update(default())
env = 'InvertedPendulumBulletEnv-v0'
max_length = 200
steps = 5e7 # 50M
return locals()
def pybullet_doublependulum():
locals().update(default())
env = 'InvertedDoublePendulumBulletEnv-v0'
max_length = 1000
steps = 5e7 # 50M
return locals()
def pybullet_pendulumswingup():
locals().update(default())
env = 'InvertedPendulumSwingupBulletEnv-v0'
max_length = 1000
steps = 5e7 # 50M
return locals()
def pybullet_cheetah():
"""Configuration for MuJoCo's half cheetah task."""
locals().update(default())
# Environment
env = 'HalfCheetahBulletEnv-v0'
max_length = 1000
steps = 1e8 # 100M
return locals()
def pybullet_ant():
locals().update(default())
env = 'AntBulletEnv-v0'
max_length = 1000
steps = 5e7 # 50M
return locals()
def pybullet_racecar():
"""Configuration for Bullet MIT Racecar task."""
locals().update(default())
# Environment
env = 'RacecarBulletEnv-v0' #functools.partial(racecarGymEnv.RacecarGymEnv, isDiscrete=False, renders=True)
max_length = 10
steps = 1e7 # 10M
return locals()
def pybullet_minitaur():
"""Configuration specific to minitaur_gym_env.MinitaurBulletEnv class."""
locals().update(default())
randomizer = (minitaur_env_randomizer.MinitaurEnvRandomizer())
env = functools.partial(
minitaur_gym_env.MinitaurBulletEnv,
accurate_motor_model_enabled=True,
motor_overheat_protection=True,
pd_control_enabled=True,
env_randomizer=randomizer,
render=False)
max_length = 1000
steps = 3e7 # 30M
return locals()

48
examples/pybullet/gym/pybullet_envs/agents/train_ppo.py Normal file
View File

@@ -0,0 +1,48 @@
r"""Script to use Proximal Policy Gradient for the minitaur environments.
Run:
python train_ppo.py --logdif=/tmp/train --config=minitaur_pybullet
"""
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import datetime
import os
import tensorflow as tf
from agents import tools
from agents.scripts import train
from agents.scripts import utility
from . import config_ppo
flags = tf.app.flags
FLAGS = tf.app.flags.FLAGS
flags.DEFINE_string(
'logdir', None,
'Base directory to store logs.')
flags.DEFINE_string(
'config', None,
'Configuration to execute.')
flags.DEFINE_string(
'timestamp', datetime.datetime.now().strftime('%Y%m%dT%H%M%S'),
'Sub directory to store logs.')
def main(_):
"""Create or load configuration and launch the trainer."""
config = tools.AttrDict(getattr(config_ppo, FLAGS.config)())
logdir = FLAGS.logdir and os.path.join(
FLAGS.logdir, '{}-{}'.format(FLAGS.timestamp, FLAGS.config))
utility.save_config(config, logdir)
for score in train.train(config, env_processes=True):
tf.logging.info(str(score))
if __name__ == '__main__':
tf.app.run()

42
examples/pybullet/gym/pybullet_envs/agents/visualize_ppo.py Normal file
View File

@@ -0,0 +1,42 @@
r"""Script to visualize the trained PPO agent.
python -m pybullet_envs.agents.visualize \
--logdir=ppo
--outdir=/tmp/video/
"""
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import tensorflow as tf
from agents.scripts import visualize
flags = tf.app.flags
FLAGS = tf.app.flags.FLAGS
flags.DEFINE_string("logdir", None,
"Directory to the checkpoint of a training run.")
flags.DEFINE_string("outdir", None,
"Local directory for storing the monitoring outdir.")
flags.DEFINE_string("checkpoint", None,
"Checkpoint name to load; defaults to most recent.")
flags.DEFINE_integer("num_agents", 1,
"How many environments to step in parallel.")
flags.DEFINE_integer("num_episodes", 1, "Minimum number of episodes to render.")
flags.DEFINE_boolean(
"env_processes", False,
"Step environments in separate processes to circumvent the GIL.")
def main(_):
visualize.visualize(FLAGS.logdir, FLAGS.outdir, FLAGS.num_agents,
FLAGS.num_episodes, FLAGS.checkpoint, FLAGS.env_processes)
if __name__ == "__main__":
tf.app.run()

25
examples/pybullet/gym/pybullet_envs/bullet/env_randomizer_base.py Normal file
View File

@@ -0,0 +1,25 @@
"""Abstract base class for environment randomizer."""
import abc
class EnvRandomizerBase(object):
"""Abstract base class for environment randomizer.
An EnvRandomizer is called in environment.reset(). It will
randomize physical parameters of the objects in the simulation.
The physical parameters will be fixed for that episode and be
randomized again in the next environment.reset().
"""
__metaclass__ = abc.ABCMeta
@abc.abstractmethod
def randomize_env(self, env):
"""Randomize the simulated_objects in the environment.
Args:
env: The environment to be randomized.
"""
pass

68
examples/pybullet/gym/pybullet_envs/bullet/minitaur_env_randomizer.py Normal file
View File

@@ -0,0 +1,68 @@
"""Randomize the minitaur_gym_env when reset() is called."""
import random
import numpy as np
from . import env_randomizer_base
# Relative range.
MINITAUR_BASE_MASS_ERROR_RANGE = (-0.2, 0.2) # 0.2 means 20%
MINITAUR_LEG_MASS_ERROR_RANGE = (-0.2, 0.2) # 0.2 means 20%
# Absolute range.
BATTERY_VOLTAGE_RANGE = (14.8, 16.8) # Unit: Volt
MOTOR_VISCOUS_DAMPING_RANGE = (0, 0.01) # Unit: N*m*s/rad (torque/angular vel)
MINITAUR_LEG_FRICTION = (0.8, 1.5) # Unit: dimensionless
class MinitaurEnvRandomizer(env_randomizer_base.EnvRandomizerBase):
"""A randomizer that change the minitaur_gym_env during every reset."""
def __init__(self,
minitaur_base_mass_err_range=MINITAUR_BASE_MASS_ERROR_RANGE,
minitaur_leg_mass_err_range=MINITAUR_LEG_MASS_ERROR_RANGE,
battery_voltage_range=BATTERY_VOLTAGE_RANGE,
motor_viscous_damping_range=MOTOR_VISCOUS_DAMPING_RANGE):
self._minitaur_base_mass_err_range = minitaur_base_mass_err_range
self._minitaur_leg_mass_err_range = minitaur_leg_mass_err_range
self._battery_voltage_range = battery_voltage_range
self._motor_viscous_damping_range = motor_viscous_damping_range
def randomize_env(self, env):
self._randomize_minitaur(env.minitaur)
def _randomize_minitaur(self, minitaur):
"""Randomize various physical properties of minitaur.
It randomizes the mass/inertia of the base, mass/inertia of the legs,
friction coefficient of the feet, the battery voltage and the motor damping
at each reset() of the environment.
Args:
minitaur: the Minitaur instance in minitaur_gym_env environment.
"""
base_mass = minitaur.GetBaseMassFromURDF()
randomized_base_mass = random.uniform(
base_mass * (1.0 + self._minitaur_base_mass_err_range[0]),
base_mass * (1.0 + self._minitaur_base_mass_err_range[1]))
minitaur.SetBaseMass(randomized_base_mass)
leg_masses = minitaur.GetLegMassesFromURDF()
leg_masses_lower_bound = np.array(leg_masses) * (
1.0 + self._minitaur_leg_mass_err_range[0])
leg_masses_upper_bound = np.array(leg_masses) * (
1.0 + self._minitaur_leg_mass_err_range[1])
randomized_leg_masses = [
np.random.uniform(leg_masses_lower_bound[i], leg_masses_upper_bound[i])
for i in range(len(leg_masses))
]
minitaur.SetLegMasses(randomized_leg_masses)
randomized_battery_voltage = random.uniform(BATTERY_VOLTAGE_RANGE[0],
BATTERY_VOLTAGE_RANGE[1])
minitaur.SetBatteryVoltage(randomized_battery_voltage)
randomized_motor_damping = random.uniform(MOTOR_VISCOUS_DAMPING_RANGE[0],
MOTOR_VISCOUS_DAMPING_RANGE[1])
minitaur.SetMotorViscousDamping(randomized_motor_damping)
randomized_foot_friction = random.uniform(MINITAUR_LEG_FRICTION[0],
MINITAUR_LEG_FRICTION[1])
minitaur.SetFootFriction(randomized_foot_friction)

3
examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py
View File

@@ -19,6 +19,7 @@ from . import bullet_client
from . import minitaur
import os
import pybullet_data
from . import minitaur_env_randomizer
NUM_SUBSTEPS = 5
NUM_MOTORS = 8
@@ -68,7 +69,7 @@ class MinitaurBulletEnv(gym.Env):
on_rack=False,
render=False,
kd_for_pd_controllers=0.3,
env_randomizer=None):
env_randomizer=minitaur_env_randomizer.MinitaurEnvRandomizer()):
"""Initialize the minitaur gym environment.
Args:

164
examples/pybullet/gym/pybullet_envs/examples/minitaur_gym_env_example.py Normal file
View File

@@ -0,0 +1,164 @@
r"""An example to run of the minitaur gym environment with sine gaits.
"""
import math
import numpy as np
from pybullet_envs.bullet import minitaur_gym_env
import argparse
from pybullet_envs.bullet import minitaur_env_randomizer
def ResetPoseExample():
"""An example that the minitaur stands still using the reset pose."""
steps = 1000
randomizer = (minitaur_env_randomizer.MinitaurEnvRandomizer())
environment = minitaur_gym_env.MinitaurBulletEnv(
render=True,
leg_model_enabled=False,
motor_velocity_limit=np.inf,
pd_control_enabled=True,
accurate_motor_model_enabled=True,
motor_overheat_protection=True,
env_randomizer = randomizer,
hard_reset=False)
action = [math.pi / 2] * 8
for _ in range(steps):
_, _, done, _ = environment.step(action)
if done:
break
environment.reset()
def MotorOverheatExample():
"""An example of minitaur motor overheat protection is triggered.
The minitaur is leaning forward and the motors are getting obove threshold
torques. The overheat protection will be triggered in ~1 sec.
"""
environment = minitaur_gym_env.MinitaurBulletEnv(
render=True,
leg_model_enabled=False,
motor_velocity_limit=np.inf,
motor_overheat_protection=True,
accurate_motor_model_enabled=True,
motor_kp=1.20,
motor_kd=0.00,
on_rack=False)
action = [2.0] * 8
for i in range(8):
action[i] = 2.0 - 0.5 * (-1 if i % 2 == 0 else 1) * (-1 if i < 4 else 1)
steps = 500
actions_and_observations = []
for step_counter in range(steps):
# Matches the internal timestep.
time_step = 0.01
t = step_counter * time_step
current_row = [t]
current_row.extend(action)
observation, _, _, _ = environment.step(action)
current_row.extend(observation.tolist())
actions_and_observations.append(current_row)
environment.reset()
def SineStandExample():
"""An example of minitaur standing and squatting on the floor.
To validate the accurate motor model we command the robot and sit and stand up
periodically in both simulation and experiment. We compare the measured motor
trajectories, torques and gains.
"""
environment = minitaur_gym_env.MinitaurBulletEnv(
render=True,
leg_model_enabled=False,
motor_velocity_limit=np.inf,
motor_overheat_protection=True,
accurate_motor_model_enabled=True,
motor_kp=1.20,
motor_kd=0.02,
on_rack=False)
steps = 1000
amplitude = 0.5
speed = 3
actions_and_observations = []
for step_counter in range(steps):
# Matches the internal timestep.
time_step = 0.01
t = step_counter * time_step
current_row = [t]
action = [math.sin(speed * t) * amplitude + math.pi / 2] * 8
current_row.extend(action)
observation, _, _, _ = environment.step(action)
current_row.extend(observation.tolist())
actions_and_observations.append(current_row)
environment.reset()
def SinePolicyExample():
"""An example of minitaur walking with a sine gait."""
randomizer = (minitaur_env_randomizer.MinitaurEnvRandomizer())
environment = minitaur_gym_env.MinitaurBulletEnv(
render=True,
motor_velocity_limit=np.inf,
pd_control_enabled=True,
hard_reset=False,
env_randomizer = randomizer,
on_rack=False)
sum_reward = 0
steps = 20000
amplitude_1_bound = 0.5
amplitude_2_bound = 0.5
speed = 40
for step_counter in range(steps):
time_step = 0.01
t = step_counter * time_step
amplitude1 = amplitude_1_bound
amplitude2 = amplitude_2_bound
steering_amplitude = 0
if t < 10:
steering_amplitude = 0.5
elif t < 20:
steering_amplitude = -0.5
else:
steering_amplitude = 0
# Applying asymmetrical sine gaits to different legs can steer the minitaur.
a1 = math.sin(t * speed) * (amplitude1 + steering_amplitude)
a2 = math.sin(t * speed + math.pi) * (amplitude1 - steering_amplitude)
a3 = math.sin(t * speed) * amplitude2
a4 = math.sin(t * speed + math.pi) * amplitude2
action = [a1, a2, a2, a1, a3, a4, a4, a3]
_, reward, done, _ = environment.step(action)
sum_reward += reward
if done:
break
environment.reset()
def main():
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('--env', help='environment ID (0==sine, 1==stand, 2=reset, 3=overheat)',type=int, default=0)
args = parser.parse_args()
print("--env=" + str(args.env))
if (args.env == 0):
SinePolicyExample()
if (args.env == 1):
SineStandExample()
if (args.env == 2):
ResetPoseExample()
if (args.env == 3):
MotorOverheatExample()
if __name__ == '__main__':
main()

2
examples/pybullet/premake4.lua
View File

@@ -116,6 +116,8 @@ if not _OPTIONS["no-enet"] then
"../../examples/SharedMemory/PhysicsDirectC_API.h",
"../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp",
"../../examples/SharedMemory/PhysicsServerCommandProcessor.h",
"../../examples/SharedMemory/b3PluginManager.cpp",
"../../examples/SharedMemory/b3PluginManager.h",
"../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
"../../examples/SharedMemory/PhysicsClientSharedMemory.h",
"../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp",

457
examples/pybullet/pybullet.c
View File

@@ -445,10 +445,10 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
{
printf("Connection terminated, couldn't get body info\n");
b3DisconnectSharedMemory(sm);
sm = 0;
sm = 0;
sPhysicsClients1[freeIndex] = 0;
sPhysicsClientsGUI[freeIndex] = 0;
sNumPhysicsClients++;
sPhysicsClientsGUI[freeIndex] = 0;
sNumPhysicsClients++;
return PyInt_FromLong(-1);
}
}
@@ -2835,7 +2835,7 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args, PyObject*
if (info.m_jointName)
{
PyTuple_SetItem(pyListJointInfo, 1,
PyString_FromString(info.m_jointName));
PyString_FromString(info.m_jointName));
} else
{
PyTuple_SetItem(pyListJointInfo, 1,
@@ -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,16 @@ 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 +3458,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 +3502,12 @@ 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);
}
{
PyObject* item = PyInt_FromLong(debugItemUniqueId);
return item;
}
PyErr_SetString(SpamError, "Error in addUserDebugLine.");
return NULL;
}
static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, PyObject* keywds)
@@ -4754,21 +4763,21 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args, PyObject*
static PyObject* MyConvertContactPoint(struct b3ContactInformation* contactPointPtr)
{
/*
0 int m_contactFlags;
1 int m_bodyUniqueIdA;
2 int m_bodyUniqueIdB;
3 int m_linkIndexA;
4 int m_linkIndexB;
5 double m_positionOnAInWS[3];//contact point location on object A,
in world space coordinates
6 double m_positionOnBInWS[3];//contact point location on object
A, in world space coordinates
7 double m_contactNormalOnBInWS[3];//the separating contact
normal, pointing from object B towards object A
8 double m_contactDistance;//negative number is penetration, positive
is distance.
9 double m_normalForce;
*/
0 int m_contactFlags;
1 int m_bodyUniqueIdA;
2 int m_bodyUniqueIdB;
3 int m_linkIndexA;
4 int m_linkIndexB;
5 double m_positionOnAInWS[3];//contact point location on object A,
in world space coordinates
6 double m_positionOnBInWS[3];//contact point location on object
A, in world space coordinates
7 double m_contactNormalOnBInWS[3];//the separating contact
normal, pointing from object B towards object A
8 double m_contactDistance;//negative number is penetration, positive
is distance.
9 double m_normalForce;
*/
int i;
@@ -4965,7 +4974,7 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, PyObject* keywds)
{
static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "gearRatio", "gearAuxLink", "physicsClientId", NULL};
static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "gearRatio", "gearAuxLink", "relativePositionTarget", "erp", "physicsClientId", NULL};
int userConstraintUniqueId = -1;
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
@@ -4979,7 +4988,9 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P
double jointChildFrameOrn[4];
double maxForce = -1;
double gearRatio = 0;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOddii", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &gearRatio, &gearAuxLink, &physicsClientId))
double relativePositionTarget=1e32;
double erp=-1;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOddiddi", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &gearRatio, &gearAuxLink, &relativePositionTarget, &erp, &physicsClientId))
{
return NULL;
}
@@ -5001,6 +5012,16 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P
{
b3InitChangeUserConstraintSetFrameInB(commandHandle, jointChildFrameOrn);
}
if (relativePositionTarget<1e10)
{
b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, relativePositionTarget);
}
if (erp>=0)
{
b3InitChangeUserConstraintSetERP(commandHandle, erp);
}
if (maxForce >= 0)
{
b3InitChangeUserConstraintSetMaxForce(commandHandle, maxForce);
@@ -5559,9 +5580,9 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, Py
{
int bodyUniqueIdA = -1;
int bodyUniqueIdB = -1;
int linkIndexA = -2;
int linkIndexB = -2;
int linkIndexA = -2;
int linkIndexB = -2;
b3SharedMemoryCommandHandle commandHandle;
struct b3ContactInformation contactPointData;
b3SharedMemoryStatusHandle statusHandle;
@@ -5583,24 +5604,24 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, Py
}
commandHandle = b3InitRequestContactPointInformation(sm);
if (bodyUniqueIdA>=0)
{
b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA);
}
if (bodyUniqueIdB>=0)
{
b3SetContactFilterBodyB(commandHandle, bodyUniqueIdB);
}
if (bodyUniqueIdA>=0)
{
b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA);
}
if (bodyUniqueIdB>=0)
{
b3SetContactFilterBodyB(commandHandle, bodyUniqueIdB);
}
if (linkIndexA>=-1)
{
b3SetContactFilterLinkA( commandHandle, linkIndexA);
}
if (linkIndexB >=-1)
{
b3SetContactFilterLinkB( commandHandle, linkIndexB);
}
if (linkIndexA>=-1)
{
b3SetContactFilterLinkA( commandHandle, linkIndexA);
}
if (linkIndexB >=-1)
{
b3SetContactFilterLinkB( commandHandle, linkIndexB);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
@@ -6574,7 +6595,7 @@ static PyObject* pybullet_invertTransform(PyObject* self,
PyErr_SetString(SpamError, "Invalid input: expected position [x,y,z] and orientation [x,y,z,w].");
return NULL;
}
/// quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo
/// https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc
@@ -6653,6 +6674,131 @@ static PyObject* pybullet_getEulerFromQuaternion(PyObject* self,
return Py_None;
}
static PyObject* pybullet_loadPlugin(PyObject* self,
PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
char* pluginPath = 0;
b3SharedMemoryCommandHandle command = 0;
b3SharedMemoryStatusHandle statusHandle = 0;
int statusType = -1;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = { "pluginPath", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &pluginPath, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
command = b3CreateCustomCommand(sm);
b3CustomCommandLoadPlugin(command, pluginPath);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusPluginUniqueId(statusHandle);
return PyInt_FromLong(statusType);
}
static PyObject* pybullet_unloadPlugin(PyObject* self,
PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
int pluginUniqueId = -1;
b3SharedMemoryCommandHandle command = 0;
b3SharedMemoryStatusHandle statusHandle = 0;
int statusType = -1;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = { "pluginUniqueId", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &pluginUniqueId,&physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
command = b3CreateCustomCommand(sm);
b3CustomCommandUnloadPlugin(command, pluginUniqueId);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
Py_INCREF(Py_None);
return Py_None;;
}
//createCustomCommand for executing commands implemented in a plugin system
static PyObject* pybullet_executePluginCommand(PyObject* self,
PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
int pluginUniqueId = -1;
char* textArgument = 0;
b3SharedMemoryCommandHandle command=0;
b3SharedMemoryStatusHandle statusHandle=0;
int statusType = -1;
PyObject* intArgs=0;
PyObject* floatArgs=0;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = { "pluginUniqueId", "textArgument", "intArgs", "floatArgs", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|sOOi", kwlist, &pluginUniqueId, &textArgument, &intArgs, &floatArgs, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
command = b3CreateCustomCommand(sm);
b3CustomCommandExecutePluginCommand(command, pluginUniqueId, textArgument);
{
PyObject* seqIntArgs = intArgs?PySequence_Fast(intArgs, "expected a sequence"):0;
PyObject* seqFloatArgs = floatArgs?PySequence_Fast(floatArgs, "expected a sequence"):0;
int numIntArgs = seqIntArgs?PySequence_Size(intArgs):0;
int numFloatArgs = seqIntArgs?PySequence_Size(floatArgs):0;
int i;
for (i=0;i<numIntArgs;i++)
{
int val = pybullet_internalGetIntFromSequence(seqIntArgs,i);
b3CustomCommandExecuteAddIntArgument(command, val);
}
for (i=0;i<numFloatArgs;i++)
{
float val = pybullet_internalGetFloatFromSequence(seqFloatArgs,i);
b3CustomCommandExecuteAddFloatArgument(command, val);
}
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusPluginCommandResult(statusHandle);
return PyInt_FromLong(statusType);
}
///Inverse Kinematics binding
static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
PyObject* args, PyObject* keywds)
@@ -6847,8 +6993,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
/// Given an object id, joint positions, joint velocities and joint
/// accelerations,
/// compute the joint forces using Inverse Dynamics
static PyObject* pybullet_calculateInverseDynamics(PyObject* self,
PyObject* args, PyObject* keywds)
static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* args, PyObject* keywds)
{
{
int bodyUniqueId;
@@ -6857,14 +7002,21 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self,
PyObject* objAccelerations;
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"bodyUniqueId", "objPositions", "objVelocities", "objAccelerations", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist, &bodyUniqueId, &objPositionsQ,
&objVelocitiesQdot, &objAccelerations, &physicsClientId))
static char* kwlist[] = {"bodyUniqueId", "objPositions",
"objVelocities", "objAccelerations",
"physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist,
&bodyUniqueId, &objPositionsQ,
&objVelocitiesQdot, &objAccelerations,
&physicsClientId))
{
static char* kwlist2[] = {"bodyIndex", "objPositions", "objVelocities", "objAccelerations", "physicsClientId", NULL};
static char* kwlist2[] = {"bodyIndex", "objPositions",
"objVelocities", "objAccelerations",
"physicsClientId", NULL};
PyErr_Clear();
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist2, &bodyUniqueId, &objPositionsQ,
&objVelocitiesQdot, &objAccelerations, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist2,
&bodyUniqueId, &objPositionsQ, &objVelocitiesQdot,
&objAccelerations, &physicsClientId))
{
return NULL;
}
@@ -6963,34 +7115,188 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self,
return Py_None;
}
/// Given an object id, joint positions, joint velocities and joint
/// accelerations, compute the Jacobian
static PyObject* pybullet_calculateJacobian(PyObject* self, PyObject* args, PyObject* keywds)
{
{
int bodyUniqueId;
int linkIndex;
PyObject* localPosition;
PyObject* objPositions;
PyObject* objVelocities;
PyObject* objAccelerations;
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "localPosition",
"objPositions", "objVelocities",
"objAccelerations", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiOOOO|i", kwlist,
&bodyUniqueId, &linkIndex, &localPosition, &objPositions,
&objVelocities, &objAccelerations, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
int szLoPos = PySequence_Size(localPosition);
int szObPos = PySequence_Size(objPositions);
int szObVel = PySequence_Size(objVelocities);
int szObAcc = PySequence_Size(objAccelerations);
int numJoints = b3GetNumJoints(sm, bodyUniqueId);
if (numJoints && (szLoPos == 3) && (szObPos == numJoints) &&
(szObVel == numJoints) && (szObAcc == numJoints))
{
int byteSizeJoints = sizeof(double) * numJoints;
int byteSizeVec3 = sizeof(double) * 3;
int i;
PyObject* pyResultList = PyTuple_New(2);
double* localPoint = (double*)malloc(byteSizeVec3);
double* jointPositions = (double*)malloc(byteSizeJoints);
double* jointVelocities = (double*)malloc(byteSizeJoints);
double* jointAccelerations = (double*)malloc(byteSizeJoints);
double* linearJacobian = (double*)malloc(3 * byteSizeJoints);
double* angularJacobian = (double*)malloc(3 * byteSizeJoints);
pybullet_internalSetVectord(localPosition, localPoint);
for (i = 0; i < numJoints; i++)
{
jointPositions[i] =
pybullet_internalGetFloatFromSequence(objPositions, i);
jointVelocities[i] =
pybullet_internalGetFloatFromSequence(objVelocities, i);
jointAccelerations[i] =
pybullet_internalGetFloatFromSequence(objAccelerations, i);
}
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle commandHandle =
b3CalculateJacobianCommandInit(sm, bodyUniqueId,
linkIndex, localPoint, jointPositions,
jointVelocities, jointAccelerations);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CALCULATED_JACOBIAN_COMPLETED)
{
int dofCount;
b3GetStatusJacobian(statusHandle, &dofCount, NULL, NULL);
if (dofCount)
{
int byteSizeDofCount = sizeof(double) * dofCount;
double* linearJacobian = (double*)malloc(3 * byteSizeDofCount);
double* angularJacobian = (double*)malloc(3 * byteSizeDofCount);
b3GetStatusJacobian(statusHandle,
NULL,
linearJacobian,
angularJacobian);
if (linearJacobian)
{
int r;
PyObject* pymat = PyTuple_New(3);
for (r = 0; r < 3; ++r) {
int c;
PyObject* pyrow = PyTuple_New(dofCount);
for (c = 0; c < dofCount; ++c) {
int element = r * dofCount + c;
PyTuple_SetItem(pyrow, c,
PyFloat_FromDouble(linearJacobian[element]));
}
PyTuple_SetItem(pymat, r, pyrow);
}
PyTuple_SetItem(pyResultList, 0, pymat);
}
if (angularJacobian)
{
int r;
PyObject* pymat = PyTuple_New(3);
for (r = 0; r < 3; ++r) {
int c;
PyObject* pyrow = PyTuple_New(dofCount);
for (c = 0; c < dofCount; ++c) {
int element = r * dofCount + c;
PyTuple_SetItem(pyrow, c,
PyFloat_FromDouble(angularJacobian[element]));
}
PyTuple_SetItem(pymat, r, pyrow);
}
PyTuple_SetItem(pyResultList, 1, pymat);
}
}
}
else
{
PyErr_SetString(SpamError,
"Internal error in calculateJacobian");
}
}
free(localPoint);
free(jointPositions);
free(jointVelocities);
free(jointAccelerations);
free(linearJacobian);
free(angularJacobian);
if (pyResultList) return pyResultList;
}
else
{
PyErr_SetString(SpamError,
"calculateJacobian [numJoints] needs to be "
"positive, [local position] needs to be of "
"size 3 and [joint positions], "
"[joint velocities], [joint accelerations] "
"need to match the number of joints.");
return NULL;
}
}
}
Py_INCREF(Py_None);
return Py_None;
}
static PyMethodDef SpamMethods[] = {
{"connect", (PyCFunction)pybullet_connectPhysicsServer, METH_VARARGS | METH_KEYWORDS,
"connect(method, key=SHARED_MEMORY_KEY, options='')\n"
"connect(method, hostname='localhost', port=1234, options='')\n"
"Connect to an existing physics server (using shared memory by default)."},
{"disconnect", (PyCFunction)pybullet_disconnectPhysicsServer, METH_VARARGS | METH_KEYWORDS,
"disconnect(physicsClientId=0)\n"
"Disconnect from the physics server."},
{"resetSimulation", (PyCFunction)pybullet_resetSimulation, METH_VARARGS | METH_KEYWORDS,
"resetSimulation(physicsClientId=0)\n"
"Reset the simulation: remove all objects and start from an empty world."},
{"stepSimulation", (PyCFunction)pybullet_stepSimulation, METH_VARARGS | METH_KEYWORDS,
"stepSimulation(physicsClientId=0)\n"
"Step the simulation using forward dynamics."},
{"setGravity", (PyCFunction)pybullet_setGravity, METH_VARARGS | METH_KEYWORDS,
"setGravity(gravX, gravY, gravZ, physicsClientId=0)\n"
"Set the gravity acceleration (x,y,z)."},
{"setTimeStep", (PyCFunction)pybullet_setTimeStep, METH_VARARGS | METH_KEYWORDS,
"setTimeStep(timestep, physicsClientId=0)\n"
"Set the amount of time to proceed at each call to stepSimulation. (unit "
"is seconds, typically range is 0.01 or 0.001)"},
{"setDefaultContactERP", (PyCFunction)pybullet_setDefaultContactERP, METH_VARARGS | METH_KEYWORDS,
"setDefaultContactERP(defaultContactERP, physicsClientId=0)\n"
"Set the amount of contact penetration Error Recovery Paramater "
"(ERP) in each time step. \
This is an tuning parameter to control resting contact stability. "
"This value depends on the time step."},
{"setRealTimeSimulation", (PyCFunction)pybullet_setRealTimeSimulation, METH_VARARGS | METH_KEYWORDS,
"setRealTimeSimulation(enableRealTimeSimulation, physicsClientId=0)\n"
"Enable or disable real time simulation (using the real time clock,"
" RTC) in the physics server. Expects one integer argument, 0 or 1"},
@@ -7001,6 +7307,8 @@ static PyMethodDef SpamMethods[] = {
"This is for experimental purposes, use at own risk, magic may or not happen"},
{"loadURDF", (PyCFunction)pybullet_loadURDF, METH_VARARGS | METH_KEYWORDS,
"bodyUniqueId = loadURDF(fileName, basePosition=[0.,0.,0.], baseOrientation=[0.,0.,0.,1.], "
"useMaximalCoordinates=0, useFixedBase=0, flags=0, globalScaling=1.0, physicsClientId=0)\n"
"Create a multibody by loading a URDF file."},
{"loadSDF", (PyCFunction)pybullet_loadSDF, METH_VARARGS | METH_KEYWORDS,
@@ -7249,6 +7557,19 @@ static PyMethodDef SpamMethods[] = {
"Given an object id, joint positions, joint velocities and joint "
"accelerations, compute the joint forces using Inverse Dynamics"},
{"calculateJacobian", (PyCFunction)pybullet_calculateJacobian, METH_VARARGS | METH_KEYWORDS,
"Compute the jacobian for a specified local position on a body and its kinematics.\n"
"Args:\n"
" bodyIndex - a scalar defining the unique object id.\n"
" linkIndex - a scalar identifying the link containing the local point.\n"
" localPosition - a list of [x, y, z] of the coordinates of the local point.\n"
" objPositions - a list of the joint positions.\n"
" objVelocities - a list of the joint velocities.\n"
" objAccelerations - a list of the joint accelerations.\n"
"Returns:\n"
" linearJacobian - a list of the partial linear velocities of the jacobian.\n"
" angularJacobian - a list of the partial angular velocities of the jacobian.\n"},
{"calculateInverseKinematics", (PyCFunction)pybullet_calculateInverseKinematics,
METH_VARARGS | METH_KEYWORDS,
"Inverse Kinematics bindings: Given an object id, "
@@ -7281,6 +7602,16 @@ static PyMethodDef SpamMethods[] = {
"Cast a batch of rays and return the result for each of the rays (first object hit, if any. or -1) "
"Takes two arguments (list of from_positions [x,y,z] and a list of to_positions [x,y,z] in Cartesian world coordinates"},
{ "loadPlugin", (PyCFunction)pybullet_loadPlugin, METH_VARARGS | METH_KEYWORDS,
"Load a plugin, could implement custom commands etc." },
{ "unloadPlugin", (PyCFunction)pybullet_unloadPlugin, METH_VARARGS | METH_KEYWORDS,
"Unload a plugin, given the pluginUniqueId." },
{ "executePluginCommand", (PyCFunction)pybullet_executePluginCommand, METH_VARARGS | METH_KEYWORDS,
"Execute a command, implemented in a plugin." },
{"submitProfileTiming", (PyCFunction)pybullet_submitProfileTiming, METH_VARARGS | METH_KEYWORDS,
"Add a custom profile timing that will be visible in performance profile recordings on the physics server."
"On the physics server (in GUI and VR mode) you can press 'p' to start and/or stop profile recordings" },

3094
examples/pybullet/unity3d/autogen/NativeMethods.cs Normal file
View File

File diff suppressed because it is too large Load Diff

86
examples/pybullet/unity3d/examples/NewBehaviourScript.cs Normal file
View File

@@ -0,0 +1,86 @@
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.UI;
using System.Runtime.InteropServices;
using System;
[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)]
public class NewBehaviourScript : MonoBehaviour {
[System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint = "b3ConnectSharedMemory")]
public static extern System.IntPtr b3ConnectSharedMemory(int key);
[System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint = "b3CreateInProcessPhysicsServerAndConnect")]
public static extern System.IntPtr b3CreateInProcessPhysicsServerAndConnect(int argc, ref System.IntPtr argv);
[System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint = "b3InitStepSimulationCommand")]
public static extern System.IntPtr b3InitStepSimulationCommand(IntPtr physClient);
[System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint = "b3LoadUrdfCommandInit")]
public static extern System.IntPtr b3LoadUrdfCommandInit(IntPtr physClient, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string urdfFileName);
[System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint = "b3InitResetSimulationCommand")]
public static extern System.IntPtr b3InitResetSimulationCommand(IntPtr physClient);
[System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint = "b3SubmitClientCommandAndWaitStatus")]
public static extern System.IntPtr b3SubmitClientCommandAndWaitStatus(IntPtr physClient, IntPtr commandHandle);
[DllImport("TestCppPlug.dll")]
static extern int Add(int a, int b);
[DllImport("TestCppPlug.dll")]
static extern int CallMe(Action<int> action);
[DllImport("TestCppPlug.dll")]
static extern IntPtr CreateSharedAPI(int id);
[DllImport("TestCppPlug.dll")]
static extern int GetMyIdPlusTen(IntPtr api);
[DllImport("TestCppPlug.dll")]
static extern void DeleteSharedAPI(IntPtr api);
private void IWasCalled(int value)
{
text.text = value.ToString();
}
Text text;
IntPtr sharedAPI;
IntPtr pybullet;
// Use this for initialization
void Start () {
text = GetComponent<Text>();
CallMe(IWasCalled);
sharedAPI = CreateSharedAPI(30);
IntPtr pybullet = b3ConnectSharedMemory(12347);
IntPtr cmd = b3InitResetSimulationCommand(pybullet);
IntPtr status = b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
cmd = b3LoadUrdfCommandInit(pybullet, "plane.urdf");
status = b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
//IntPtr clientPtr = b3CreateInProcessPhysicsServerAndConnect(0, ref ptr);
}
// Update is called once per frame
void Update () {
IntPtr cmd = b3InitStepSimulationCommand(pybullet);
IntPtr status = b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
//System.IO.Directory.GetCurrentDirectory().ToString();//
//text.text = Add(4, 5).ToString();
text.text = UnityEngine.Random.Range(0f, 1f).ToString();// GetMyIdPlusTen(sharedAPI).ToString();
}
void OnDestroy()
{
DeleteSharedAPI(sharedAPI);
}
}

12
examples/pybullet/unity3d/examples/NewBehaviourScript.cs.meta Normal file
View File

@@ -0,0 +1,12 @@
fileFormatVersion: 2
guid: 6197b3a0389e92c47b7d8698e5f61f06
timeCreated: 1505961790
licenseType: Free
MonoImporter:
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

33
examples/pybullet/unity3d/readme.txt Normal file
View File

@@ -0,0 +1,33 @@
Quick prototype to connect Unity 3D to pybullet
Generate C# Native Methods using the Microsoft PInvoke Signature Toolkit:
sigimp.exe /lang:cs e:\develop\bullet3\examples\SharedMemory\PhysicsClientC_API.h
Add some #define B3_SHARED_API __declspec(dllexport) to the exported methods,
replace [3], [4], [16] by [] to get sigimp.exe working.
This generates autogen/NativeMethods.cs
Then put pybullet.dll in the right location, so Unity finds it.
NewBehaviourScript.cs is a 1 evening prototype that works within Unity 3D:
Create a connection to pybullet, reset the world, load a urdf at startup.
Step the simulation each Update.
Now the real work can start, converting Unity objects to pybullet,
pybullet robots to Unity, synchronizing the transforms each Update.
void Start () {
IntPtr pybullet = b3ConnectSharedMemory(12347);
IntPtr cmd = b3InitResetSimulationCommand(pybullet);
IntPtr status = b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
cmd = b3LoadUrdfCommandInit(pybullet, "plane.urdf");
status = b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
}
void Update ()
{
IntPtr cmd = b3InitStepSimulationCommand(pybullet);
IntPtr status = b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
}