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

PyBullet / BulletRobotics: prepare for pdControlPlugin and collisionFilterPlugin

Browse Source 
Split examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.* and move to examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp and examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
This commit is contained in:
Erwin Coumans
2018-06-05 11:41:41 +10:00
parent e4cd88e24f
commit 7bd84740d7
23 changed files with 719 additions and 198 deletions
Download Patch File Download Diff File Expand all files Collapse all files

98
examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp Normal file
View File

@@ -0,0 +1,98 @@
//tinyRendererPlugin implements the TinyRenderer as a plugin
//it is statically linked when using preprocessor #define STATIC_LINK_VR_PLUGIN
//otherwise you can dynamically load it using pybullet.loadPlugin
#include "collisionFilterPlugin.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_testPlugin(struct b3PluginContext* context)
{
MyClass* obj = new MyClass();
context->m_userPointer = obj;
printf("hi!\n");
return SHARED_MEMORY_MAGIC_NUMBER;
}
B3_SHARED_API int preTickPluginCallback_testPlugin(struct b3PluginContext* context)
{
//apply pd control here, apply forces using the PD gains
return 0;
}
B3_SHARED_API int postTickPluginCallback_testPlugin(struct b3PluginContext* context)
{
MyClass* obj = (MyClass* )context->m_userPointer;
obj->m_testData++;
return 0;
}
B3_SHARED_API int executePluginCommand_testPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
//set the PD gains
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_testPlugin(struct b3PluginContext* context)
{
MyClass* obj = (MyClass*) context->m_userPointer;
delete obj;
context->m_userPointer = 0;
printf("bye!\n");
}

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

@@ -0,0 +1,25 @@
#ifndef COLLISION_FILTER_PLUGIN_H
#define COLLISION_FILTER_PLUGIN_H
#include "../b3PluginAPI.h"
#ifdef __cplusplus
extern "C"
{
#endif
//the following 3 APIs are required
B3_SHARED_API int initPlugin_collisionFilterPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_collisionFilterPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_collisionFilterPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//all the APIs below are optional
B3_SHARED_API int preTickPluginCallback_collisionFilterPlugin(struct b3PluginContext* context);
B3_SHARED_API int postTickPluginCallback_collisionFilterPlugin(struct b3PluginContext* context);
#ifdef __cplusplus
};
#endif
#endif//#define COLLISION_FILTER_PLUGIN_H

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

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

162
examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp Normal file
View File

@@ -0,0 +1,162 @@
#include "pdControlPlugin.h"
#include "../../SharedMemoryPublic.h"
#include "../b3PluginContext.h"
#include <stdio.h>
#include "LinearMath/btScalar.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "../../b3RobotSimulatorClientAPI_NoDirect.h"
#include "../../b3RobotSimulatorClientAPI_InternalData.h"
struct MyPDControl
{
int m_objectUniqueId;
int m_linkIndex;
btScalar m_desiredPosition;
btScalar m_desiredVelocity;
btScalar m_kd;
btScalar m_kp;
btScalar m_maxForce;
};
struct MyPDControlContainer
{
int m_testData;
btAlignedObjectArray<MyPDControl> m_controllers;
b3RobotSimulatorClientAPI_NoDirect m_api;
MyPDControlContainer()
:m_testData(42)
{
}
virtual ~MyPDControlContainer()
{
}
};
B3_SHARED_API int initPlugin_pdControlPlugin(struct b3PluginContext* context)
{
MyPDControlContainer* obj = new MyPDControlContainer();
b3RobotSimulatorClientAPI_InternalData data;
data.m_physicsClientHandle = context->m_physClient;
data.m_guiHelper = 0;
obj->m_api.setInternalData(&data);
context->m_userPointer = obj;
return SHARED_MEMORY_MAGIC_NUMBER;
}
B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext* context)
{
//apply pd control here, apply forces using the PD gains
MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer;
for (int i = 0; i < obj->m_controllers.size(); i++)
{
const MyPDControl& pdControl = obj->m_controllers[i];
//compute torque
//apply torque
}
//for each registered pd controller, execute PD control
return 0;
}
B3_SHARED_API int postTickPluginCallback_pdControlPlugin(struct b3PluginContext* context)
{
MyPDControlContainer* obj = (MyPDControlContainer* )context->m_userPointer;
obj->m_testData++;
return 0;
}
B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer;
int numObj = obj->m_api.getNumBodies();
printf("numObj = %d\n", numObj);
//protocol:
//first int is the type of command
//second int is body unique id
//third int is link index
if (arguments->m_numInts != 3)
return -1;
switch (arguments->m_ints[0])
{
case eAddPDControl:
{
if (arguments->m_numFloats < 5)
return -1;
MyPDControl controller;
controller.m_desiredPosition = arguments->m_floats[0];
controller.m_desiredVelocity = arguments->m_floats[1];
controller.m_kd = arguments->m_floats[2];
controller.m_kp = arguments->m_floats[3];
controller.m_maxForce = arguments->m_floats[4];
controller.m_objectUniqueId = arguments->m_ints[1];
controller.m_linkIndex = arguments->m_ints[2];
obj->m_controllers.push_back(controller);
}
case eSetPDControl:
{
if (arguments->m_numFloats < 5)
return -1;
MyPDControl controller;
controller.m_desiredPosition = arguments->m_floats[0];
controller.m_desiredVelocity = arguments->m_floats[1];
controller.m_kd = arguments->m_floats[2];
controller.m_kp = arguments->m_floats[3];
controller.m_maxForce = arguments->m_floats[4];
controller.m_objectUniqueId = arguments->m_ints[1];
controller.m_linkIndex = arguments->m_ints[2];
for (int i = 0; i < obj->m_controllers.size(); i++)
{
if (obj->m_controllers[i].m_objectUniqueId == controller.m_objectUniqueId && obj->m_controllers[i].m_linkIndex == controller.m_linkIndex)
{
obj->m_controllers[i] = controller;
break;
}
}
break;
}
case eRemovePDControl:
{
MyPDControl controller;
controller.m_objectUniqueId = arguments->m_ints[1];
controller.m_linkIndex = arguments->m_ints[2];
for (int i = 0; i < obj->m_controllers.size(); i++)
{
if (obj->m_controllers[i].m_objectUniqueId == controller.m_objectUniqueId && obj->m_controllers[i].m_linkIndex == controller.m_linkIndex)
{
obj->m_controllers.removeAtIndex(i);
break;
}
}
break;
}
default:
{
return -1;
}
}
int result = 42;
return result;
}
B3_SHARED_API void exitPlugin_pdControlPlugin(struct b3PluginContext* context)
{
MyPDControlContainer* obj = (MyPDControlContainer*) context->m_userPointer;
delete obj;
context->m_userPointer = 0;
}

33
examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h Normal file
View File

@@ -0,0 +1,33 @@
#ifndef PID_CONTROL_PLUGIN_H
#define PID_CONTROL_PLUGIN_H
#include "../b3PluginAPI.h"
#ifdef __cplusplus
extern "C"
{
#endif
//the following 3 APIs are required
B3_SHARED_API int initPlugin_pdControlPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_pdControlPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
///
enum PDControlCommandEnum
{
eAddPDControl = 1,
eSetPDControl = 2,
eRemovePDControl = 4,
};
//all the APIs below are optional
B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext* context);
B3_SHARED_API int postTickPluginCallback_pdControlPlugin(struct b3PluginContext* context);
#ifdef __cplusplus
};
#endif
#endif//#define PID_CONTROL_PLUGIN_H

44
examples/SharedMemory/plugins/pdControlPlugin/premake4.lua Normal file
View File

@@ -0,0 +1,44 @@
project ("pybullet_pdControlPlugin")
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 {
"pdControlPlugin.cpp",
"../../b3RobotSimulatorClientAPI_NoDirect.cpp",
"../../b3RobotSimulatorClientAPI_NoDirect.h",
"../../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",
}

56
examples/SharedMemory/plugins/testPlugin/testplugin.cpp
View File

@@ -29,7 +29,7 @@ struct MyClass
}
};
B3_SHARED_API int initPlugin(struct b3PluginContext* context)
B3_SHARED_API int initPlugin_testPlugin(struct b3PluginContext* context)
{
MyClass* obj = new MyClass();
context->m_userPointer = obj;
@@ -39,67 +39,23 @@ B3_SHARED_API int initPlugin(struct b3PluginContext* context)
}
B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context)
B3_SHARED_API int preTickPluginCallback_testPlugin(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)
B3_SHARED_API int postTickPluginCallback_testPlugin(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)
B3_SHARED_API int executePluginCommand_testPlugin(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++)
@@ -140,7 +96,7 @@ B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const st
}
B3_SHARED_API void exitPlugin(struct b3PluginContext* context)
B3_SHARED_API void exitPlugin_testPlugin(struct b3PluginContext* context)
{
MyClass* obj = (MyClass*) context->m_userPointer;
delete obj;

11
examples/SharedMemory/plugins/testPlugin/testplugin.h
View File

@@ -9,14 +9,13 @@ 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);
B3_SHARED_API int initPlugin_testPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_testPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_testPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//all the APIs below are optional
B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context);
B3_SHARED_API int postTickPluginCallback(struct b3PluginContext* context);
B3_SHARED_API struct UrdfRenderingInterface* getRenderInterface(struct b3PluginContext* context);
B3_SHARED_API int preTickPluginCallback_testPlugin(struct b3PluginContext* context);
B3_SHARED_API int postTickPluginCallback_testPlugin(struct b3PluginContext* context);
#ifdef __cplusplus