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

enable pdControlPlugin by default (requires pdControlPlugin.cpp and b3RobotSimulatorClientAPI_NoDirect.cpp)

Browse Source 
add pdControl.py example, make pdControlPlugin functional
reduce memory usage
fix examples/pybullet/gym/pybullet_data/random_urdfs/948/948.urdf, fixes issue #1704
This commit is contained in:
erwincoumans
2018-06-05 15:59:01 -07:00
parent 49eb83c24e
commit b6f5cb4c34
23 changed files with 286 additions and 179 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
examples/SharedMemory/CMakeLists.txt
View File

@@ -1,5 +1,9 @@
SET(SharedMemory_SRCS
plugins/pdControlPlugin/pdControlPlugin.cpp
plugins/pdControlPlugin/pdControlPlugin.h
b3RobotSimulatorClientAPI_NoDirect.cpp
b3RobotSimulatorClientAPI_NoDirect.h
IKTrajectoryHelper.cpp
IKTrajectoryHelper.h
PhysicsClient.cpp

42
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -45,9 +45,9 @@
#include "../Extras/Serialize/BulletFileLoader/btBulletFile.h"
#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
#ifdef STATIC_PD_CONTROL_PLUGIN
#ifndef SKIP_STATIC_PD_CONTROL_PLUGIN
#include "plugins/pdControlPlugin/pdControlPlugin.h"
#endif //STATIC_PD_CONTROL_PLUGIN
#endif //SKIP_STATIC_PD_CONTROL_PLUGIN
#ifdef STATIC_LINK_VR_PLUGIN
#include "plugins/vrSyncPlugin/vrSyncPlugin.h"
@@ -105,7 +105,6 @@ struct UrdfLinkNameMapUtil
}
virtual ~UrdfLinkNameMapUtil()
{
delete m_memSerializer;
}
};
@@ -1599,7 +1598,7 @@ struct PhysicsServerCommandProcessorInternalData
btAlignedObjectArray<btMultiBodyWorldImporter*> m_worldImporters;
btAlignedObjectArray<UrdfLinkNameMapUtil*> m_urdfLinkNameMapper;
btAlignedObjectArray<std::string*> m_strings;
btAlignedObjectArray<btCollisionShape*> m_collisionShapes;
@@ -1692,11 +1691,11 @@ struct PhysicsServerCommandProcessorInternalData
m_pluginManager.registerStaticLinkedPlugin("vrSyncPlugin", initPlugin_vrSyncPlugin, exitPlugin_vrSyncPlugin, executePluginCommand_vrSyncPlugin, preTickPluginCallback_vrSyncPlugin, 0, 0);
#endif //STATIC_LINK_VR_PLUGIN
#ifdef STATIC_PD_CONTROL_PLUGIN
#ifndef SKIP_STATIC_PD_CONTROL_PLUGIN
{
m_pdControlPlugin = m_pluginManager.registerStaticLinkedPlugin("pdControlPlugin", initPlugin_pdControlPlugin, exitPlugin_pdControlPlugin, executePluginCommand_pdControlPlugin, preTickPluginCallback_pdControlPlugin, postTickPluginCallback_pdControlPlugin, 0);
m_pdControlPlugin = m_pluginManager.registerStaticLinkedPlugin("pdControlPlugin", initPlugin_pdControlPlugin, exitPlugin_pdControlPlugin, executePluginCommand_pdControlPlugin, preTickPluginCallback_pdControlPlugin, 0, 0);
}
#endif //STATIC_PD_CONTROL_PLUGIN
#endif //SKIP_STATIC_PD_CONTROL_PLUGIN
#ifndef SKIP_STATIC_TINYRENDERER_PLUGIN
@@ -2482,12 +2481,13 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
}
m_data->m_worldImporters.clear();
#ifdef ENABLE_LINK_MAPPER
for (int i=0;i<m_data->m_urdfLinkNameMapper.size();i++)
{
delete m_data->m_urdfLinkNameMapper[i];
}
m_data->m_urdfLinkNameMapper.clear();
#endif //ENABLE_LINK_MAPPER
for (int i=0;i<m_data->m_strings.size();i++)
{
@@ -3010,10 +3010,12 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
btMultiBody* mb = bodyHandle? bodyHandle->m_multiBody:0;
if (mb)
{
UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil;
m_data->m_urdfLinkNameMapper.push_back(util);
UrdfLinkNameMapUtil utilBla;
UrdfLinkNameMapUtil* util = &utilBla;
btDefaultSerializer ser(bufferSizeInBytes ,(unsigned char*)bufferServerToClient);
util->m_mb = mb;
util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient);
util->m_memSerializer = &ser;
util->m_memSerializer->startSerialization();
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
@@ -3046,9 +3048,11 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
btRigidBody* rb = bodyHandle? bodyHandle->m_rigidBody :0;
if (rb)
{
UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil;
m_data->m_urdfLinkNameMapper.push_back(util);
util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient);
UrdfLinkNameMapUtil utilBla;
UrdfLinkNameMapUtil* util = &utilBla;
btDefaultSerializer ser(bufferSizeInBytes ,(unsigned char*)bufferServerToClient);
util->m_memSerializer = &ser;
util->m_memSerializer->startSerialization();
util->m_memSerializer->registerNameForPointer(bodyHandle->m_rigidBody,bodyHandle->m_bodyName.c_str());
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
@@ -6049,10 +6053,8 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S
serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_COMPLETED;
int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
if (m_data->m_urdfLinkNameMapper.size())
{
serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
}
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
@@ -6120,12 +6122,14 @@ bool PhysicsServerCommandProcessor::processLoadURDFCommand(const struct SharedMe
serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED;
int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
#ifdef ENABLE_LINK_MAPPER
if (m_data->m_urdfLinkNameMapper.size())
{
serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
}
#endif
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());

65
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View File

@@ -1,6 +1,5 @@
#include "b3RobotSimulatorClientAPI_NoDirect.h"
#include "PhysicsClientC_API.h"
#include "b3RobotSimulatorClientAPI_InternalData.h"
@@ -35,71 +34,7 @@ b3RobotSimulatorClientAPI_NoDirect::~b3RobotSimulatorClientAPI_NoDirect()
}
bool b3RobotSimulatorClientAPI_NoDirect::connect(int mode, const std::string& hostName, int portOrKey)
{
if (m_data->m_physicsClientHandle)
{
b3Warning("Already connected, disconnect first.");
return false;
}
b3PhysicsClientHandle sm = 0;
int udpPort = 1234;
int tcpPort = 6667;
int key = SHARED_MEMORY_KEY;
bool connected = false;
switch (mode)
{
case eCONNECT_DIRECT:
{
break;
}
case eCONNECT_SHARED_MEMORY:
{
if (portOrKey >= 0)
{
key = portOrKey;
}
sm = b3ConnectSharedMemory(key);
break;
}
case eCONNECT_UDP:
{
if (portOrKey >= 0)
{
udpPort = portOrKey;
}
break;
}
case eCONNECT_TCP:
{
if (portOrKey >= 0)
{
tcpPort = portOrKey;
}
break;
}
default:
{
b3Warning("connectPhysicsServer unexpected argument");
}
};
if (sm)
{
m_data->m_physicsClientHandle = sm;
if (!b3CanSubmitCommand(m_data->m_physicsClientHandle))
{
disconnect();
return false;
}
return true;
}
return false;
}
bool b3RobotSimulatorClientAPI_NoDirect::isConnected() const
{

4
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
View File

@@ -419,7 +419,8 @@ public:
b3RobotSimulatorClientAPI_NoDirect();
virtual ~b3RobotSimulatorClientAPI_NoDirect();
bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1);
//No 'connect', use setInternalData to bypass the connect method, pass an existing client
virtual void setInternalData(struct b3RobotSimulatorClientAPI_InternalData* data);
void disconnect();
@@ -580,7 +581,6 @@ public:
bool getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo);
virtual void setInternalData(struct b3RobotSimulatorClientAPI_InternalData* data);
};

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

@@ -4,6 +4,7 @@
#include "../b3PluginContext.h"
#include <stdio.h>
#include "LinearMath/btScalar.h"
#include "LinearMath/btAlignedObjectArray.h"
@@ -52,31 +53,51 @@ B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext*
{
//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
int dof1 = 0;
b3JointSensorState actualState;
if (obj->m_api.getJointState(pdControl.m_objectUniqueId, pdControl.m_linkIndex,&actualState))
{
if (pdControl.m_maxForce>0)
{
//compute torque
btScalar qActual = actualState.m_jointPosition;
btScalar qdActual = actualState.m_jointVelocity;
btScalar positionError = (pdControl.m_desiredPosition -qActual);
double desiredVelocity = 0;
btScalar velocityError = (pdControl.m_desiredVelocity-qdActual);
btScalar force = pdControl.m_kp * positionError + pdControl.m_kd*velocityError;
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);
}
}
}
//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;
obj->m_api.syncBodies();
int numObj = obj->m_api.getNumBodies();
printf("numObj = %d\n", numObj);
//printf("numObj = %d\n", numObj);
//protocol:
//first int is the type of command
@@ -89,20 +110,6 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c
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)
@@ -116,14 +123,19 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c
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++)
{
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;
foundIndex=i;
}
}
if (foundIndex<0)
{
obj->m_controllers.push_back(controller);
}
break;
}
case eRemovePDControl:

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

@@ -16,14 +16,12 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c
///
enum PDControlCommandEnum
{
eAddPDControl = 1,
eSetPDControl = 2,
eRemovePDControl = 4,
eSetPDControl = 1,
eRemovePDControl = 2,
};
//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

4
examples/SharedMemory/premake4.lua
View File

@@ -20,6 +20,8 @@ language "C++"
myfiles =
{
"b3RobotSimulatorClientAPI_NoDirect.cpp",
"b3RobotSimulatorClientAPI_NoDirect.h",
"IKTrajectoryHelper.cpp",
"IKTrajectoryHelper.h",
"PhysicsClient.cpp",
@@ -58,6 +60,8 @@ myfiles =
"PhysicsServerCommandProcessor.h",
"b3PluginManager.cpp",
"b3PluginManager.h",
"plugins/pdControlPlugin/pdControlPlugin.cpp",
"plugins/pdControlPlugin/pdControlPlugin.h",
"../OpenGLWindow/SimpleCamera.cpp",
"../OpenGLWindow/SimpleCamera.h",
"../Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h",

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

@@ -91,7 +91,12 @@ myfiles =
"../PhysicsServerCommandProcessor.h",
"../b3PluginManager.cpp",
"../PhysicsDirect.cpp",
"../PhysicsClientC_API.cpp",
"../PhysicsClient.cpp",
"../plugins/pdControlPlugin/pdControlPlugin.cpp",
"../plugins/pdControlPlugin/pdControlPlugin.h",
"../b3RobotSimulatorClientAPI_NoDirect.cpp",
"../b3RobotSimulatorClientAPI_NoDirect.h",
"../plugins/tinyRendererPlugin/tinyRendererPlugin.cpp",
"../plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp",
"../../TinyRenderer/geometry.cpp",

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

@@ -77,6 +77,10 @@ language "C++"
myfiles =
{
"../plugins/pdControlPlugin/pdControlPlugin.cpp",
"../plugins/pdControlPlugin/pdControlPlugin.h",
"../b3RobotSimulatorClientAPI_NoDirect.cpp",
"../b3RobotSimulatorClientAPI_NoDirect.h",
"../IKTrajectoryHelper.cpp",
"../IKTrajectoryHelper.h",
"../SharedMemoryCommands.h",
@@ -85,6 +89,7 @@ myfiles =
"../PhysicsServerCommandProcessor.h",
"../b3PluginManager.cpp",
"../PhysicsDirect.cpp",
"../PhysicsClientC_API.cpp",
"../PhysicsClient.cpp",
"../plugins/tinyRendererPlugin/tinyRendererPlugin.cpp",
"../plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp",