PyBullet / BulletRobotics: prepare for pdControlPlugin and collisionFilterPlugin
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
@@ -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;
|
||||
|
||||
}
|
||||
@@ -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
44
examples/SharedMemory/plugins/pdControlPlugin/premake4.lua
Normal 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",
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user