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

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
examples/SharedMemory/plugins/b3PluginAPI.h
View File

@@ -3,40 +3,37 @@
#ifdef _WIN32
#define B3_SHARED_API __declspec(dllexport)
#elif defined (__GNUC__)
#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
#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" {
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);
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);
typedef B3_API_ENTRY struct UrdfRenderingInterface* (B3_API_CALL * PFN_GET_RENDER_INTERFACE)(struct b3PluginContext* context);
typedef B3_API_ENTRY struct b3PluginCollisionInterface* (B3_API_CALL * PFN_GET_COLLISION_INTERFACE)(struct b3PluginContext* context);
typedef B3_API_ENTRY struct UrdfRenderingInterface*(B3_API_CALL* PFN_GET_RENDER_INTERFACE)(struct b3PluginContext* context);
typedef B3_API_ENTRY struct b3PluginCollisionInterface*(B3_API_CALL* PFN_GET_COLLISION_INTERFACE)(struct b3PluginContext* context);
#ifdef __cplusplus
}
#endif
#endif //B3_PLUGIN_API_H
#endif //B3_PLUGIN_API_H

23
examples/SharedMemory/plugins/b3PluginCollisionInterface.h
View File

@@ -3,7 +3,7 @@
enum b3PluginCollisionFilterModes
{
B3_FILTER_GROUPAMASKB_AND_GROUPBMASKA=0,
B3_FILTER_GROUPAMASKB_AND_GROUPBMASKA = 0,
B3_FILTER_GROUPAMASKB_OR_GROUPBMASKA
};
@@ -11,23 +11,22 @@ struct b3PluginCollisionInterface
{
virtual void setBroadphaseCollisionFilter(
int objectUniqueIdA, int objectUniqueIdB,
int linkIndexA, int linkIndexB,
bool enableCollision)=0;
int linkIndexA, int linkIndexB,
bool enableCollision) = 0;
virtual void removeBroadphaseCollisionFilter(
int objectUniqueIdA, int objectUniqueIdB,
int linkIndexA, int linkIndexB)=0;
int linkIndexA, int linkIndexB) = 0;
virtual int getNumRules() const = 0;
virtual void resetAll()=0;
virtual void resetAll() = 0;
virtual int needsBroadphaseCollision(int objectUniqueIdA, int linkIndexA,
int collisionFilterGroupA,int collisionFilterMaskA,
int objectUniqueIdB, int linkIndexB,
int collisionFilterGroupB,int collisionFilterMaskB,
int filterMode
)=0;
virtual int needsBroadphaseCollision(int objectUniqueIdA, int linkIndexA,
int collisionFilterGroupA, int collisionFilterMaskA,
int objectUniqueIdB, int linkIndexB,
int collisionFilterGroupB, int collisionFilterMaskB,
int filterMode) = 0;
};
#endif //B3_PLUGIN_COLLISION_INTERFACE_H
#endif //B3_PLUGIN_COLLISION_INTERFACE_H

13
examples/SharedMemory/plugins/b3PluginContext.h
View File

@@ -5,11 +5,11 @@
struct b3PluginContext
{
b3PhysicsClientHandle m_physClient;
b3PhysicsClientHandle m_physClient;
//plugin can modify the m_userPointer to store persistent object pointer (class or struct instance etc)
void* m_userPointer;
const struct b3VRControllerEvent* m_vrControllerEvents;
int m_numVRControllerEvents;
const struct b3KeyboardEvent* m_keyEvents;
@@ -18,14 +18,9 @@ struct b3PluginContext
int m_numMouseEvents;
const struct b3Notification* m_notifications;
int m_numNotifications;
//only used for grpc/processClientCommands
class PhysicsCommandProcessorInterface* m_rpcCommandProcessorInterface;
};
#endif //B3_PLUGIN_CONTEXT_H
#endif //B3_PLUGIN_CONTEXT_H

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

@@ -1,6 +1,6 @@
//tinyRendererPlugin implements the TinyRenderer as a plugin
//it is statically linked when using preprocessor #define STATIC_LINK_VR_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"
@@ -19,25 +19,29 @@ struct b3CustomCollisionFilter
int m_linkIndexB;
bool m_enableCollision;
B3_FORCE_INLINE unsigned int getHash()const
B3_FORCE_INLINE unsigned int getHash() const
{
int obA = (m_objectUniqueIdA&0xff);
int obB = ((m_objectUniqueIdB &0xf)<<8);
int linkA = ((m_linkIndexA&0xff)<<16);
int linkB = ((m_linkIndexB&0xff)<<24);
int key = obA+obB+linkA+linkB;
int obA = (m_objectUniqueIdA & 0xff);
int obB = ((m_objectUniqueIdB & 0xf) << 8);
int linkA = ((m_linkIndexA & 0xff) << 16);
int linkB = ((m_linkIndexB & 0xff) << 24);
int key = obA + obB + linkA + linkB;
// Thomas Wang's hash
key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16);
key += ~(key << 15);
key ^= (key >> 10);
key += (key << 3);
key ^= (key >> 6);
key += ~(key << 11);
key ^= (key >> 16);
return key;
}
bool equals(const b3CustomCollisionFilter& other) const
{
return m_objectUniqueIdA == other.m_objectUniqueIdA &&
m_objectUniqueIdB == other.m_objectUniqueIdB&&
m_linkIndexA == other.m_linkIndexA &&
m_linkIndexB == other.m_linkIndexB;
m_objectUniqueIdB == other.m_objectUniqueIdB &&
m_linkIndexA == other.m_linkIndexA &&
m_linkIndexB == other.m_linkIndexB;
}
};
struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface
@@ -49,7 +53,6 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface
int linkIndexA, int linkIndexB,
bool enableCollision)
{
b3CustomCollisionFilter keyValue;
keyValue.m_objectUniqueIdA = objectUniqueIdA;
keyValue.m_linkIndexA = linkIndexA;
@@ -57,22 +60,20 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface
keyValue.m_linkIndexB = linkIndexB;
keyValue.m_enableCollision = enableCollision;
if (objectUniqueIdA>objectUniqueIdB)
if (objectUniqueIdA > objectUniqueIdB)
{
b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB);
b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
b3Swap(keyValue.m_objectUniqueIdA, keyValue.m_objectUniqueIdB);
b3Swap(keyValue.m_linkIndexA, keyValue.m_linkIndexB);
}
if (objectUniqueIdA==objectUniqueIdB)
if (objectUniqueIdA == objectUniqueIdB)
{
if (keyValue.m_linkIndexA>keyValue.m_linkIndexB)
if (keyValue.m_linkIndexA > keyValue.m_linkIndexB)
{
b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
b3Swap(keyValue.m_linkIndexA, keyValue.m_linkIndexB);
}
}
m_customCollisionFilters.insert(keyValue,keyValue);
m_customCollisionFilters.insert(keyValue, keyValue);
}
virtual void removeBroadphaseCollisionFilter(
@@ -84,17 +85,17 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface
keyValue.m_linkIndexA = linkIndexA;
keyValue.m_objectUniqueIdB = objectUniqueIdB;
keyValue.m_linkIndexB = linkIndexB;
if (objectUniqueIdA>objectUniqueIdB)
if (objectUniqueIdA > objectUniqueIdB)
{
b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB);
b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
b3Swap(keyValue.m_objectUniqueIdA, keyValue.m_objectUniqueIdB);
b3Swap(keyValue.m_linkIndexA, keyValue.m_linkIndexB);
}
if (objectUniqueIdA==objectUniqueIdB)
if (objectUniqueIdA == objectUniqueIdB)
{
if (keyValue.m_linkIndexA>keyValue.m_linkIndexB)
if (keyValue.m_linkIndexA > keyValue.m_linkIndexB)
{
b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
b3Swap(keyValue.m_linkIndexA, keyValue.m_linkIndexB);
}
}
@@ -111,12 +112,11 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface
m_customCollisionFilters.clear();
}
virtual int needsBroadphaseCollision(int objectUniqueIdA, int linkIndexA,
int collisionFilterGroupA,int collisionFilterMaskA,
int objectUniqueIdB, int linkIndexB,
int collisionFilterGroupB,int collisionFilterMaskB,
int filterMode
)
virtual int needsBroadphaseCollision(int objectUniqueIdA, int linkIndexA,
int collisionFilterGroupA, int collisionFilterMaskA,
int objectUniqueIdB, int linkIndexB,
int collisionFilterGroupB, int collisionFilterMaskB,
int filterMode)
{
//check and apply any custom rules for those objects/links
b3CustomCollisionFilter keyValue;
@@ -125,16 +125,16 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface
keyValue.m_objectUniqueIdB = objectUniqueIdB;
keyValue.m_linkIndexB = linkIndexB;
if (objectUniqueIdA>objectUniqueIdB)
if (objectUniqueIdA > objectUniqueIdB)
{
b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB);
b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
b3Swap(keyValue.m_objectUniqueIdA, keyValue.m_objectUniqueIdB);
b3Swap(keyValue.m_linkIndexA, keyValue.m_linkIndexB);
}
if (objectUniqueIdA==objectUniqueIdB)
if (objectUniqueIdA == objectUniqueIdB)
{
if (keyValue.m_linkIndexA>keyValue.m_linkIndexB)
if (keyValue.m_linkIndexA > keyValue.m_linkIndexB)
{
b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
b3Swap(keyValue.m_linkIndexA, keyValue.m_linkIndexB);
}
}
@@ -146,21 +146,20 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface
//otherwise use the default fallback
if (filterMode==B3_FILTER_GROUPAMASKB_AND_GROUPBMASKA)
if (filterMode == B3_FILTER_GROUPAMASKB_AND_GROUPBMASKA)
{
bool collides = (collisionFilterGroupA & collisionFilterMaskB) != 0;
collides = collides && (collisionFilterGroupB & collisionFilterMaskA);
return collides;
}
if (filterMode==B3_FILTER_GROUPAMASKB_OR_GROUPBMASKA)
if (filterMode == B3_FILTER_GROUPAMASKB_OR_GROUPBMASKA)
{
bool collides = (collisionFilterGroupA & collisionFilterMaskB) != 0;
collides = collides || (collisionFilterGroupB & collisionFilterMaskA);
return collides;
}
return false;
}
};
@@ -171,7 +170,7 @@ struct CollisionFilterMyClass
DefaultPluginCollisionInterface m_collisionFilter;
CollisionFilterMyClass()
:m_testData(42)
: m_testData(42)
{
}
virtual ~CollisionFilterMyClass()
@@ -186,23 +185,20 @@ B3_SHARED_API int initPlugin_collisionFilterPlugin(struct b3PluginContext* conte
return SHARED_MEMORY_MAGIC_NUMBER;
}
B3_SHARED_API struct b3PluginCollisionInterface* getCollisionInterface_collisionFilterPlugin(struct b3PluginContext* context)
{
CollisionFilterMyClass* obj = (CollisionFilterMyClass* )context->m_userPointer;
CollisionFilterMyClass* obj = (CollisionFilterMyClass*)context->m_userPointer;
return &obj->m_collisionFilter;
}
B3_SHARED_API int executePluginCommand_collisionFilterPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
return 0;
}
B3_SHARED_API void exitPlugin_collisionFilterPlugin(struct b3PluginContext* context)
{
CollisionFilterMyClass* obj = (CollisionFilterMyClass*) context->m_userPointer;
CollisionFilterMyClass* obj = (CollisionFilterMyClass*)context->m_userPointer;
delete obj;
context->m_userPointer = 0;
}

16
examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.h
View File

@@ -4,20 +4,20 @@
#include "../b3PluginAPI.h"
#ifdef __cplusplus
extern "C"
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);
//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 struct b3PluginCollisionInterface* getCollisionInterface_collisionFilterPlugin(struct b3PluginContext* context);
//all the APIs below are optional
B3_SHARED_API struct b3PluginCollisionInterface* getCollisionInterface_collisionFilterPlugin(struct b3PluginContext* context);
#ifdef __cplusplus
};
#endif
#endif//#define COLLISION_FILTER_PLUGIN_H
#endif //#define COLLISION_FILTER_PLUGIN_H

51
examples/SharedMemory/plugins/eglPlugin/eglRendererPlugin.cpp
View File

@@ -5,7 +5,7 @@
#else
#include <Python.h>
#endif
#endif //EGL_ADD_PYTHON_INIT
#endif //EGL_ADD_PYTHON_INIT
//eglRenderer plugin
@@ -18,11 +18,8 @@
#include "../b3PluginContext.h"
#include <stdio.h>
struct EGLRendererPluginClass
{
EGLRendererVisualShapeConverter m_renderer;
EGLRendererPluginClass()
{
@@ -39,16 +36,14 @@ B3_SHARED_API int initPlugin_eglRendererPlugin(struct b3PluginContext* context)
return SHARED_MEMORY_MAGIC_NUMBER;
}
B3_SHARED_API int executePluginCommand_eglRendererPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
return -1;
return -1;
}
B3_SHARED_API void exitPlugin_eglRendererPlugin(struct b3PluginContext* context)
{
EGLRendererPluginClass* obj = (EGLRendererPluginClass*) context->m_userPointer;
EGLRendererPluginClass* obj = (EGLRendererPluginClass*)context->m_userPointer;
delete obj;
context->m_userPointer = 0;
}
@@ -56,34 +51,29 @@ B3_SHARED_API void exitPlugin_eglRendererPlugin(struct b3PluginContext* context)
//all the APIs below are optional
B3_SHARED_API struct UrdfRenderingInterface* getRenderInterface_eglRendererPlugin(struct b3PluginContext* context)
{
EGLRendererPluginClass* obj = (EGLRendererPluginClass*) context->m_userPointer;
EGLRendererPluginClass* obj = (EGLRendererPluginClass*)context->m_userPointer;
return &obj->m_renderer;
}
#ifdef EGL_ADD_PYTHON_INIT
static PyMethodDef eglMethods[] = {
{NULL, NULL, 0, NULL} /* Sentinel */
{NULL, NULL, 0, NULL} /* Sentinel */
};
#if PY_MAJOR_VERSION >= 3
static struct PyModuleDef moduledef = {
PyModuleDef_HEAD_INIT, "eglRenderer", /* m_name */
"eglRenderer for PyBullet "
, /* m_doc */
-1, /* m_size */
eglMethods, /* m_methods */
NULL, /* m_reload */
NULL, /* m_traverse */
NULL, /* m_clear */
NULL, /* m_free */
PyModuleDef_HEAD_INIT, "eglRenderer", /* m_name */
"eglRenderer for PyBullet ", /* m_doc */
-1, /* m_size */
eglMethods, /* m_methods */
NULL, /* m_reload */
NULL, /* m_traverse */
NULL, /* m_clear */
NULL, /* m_free */
};
#endif
PyMODINIT_FUNC
#if PY_MAJOR_VERSION >= 3
PyInit_eglRenderer(void)
@@ -91,20 +81,17 @@ PyInit_eglRenderer(void)
initeglRenderer(void)
#endif
{
PyObject* m;
PyObject* m;
#if PY_MAJOR_VERSION >= 3
m = PyModule_Create(&moduledef);
m = PyModule_Create(&moduledef);
#else
m = Py_InitModule3("eglRenderer", eglMethods, "eglRenderer for PyBullet");
m = Py_InitModule3("eglRenderer", eglMethods, "eglRenderer for PyBullet");
#endif
#if PY_MAJOR_VERSION >= 3
if (m == NULL) return m;
if (m == NULL) return m;
#else
if (m == NULL) return;
if (m == NULL) return;
#endif
}
#endif //EGL_ADD_PYTHON_INIT
#endif //EGL_ADD_PYTHON_INIT

18
examples/SharedMemory/plugins/eglPlugin/eglRendererPlugin.h
View File

@@ -4,22 +4,20 @@
#include "../b3PluginAPI.h"
#ifdef __cplusplus
extern "C"
extern "C"
{
#endif
//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
B3_SHARED_API int initPlugin_eglRendererPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_eglRendererPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_eglRendererPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//all the APIs below are optional
B3_SHARED_API struct UrdfRenderingInterface* getRenderInterface_eglRendererPlugin(struct b3PluginContext* context);
//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
B3_SHARED_API int initPlugin_eglRendererPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_eglRendererPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_eglRendererPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//all the APIs below are optional
B3_SHARED_API struct UrdfRenderingInterface* getRenderInterface_eglRendererPlugin(struct b3PluginContext* context);
#ifdef __cplusplus
};
#endif
#endif//#define EGL_RENDERER_PLUGIN_H
#endif //#define EGL_RENDERER_PLUGIN_H

936
examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp
View File

File diff suppressed because it is too large Load Diff

31
examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h
View File

@@ -5,19 +5,18 @@
struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface
{
struct EGLRendererVisualShapeConverterInternalData* m_data;
EGLRendererVisualShapeConverter();
virtual ~EGLRendererVisualShapeConverter();
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int shapeUid, int objectIndex);
virtual int getNumVisualShapes(int bodyUniqueId);
virtual int getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData);
virtual void changeRGBAColor(int bodyUniqueId, int linkIndex, int shapeIndex, const double rgbaColor[4]);
virtual void changeShapeTexture(int objectUniqueId, int linkIndex, int shapeIndex, int textureUniqueId);
@@ -25,9 +24,9 @@ struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface
virtual void removeVisualShape(int shapeUid);
virtual void setUpAxis(int axis);
virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX,float camPosY, float camPosZ);
virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX, float camPosY, float camPosZ);
virtual void clearBuffers(struct TGAColor& clearColor);
virtual void resetAll();
@@ -43,22 +42,16 @@ struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface
virtual void setShadow(bool hasShadow);
virtual void setFlags(int flags);
virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
void copyCameraImageDataGL(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
void copyCameraImageDataGL(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
virtual void render();
virtual void render(const float viewMat[16], const float projMat[16]);
virtual int loadTextureFile(const char* filename);
virtual int registerTexture(unsigned char* texels, int width, int height);
virtual void syncTransform(int shapeUid, const class btTransform& worldTransform, const class btVector3& localScaling);
};
#endif //EGL_RENDERER_VISUAL_SHAPE_CONVERTER_H
#endif //EGL_RENDERER_VISUAL_SHAPE_CONVERTER_H

86
examples/SharedMemory/plugins/grpcPlugin/grpcPlugin.cpp
View File

@@ -19,24 +19,23 @@
using grpc::Server;
using grpc::ServerAsyncResponseWriter;
using grpc::ServerBuilder;
using grpc::ServerContext;
using grpc::ServerCompletionQueue;
using grpc::ServerContext;
using grpc::Status;
using pybullet_grpc::PyBulletAPI;
using pybullet_grpc::PyBulletCommand;
using pybullet_grpc::PyBulletStatus;
using pybullet_grpc::PyBulletAPI;
bool gVerboseNetworkMessagesServer4 = false;
class ServerImpl final {
class ServerImpl final
{
public:
ServerImpl()
{
}
~ServerImpl() {
~ServerImpl()
{
Exit();
}
@@ -51,8 +50,8 @@ public:
}
}
void Init(PhysicsCommandProcessorInterface* comProc, const std::string& hostNamePort) {
void Init(PhysicsCommandProcessorInterface* comProc, const std::string& hostNamePort)
{
// Listen on the given address without any authentication mechanism.
m_builder.AddListeningPort(hostNamePort, grpc::InsecureServerCredentials());
// Register "service_" as the instance through which we'll communicate with
@@ -72,7 +71,6 @@ public:
// This can be run in multiple threads if needed.
bool HandleSingleRpc()
{
CallData::CallStatus status = CallData::CallStatus::CREATE;
{
@@ -98,21 +96,31 @@ public:
private:
// Class encompasing the state and logic needed to serve a request.
class CallData {
class CallData
{
public:
// Take in the "service" instance (in this case representing an asynchronous
// server) and the completion queue "cq" used for asynchronous communication
// with the gRPC runtime.
CallData(PyBulletAPI::AsyncService* service, ServerCompletionQueue* cq, PhysicsCommandProcessorInterface* comProc)
: service_(service), cq_(cq), responder_(&ctx_), status_(CREATE), m_finished(false), m_comProc(comProc) {
: service_(service), cq_(cq), responder_(&ctx_), status_(CREATE), m_finished(false), m_comProc(comProc)
{
// Invoke the serving logic right away.
Proceed();
}
enum CallStatus { CREATE, PROCESS, FINISH, TERMINATE };
enum CallStatus
{
CREATE,
PROCESS,
FINISH,
TERMINATE
};
CallStatus Proceed() {
if (status_ == CREATE) {
CallStatus Proceed()
{
if (status_ == CREATE)
{
// Make this instance progress to the PROCESS state.
status_ = PROCESS;
@@ -122,11 +130,11 @@ private:
// instances can serve different requests concurrently), in this case
// the memory address of this CallData instance.
service_->RequestSubmitCommand(&ctx_, &m_command, &responder_, cq_, cq_,
this);
this);
}
else if (status_ == PROCESS) {
else if (status_ == PROCESS)
{
// Spawn a new CallData instance to serve new clients while we process
// the one for this CallData. The instance will deallocate itself as
// part of its FINISH state.
@@ -144,7 +152,6 @@ private:
m_status.set_statustype(CMD_UNKNOWN_COMMAND_FLUSHED);
if (m_command.has_checkversioncommand())
{
m_status.set_statustype(CMD_CLIENT_COMMAND_COMPLETED);
@@ -154,11 +161,10 @@ private:
{
cmdPtr = convertGRPCToBulletCommand(m_command, cmd);
if (cmdPtr)
{
bool hasStatus = m_comProc->processCommand(*cmdPtr, serverStatus, &buffer[0], buffer.size());
double timeOutInSeconds = 10;
b3Clock clock;
double startTimeSeconds = clock.getTimeInSeconds();
@@ -196,7 +202,8 @@ private:
responder_.Finish(m_status, Status::OK, this);
}
else {
else
{
GPR_ASSERT(status_ == FINISH);
// Once in the FINISH state, deallocate ourselves (CallData).
delete this;
@@ -215,8 +222,6 @@ private:
// client.
ServerContext ctx_;
// What we get from the client.
PyBulletCommand m_command;
// What we send back to the client.
@@ -231,16 +236,15 @@ private:
bool m_finished;
PhysicsCommandProcessorInterface* m_comProc; //physics server command processor
PhysicsCommandProcessorInterface* m_comProc; //physics server command processor
};
// This can be run in multiple threads if needed.
void InitRpcs(PhysicsCommandProcessorInterface* comProc)
void InitRpcs(PhysicsCommandProcessorInterface* comProc)
{
// Spawn a new CallData instance to serve new clients.
new CallData(&service_, cq_.get(), comProc);
}
ServerBuilder m_builder;
std::unique_ptr<ServerCompletionQueue> cq_;
@@ -248,19 +252,18 @@ private:
std::unique_ptr<Server> server_;
};
struct grpcMyClass
{
int m_testData;
ServerImpl m_grpcServer;
bool m_grpcInitialized;
bool m_grpcTerminated;
grpcMyClass()
:m_testData(42),
m_grpcInitialized(false),
m_grpcTerminated(false)
: m_testData(42),
m_grpcInitialized(false),
m_grpcTerminated(false)
{
}
virtual ~grpcMyClass()
@@ -272,12 +275,10 @@ B3_SHARED_API int initPlugin_grpcPlugin(struct b3PluginContext* context)
{
grpcMyClass* obj = new grpcMyClass();
context->m_userPointer = obj;
return SHARED_MEMORY_MAGIC_NUMBER;
}
B3_SHARED_API int preTickPluginCallback_grpcPlugin(struct b3PluginContext* context)
{
//process grpc server messages
@@ -287,32 +288,32 @@ B3_SHARED_API int preTickPluginCallback_grpcPlugin(struct b3PluginContext* conte
B3_SHARED_API int processClientCommands_grpcPlugin(struct b3PluginContext* context)
{
grpcMyClass* obj = (grpcMyClass*)context->m_userPointer;
if (obj->m_grpcInitialized && !obj->m_grpcTerminated)
{
obj->m_grpcTerminated = obj->m_grpcServer.HandleSingleRpc();
}
obj->m_testData++;
return 0;
}
B3_SHARED_API int postTickPluginCallback_grpcPlugin(struct b3PluginContext* context)
{
grpcMyClass* obj = (grpcMyClass* )context->m_userPointer;
grpcMyClass* obj = (grpcMyClass*)context->m_userPointer;
obj->m_testData++;
return 0;
}
B3_SHARED_API int executePluginCommand_grpcPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
///3 cases:
///3 cases:
/// 1: send a non-empty string to start the GRPC server
/// 2: send some integer n, to call n times to HandleSingleRpc
/// 3: send nothing to terminate the GRPC server
grpcMyClass* obj = (grpcMyClass*)context->m_userPointer;
if (strlen(arguments->m_text))
{
if (!obj->m_grpcInitialized && context->m_rpcCommandProcessorInterface)
@@ -339,14 +340,13 @@ B3_SHARED_API int executePluginCommand_grpcPlugin(struct b3PluginContext* contex
obj->m_grpcInitialized = false;
}
}
return 0;
}
B3_SHARED_API void exitPlugin_grpcPlugin(struct b3PluginContext* context)
{
grpcMyClass* obj = (grpcMyClass*) context->m_userPointer;
grpcMyClass* obj = (grpcMyClass*)context->m_userPointer;
obj->m_grpcServer.Exit();
delete obj;
context->m_userPointer = 0;

23
examples/SharedMemory/plugins/grpcPlugin/grpcPlugin.h
View File

@@ -4,26 +4,23 @@
#include "../b3PluginAPI.h"
#ifdef __cplusplus
extern "C"
extern "C"
{
#endif
//the following 3 APIs are required
B3_SHARED_API int initPlugin_grpcPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_grpcPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_grpcPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//all the APIs below are optional
B3_SHARED_API int preTickPluginCallback_grpcPlugin(struct b3PluginContext* context);
B3_SHARED_API int postTickPluginCallback_grpcPlugin(struct b3PluginContext* context);
B3_SHARED_API int processClientCommands_grpcPlugin(struct b3PluginContext* context);
//the following 3 APIs are required
B3_SHARED_API int initPlugin_grpcPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_grpcPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_grpcPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//all the APIs below are optional
B3_SHARED_API int preTickPluginCallback_grpcPlugin(struct b3PluginContext* context);
B3_SHARED_API int postTickPluginCallback_grpcPlugin(struct b3PluginContext* context);
B3_SHARED_API int processClientCommands_grpcPlugin(struct b3PluginContext* context);
#ifdef __cplusplus
};
#endif
#endif//#define GRPC_PLUGIN_H
#endif //#define GRPC_PLUGIN_H

103
examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
View File

@@ -4,7 +4,6 @@
#include "../b3PluginContext.h"
#include <stdio.h>
#include "LinearMath/btScalar.h"
#include "LinearMath/btAlignedObjectArray.h"
@@ -28,7 +27,7 @@ struct MyPDControlContainer
btAlignedObjectArray<MyPDControl> m_controllers;
b3RobotSimulatorClientAPI_NoDirect m_api;
MyPDControlContainer()
:m_testData(42)
: m_testData(42)
{
}
virtual ~MyPDControlContainer()
@@ -48,54 +47,50 @@ B3_SHARED_API int initPlugin_pdControlPlugin(struct b3PluginContext* context)
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];
int dof1 = 0;
b3JointSensorState actualState;
if (obj->m_api.getJointState(pdControl.m_objectUniqueId, pdControl.m_linkIndex,&actualState))
if (obj->m_api.getJointState(pdControl.m_objectUniqueId, pdControl.m_linkIndex, &actualState))
{
if (pdControl.m_maxForce>0)
if (pdControl.m_maxForce > 0)
{
//compute torque
btScalar qActual = actualState.m_jointPosition;
btScalar qdActual = actualState.m_jointVelocity;
btScalar positionError = (pdControl.m_desiredPosition -qActual);
btScalar positionError = (pdControl.m_desiredPosition - qActual);
double desiredVelocity = 0;
btScalar velocityError = (pdControl.m_desiredVelocity-qdActual);
btScalar velocityError = (pdControl.m_desiredVelocity - qdActual);
btScalar force = pdControl.m_kp * positionError + pdControl.m_kd*velocityError;
btScalar force = pdControl.m_kp * positionError + pdControl.m_kd * velocityError;
btClamp(force, -pdControl.m_maxForce, pdControl.m_maxForce);
btClamp(force,-pdControl.m_maxForce,pdControl.m_maxForce);
//apply torque
b3RobotSimulatorJointMotorArgs args(CONTROL_MODE_TORQUE);
args.m_maxTorqueValue = force;
obj->m_api.setJointMotorControl(pdControl.m_objectUniqueId, pdControl.m_linkIndex, args);
}
}
}
return 0;
}
B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer;
obj->m_api.syncBodies();
int numObj = obj->m_api.getNumBodies();
//printf("numObj = %d\n", numObj);
@@ -109,50 +104,50 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c
switch (arguments->m_ints[0])
{
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];
int foundIndex = -1;
for (int i = 0; i < obj->m_controllers.size(); i++)
case eSetPDControl:
{
if (obj->m_controllers[i].m_objectUniqueId == controller.m_objectUniqueId && obj->m_controllers[i].m_linkIndex == controller.m_linkIndex)
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];
int foundIndex = -1;
for (int i = 0; i < obj->m_controllers.size(); i++)
{
obj->m_controllers[i] = controller;
foundIndex=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;
foundIndex = i;
}
}
if (foundIndex < 0)
{
obj->m_controllers.push_back(controller);
}
break;
}
if (foundIndex<0)
case eRemovePDControl:
{
obj->m_controllers.push_back(controller);
}
break;
}
case eRemovePDControl:
{
MyPDControl controller;
controller.m_objectUniqueId = arguments->m_ints[1];
controller.m_linkIndex = arguments->m_ints[2];
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)
for (int i = 0; i < obj->m_controllers.size(); i++)
{
obj->m_controllers.removeAtIndex(i);
break;
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;
}
break;
}
default:
default:
{
return -1;
}
@@ -162,11 +157,9 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c
return result;
}
B3_SHARED_API void exitPlugin_pdControlPlugin(struct b3PluginContext* context)
{
MyPDControlContainer* obj = (MyPDControlContainer*) context->m_userPointer;
MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer;
delete obj;
context->m_userPointer = 0;
}

29
examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
View File

@@ -4,28 +4,27 @@
#include "../b3PluginAPI.h"
#ifdef __cplusplus
extern "C"
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);
//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
{
eSetPDControl = 1,
eRemovePDControl = 2,
};
//all the APIs below are optional
B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext* context);
///
enum PDControlCommandEnum
{
eSetPDControl = 1,
eRemovePDControl = 2,
};
//all the APIs below are optional
B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext* context);
#ifdef __cplusplus
};
#endif
#endif//#define PID_CONTROL_PLUGIN_H
#endif //#define PID_CONTROL_PLUGIN_H

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

@@ -21,7 +21,7 @@ struct MyClass
int m_testData;
MyClass()
:m_testData(42)
: m_testData(42)
{
}
virtual ~MyClass()
@@ -38,47 +38,43 @@ B3_SHARED_API int initPlugin_testPlugin(struct b3PluginContext* context)
return SHARED_MEMORY_MAGIC_NUMBER;
}
B3_SHARED_API int preTickPluginCallback_testPlugin(struct b3PluginContext* context)
{
return 0;
}
B3_SHARED_API int postTickPluginCallback_testPlugin(struct b3PluginContext* context)
{
MyClass* obj = (MyClass* )context->m_userPointer;
MyClass* obj = (MyClass*)context->m_userPointer;
obj->m_testData++;
return 0;
}
B3_SHARED_API int executePluginCommand_testPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
printf("text argument:%s\n",arguments->m_text);
printf("text argument:%s\n", arguments->m_text);
printf("int args: [");
for (int i=0;i<arguments->m_numInts;i++)
for (int i = 0; i < arguments->m_numInts; i++)
{
printf("%d", arguments->m_ints[i]);
if ((i+1)<arguments->m_numInts)
if ((i + 1) < arguments->m_numInts)
{
printf(",");
}
}
printf("]\nfloat args: [");
for (int i=0;i<arguments->m_numFloats;i++)
for (int i = 0; i < arguments->m_numFloats; i++)
{
printf("%f", arguments->m_floats[i]);
if ((i+1)<arguments->m_numFloats)
if ((i + 1) < arguments->m_numFloats)
{
printf(",");
}
}
printf("]\n");
MyClass* obj = (MyClass*) context->m_userPointer;
MyClass* obj = (MyClass*)context->m_userPointer;
b3SharedMemoryStatusHandle statusHandle;
int statusType = -1;
int bodyUniqueId = -1;
@@ -90,15 +86,14 @@ B3_SHARED_API int executePluginCommand_testPlugin(struct b3PluginContext* contex
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_URDF_LOADING_COMPLETED)
{
bodyUniqueId = b3GetStatusBodyIndex(statusHandle);
bodyUniqueId = b3GetStatusBodyIndex(statusHandle);
}
return bodyUniqueId;
}
B3_SHARED_API void exitPlugin_testPlugin(struct b3PluginContext* context)
{
MyClass* obj = (MyClass*) context->m_userPointer;
MyClass* obj = (MyClass*)context->m_userPointer;
delete obj;
context->m_userPointer = 0;

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

@@ -4,22 +4,21 @@
#include "../b3PluginAPI.h"
#ifdef __cplusplus
extern "C"
extern "C"
{
#endif
//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
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_testPlugin(struct b3PluginContext* context);
B3_SHARED_API int postTickPluginCallback_testPlugin(struct b3PluginContext* context);
//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
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_testPlugin(struct b3PluginContext* context);
B3_SHARED_API int postTickPluginCallback_testPlugin(struct b3PluginContext* context);
#ifdef __cplusplus
};
#endif
#endif//#define TEST_PLUGIN_H
#endif //#define TEST_PLUGIN_H

916
examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
View File

File diff suppressed because it is too large Load Diff

31
examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h
View File

@@ -5,19 +5,18 @@
struct TinyRendererVisualShapeConverter : public UrdfRenderingInterface
{
struct TinyRendererVisualShapeConverterInternalData* m_data;
TinyRendererVisualShapeConverter();
virtual ~TinyRendererVisualShapeConverter();
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int shapeUid, int objectIndex);
virtual int getNumVisualShapes(int bodyUniqueId);
virtual int getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData);
virtual void changeRGBAColor(int bodyUniqueId, int linkIndex, int shapeIndex, const double rgbaColor[4]);
virtual void changeShapeTexture(int objectUniqueId, int linkIndex, int shapeIndex, int textureUniqueId);
@@ -25,9 +24,9 @@ struct TinyRendererVisualShapeConverter : public UrdfRenderingInterface
virtual void removeVisualShape(int shapeUid);
virtual void setUpAxis(int axis);
virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX,float camPosY, float camPosZ);
virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX, float camPosY, float camPosZ);
virtual void clearBuffers(struct TGAColor& clearColor);
virtual void resetAll();
@@ -43,21 +42,15 @@ struct TinyRendererVisualShapeConverter : public UrdfRenderingInterface
virtual void setShadow(bool hasShadow);
virtual void setFlags(int flags);
virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
virtual void render();
virtual void render(const float viewMat[16], const float projMat[16]);
virtual int loadTextureFile(const char* filename);
virtual int registerTexture(unsigned char* texels, int width, int height);
virtual void syncTransform(int shapeUid, const class btTransform& worldTransform, const class btVector3& localScaling);
};
#endif //TINY_RENDERER_VISUAL_SHAPE_CONVERTER_H
#endif //TINY_RENDERER_VISUAL_SHAPE_CONVERTER_H

12
examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp
View File

@@ -18,11 +18,8 @@ while (1):
#include "../b3PluginContext.h"
#include <stdio.h>
struct MyRendererPluginClass
{
TinyRendererVisualShapeConverter m_renderer;
MyRendererPluginClass()
{
@@ -39,16 +36,14 @@ B3_SHARED_API int initPlugin_tinyRendererPlugin(struct b3PluginContext* context)
return SHARED_MEMORY_MAGIC_NUMBER;
}
B3_SHARED_API int executePluginCommand_tinyRendererPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
return -1;
return -1;
}
B3_SHARED_API void exitPlugin_tinyRendererPlugin(struct b3PluginContext* context)
{
MyRendererPluginClass* obj = (MyRendererPluginClass*) context->m_userPointer;
MyRendererPluginClass* obj = (MyRendererPluginClass*)context->m_userPointer;
delete obj;
context->m_userPointer = 0;
}
@@ -56,7 +51,6 @@ B3_SHARED_API void exitPlugin_tinyRendererPlugin(struct b3PluginContext* context
//all the APIs below are optional
B3_SHARED_API struct UrdfRenderingInterface* getRenderInterface_tinyRendererPlugin(struct b3PluginContext* context)
{
MyRendererPluginClass* obj = (MyRendererPluginClass*) context->m_userPointer;
MyRendererPluginClass* obj = (MyRendererPluginClass*)context->m_userPointer;
return &obj->m_renderer;
}

18
examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.h
View File

@@ -4,22 +4,20 @@
#include "../b3PluginAPI.h"
#ifdef __cplusplus
extern "C"
extern "C"
{
#endif
//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
B3_SHARED_API int initPlugin_tinyRendererPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_tinyRendererPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_tinyRendererPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//all the APIs below are optional
B3_SHARED_API struct UrdfRenderingInterface* getRenderInterface_tinyRendererPlugin(struct b3PluginContext* context);
//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
B3_SHARED_API int initPlugin_tinyRendererPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_tinyRendererPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_tinyRendererPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//all the APIs below are optional
B3_SHARED_API struct UrdfRenderingInterface* getRenderInterface_tinyRendererPlugin(struct b3PluginContext* context);
#ifdef __cplusplus
};
#endif
#endif//#define TEST_PLUGIN_H
#endif //#define TEST_PLUGIN_H

67
examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp
View File

@@ -23,13 +23,13 @@ struct MyClass
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)
: m_testData(42),
m_controllerId(-1),
m_constraintId(-1),
m_constraintId2(-1),
m_gripperId(-1),
m_maxForce(0),
m_maxForce2(0)
{
}
virtual ~MyClass()
@@ -46,21 +46,20 @@ B3_SHARED_API int initPlugin_vrSyncPlugin(struct b3PluginContext* context)
return SHARED_MEMORY_MAGIC_NUMBER;
}
B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* context)
{
MyClass* obj = (MyClass* )context->m_userPointer;
if (obj && obj->m_controllerId>=0)
MyClass* obj = (MyClass*)context->m_userPointer;
if (obj && obj->m_controllerId >= 0)
{
{
int i = 0;
{
for (int n=0;n<context->m_numVRControllerEvents;n++)
for (int n = 0; n < context->m_numVRControllerEvents; n++)
{
const b3VRControllerEvent& event = context->m_vrControllerEvents[n];
if (event.m_controllerId ==obj->m_controllerId)
if (event.m_controllerId == obj->m_controllerId)
{
if (obj->m_constraintId>=0)
if (obj->m_constraintId >= 0)
{
struct b3UserConstraint constraintInfo;
if (b3GetUserConstraintInfo(context->m_physClient, obj->m_constraintId, &constraintInfo))
@@ -70,16 +69,16 @@ B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* con
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};
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]};
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)
if (obj->m_constraintId2 >= 0)
{
struct b3UserConstraint constraintInfo;
if (b3GetUserConstraintInfo(context->m_physClient, obj->m_constraintId2, &constraintInfo))
@@ -92,16 +91,16 @@ B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* con
//0 -> open, 1 = closed
double openPos = 1.;
double relPosTarget = openPos - (event.m_analogAxis*openPos);
double relPosTarget = openPos - (event.m_analogAxis * openPos);
b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, relPosTarget);
b3InitChangeUserConstraintSetERP(commandHandle,1);
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)
if (obj->m_gripperId >= 0)
{
//this block is similar to
//b = p.getJointState(pr2_gripper,2)[0]
@@ -111,9 +110,9 @@ B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* con
//printf("obj->m_gripperId=%d\n", obj->m_gripperId);
{
b3SharedMemoryCommandHandle cmd_handle =
b3RequestActualStateCommandInit(context->m_physClient, obj->m_gripperId);
b3RequestActualStateCommandInit(context->m_physClient, obj->m_gripperId);
b3SharedMemoryStatusHandle status_handle =
b3SubmitClientCommandAndWaitStatus(context->m_physClient, cmd_handle);
b3SubmitClientCommandAndWaitStatus(context->m_physClient, cmd_handle);
int status_type = b3GetStatusType(status_handle);
if (status_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
@@ -123,8 +122,6 @@ B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* con
b3JointSensorState sensorState;
if (b3GetJointState(context->m_physClient, status_handle, 2, &sensorState))
{
b3SharedMemoryCommandHandle commandHandle;
double targetPosition = sensorState.m_jointPosition;
//printf("targetPosition =%f\n", targetPosition);
@@ -138,25 +135,23 @@ B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* con
double kd = .6;
b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition);
b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex,targetVelocity);
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
}
else
{
//printf("???\n");
}
} else
}
else
{
//printf("no\n");
}
}
}
}
}
}
@@ -166,12 +161,10 @@ B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* con
return 0;
}
B3_SHARED_API int executePluginCommand_vrSyncPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
MyClass* obj = (MyClass*) context->m_userPointer;
if (arguments->m_numInts>=4 && arguments->m_numFloats >= 2)
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];
@@ -187,18 +180,16 @@ B3_SHARED_API int executePluginCommand_vrSyncPlugin(struct b3PluginContext* cont
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, command);
int statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_SYNC_BODY_INFO_COMPLETED)
if (statusType != CMD_SYNC_BODY_INFO_COMPLETED)
{
}
}
return 0;
}
B3_SHARED_API void exitPlugin_vrSyncPlugin(struct b3PluginContext* context)
{
MyClass* obj = (MyClass*) context->m_userPointer;
MyClass* obj = (MyClass*)context->m_userPointer;
delete obj;
context->m_userPointer = 0;

17
examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.h
View File

@@ -4,21 +4,20 @@
#include "../b3PluginAPI.h"
#ifdef __cplusplus
extern "C"
extern "C"
{
#endif
//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
B3_SHARED_API int initPlugin_vrSyncPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_vrSyncPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_vrSyncPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//optional APIs
B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* context);
//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
B3_SHARED_API int initPlugin_vrSyncPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_vrSyncPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_vrSyncPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//optional APIs
B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* context);
#ifdef __cplusplus
};
#endif
#endif//#define TEST_PLUGIN_H
#endif //#define TEST_PLUGIN_H