AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-05 15:21:06 +00:00
Code Issues 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Extras/BulletRobotics/CMakeLists.txt
View File

@ -9,6 +9,8 @@ INCLUDE_DIRECTORIES(
)
SET(BulletRobotics_SRCS
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp

2
examples/ExampleBrowser/CMakeLists.txt
View File

@ -145,6 +145,8 @@ SET(BulletExampleBrowser_SRCS
../TinyRenderer/tgaimage.cpp
../TinyRenderer/our_gl.cpp
../TinyRenderer/TinyRenderer.cpp
../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
../SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp
../SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
../SharedMemory/IKTrajectoryHelper.cpp

2
examples/ExampleBrowser/premake4.lua
View File

@ -117,6 +117,8 @@ project "App_BulletExampleBrowser"
"../SharedMemory/b3PluginManager.cpp",
"../SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp",
"../SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp",
"../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
"../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
"../SharedMemory/SharedMemoryCommands.h",
"../SharedMemory/SharedMemoryPublic.h",
"../SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp",

2
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@ -595,7 +595,7 @@ void ConvertURDF2BulletInternal(
}
} else
{
if (cache.m_bulletMultiBody->getBaseMass()==0)
if (cache.m_bulletMultiBody->getBaseMass()==0 && cache.m_bulletMultiBody->getNumLinks()==0)
{
//col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
col->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);

3
examples/RobotSimulator/premake4.lua
View File

@ -1,6 +1,9 @@
myfiles =
{
"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp",

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

4
examples/TwoJoint/CMakeLists.txt
View File

@ -10,6 +10,10 @@ INCLUDE_DIRECTORIES(
SET(RobotSimulator_SRCS
TwoJointMain.cpp
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
../../examples/SharedMemory/IKTrajectoryHelper.cpp
../../examples/SharedMemory/IKTrajectoryHelper.h
../../examples/ExampleBrowser/InProcessExampleBrowser.cpp

4
examples/pybullet/CMakeLists.txt
View File

@ -15,6 +15,10 @@ ENDIF()
SET(pybullet_SRCS
pybullet.c
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
../../examples/SharedMemory/IKTrajectoryHelper.cpp
../../examples/SharedMemory/IKTrajectoryHelper.h
../../examples/ExampleBrowser/InProcessExampleBrowser.cpp

62
examples/pybullet/examples/pdControl.py Normal file
View File

@ -0,0 +1,62 @@
import pybullet as p
import time
useMaximalCoordinates=False
p.connect(p.GUI)
pole = p.loadURDF("cartpole.urdf", useMaximalCoordinates=useMaximalCoordinates)
for i in range (p.getNumJoints(pole)):
#disable default constraint-based motors
p.setJointMotorControl2(pole,i,p.POSITION_CONTROL,targetPosition=0,force=0)
print("joint",i,"=",p.getJointInfo(pole,i))
desiredPosCartId = p.addUserDebugParameter("desiredPosCart",-10,10,2)
desiredVelCartId = p.addUserDebugParameter("desiredVelCart",-10,10,0)
kpCartId = p.addUserDebugParameter("kpCart",0,500,300)
kdCartId = p.addUserDebugParameter("kpCart",0,300,150)
maxForceCartId = p.addUserDebugParameter("maxForceCart",0,5000,1000)
desiredPosPoleId = p.addUserDebugParameter("desiredPosPole",-10,10,0)
desiredVelPoleId = p.addUserDebugParameter("desiredVelPole",-10,10,0)
kpPoleId = p.addUserDebugParameter("kpPole",0,500,200)
kdPoleId = p.addUserDebugParameter("kpPole",0,300,100)
maxForcePoleId = p.addUserDebugParameter("maxForcePole",0,5000,1000)
pd = p.loadPlugin("pdControlPlugin")
p.setGravity(0,0,0)
useRealTimeSim = True
p.setRealTimeSimulation(useRealTimeSim)
p.setTimeStep(0.001)
while p.isConnected():
if (pd>=0):
desiredPosCart = p.readUserDebugParameter(desiredPosCartId)
desiredVelCart = p.readUserDebugParameter(desiredVelCartId)
kpCart = p.readUserDebugParameter(kpCartId)
kdCart = p.readUserDebugParameter(kdCartId)
maxForceCart = p.readUserDebugParameter(maxForceCartId)
link = 0
p.executePluginCommand(pd,"test",[1, pole, link], [desiredPosCart, desiredVelCart, kdCart, kpCart, maxForceCart])
desiredPosPole = p.readUserDebugParameter(desiredPosPoleId)
desiredVelPole = p.readUserDebugParameter(desiredVelPoleId)
kpPole = p.readUserDebugParameter(kpPoleId)
kdPole = p.readUserDebugParameter(kdPoleId)
maxForcePole = p.readUserDebugParameter(maxForcePoleId)
link = 1
p.executePluginCommand(pd,"test",[1, pole, link], [desiredPosPole, desiredVelPole, kdPole, kpPole, maxForcePole])
if (not useRealTimeSim):
p.stepSimulation()
time.sleep(1./240.)

31
examples/pybullet/gym/pybullet_data/random_urdfs/948/948.urdf
View File

@ -0,0 +1,31 @@
<robot name="blob948">
<link name="random_obj_948">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<inertia_scaling value="3.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.1"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="948.obj" scale="0.015 0.015 0.015"/>
</geometry>
<material name="blockmat">
<color rgba="0.26 0.59 0.61 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="948.obj" scale="0.015 0.015 0.015"/>
</geometry>
</collision>
</link>
</robot>

4
examples/pybullet/premake4.lua
View File

@ -103,6 +103,8 @@ if not _OPTIONS["no-enet"] then
"../../examples/TinyRenderer/our_gl.cpp",
"../../examples/TinyRenderer/TinyRenderer.cpp",
"../../examples/SharedMemory/InProcessMemory.cpp",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h",
"../../examples/SharedMemory/PhysicsClient.cpp",
"../../examples/SharedMemory/PhysicsClient.h",
"../../examples/SharedMemory/PhysicsServer.cpp",
@ -153,6 +155,8 @@ if not _OPTIONS["no-enet"] then
"../../examples/MultiThreading/b3PosixThreadSupport.cpp",
"../../examples/MultiThreading/b3Win32ThreadSupport.cpp",
"../../examples/MultiThreading/b3ThreadSupportInterface.cpp",
"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
}
if (_OPTIONS["enable_static_vr_plugin"]) then

9
examples/pybullet/pybullet.c
View File

@ -8316,7 +8316,14 @@ static PyObject* pybullet_executePluginCommand(PyObject* self,
float val = pybullet_internalGetFloatFromSequence(seqFloatArgs,i);
b3CustomCommandExecuteAddFloatArgument(command, val);
}
if (seqFloatArgs)
{
Py_DECREF(seqFloatArgs);
}
if (seqIntArgs)
{
Py_DECREF(seqIntArgs);
}
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);

2
setup.py
View File

@ -49,6 +49,8 @@ sources = ["examples/pybullet/pybullet.c"]\
+["examples/TinyRenderer/tgaimage.cpp"]\
+["examples/TinyRenderer/our_gl.cpp"]\
+["examples/TinyRenderer/TinyRenderer.cpp"]\
+["examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp"]\
+["examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp"]\
+["examples/SharedMemory/IKTrajectoryHelper.cpp"]\
+["examples/SharedMemory/InProcessMemory.cpp"]\
+["examples/SharedMemory/PhysicsClient.cpp"]\

123
test/SharedMemory/CMakeLists.txt
View File

@ -29,66 +29,69 @@ ENDIF()
ADD_EXECUTABLE(Test_PhysicsClientServer
gtestwrap.cpp
../../examples/SharedMemory/PhysicsClient.cpp
../../examples/SharedMemory/IKTrajectoryHelper.cpp
../../examples/SharedMemory/IKTrajectoryHelper.h
../../examples/SharedMemory/PhysicsClient.h
../../examples/SharedMemory/PhysicsServer.cpp
../../examples/SharedMemory/PhysicsServer.h
../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
../../examples/SharedMemory/PhysicsServerSharedMemory.h
../../examples/SharedMemory/PhysicsDirect.cpp
../../examples/SharedMemory/PhysicsDirect.h
../../examples/SharedMemory/PhysicsDirectC_API.cpp
../../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
../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h
../../examples/SharedMemory/PhysicsClientC_API.cpp
../../examples/SharedMemory/PhysicsClientC_API.h
../../examples/SharedMemory/PhysicsLoopBack.cpp
../../examples/SharedMemory/PhysicsLoopBack.h
../../examples/SharedMemory/PhysicsLoopBackC_API.cpp
../../examples/SharedMemory/PhysicsLoopBackC_API.h
../../examples/SharedMemory/Win32SharedMemory.cpp
../../examples/SharedMemory/Win32SharedMemory.h
../../examples/SharedMemory/PosixSharedMemory.cpp
../../examples/SharedMemory/PosixSharedMemory.h
../../examples/Utils/b3ResourcePath.cpp
../../examples/Utils/b3ResourcePath.h
../../examples/Utils/b3Clock.cpp
../../examples/Utils/b3Clock.h
../../examples/Utils/ChromeTraceUtil.cpp
../../examples/Utils/ChromeTraceUtil.h
../../examples/Utils/RobotLoggingUtil.cpp
../../examples/Utils/RobotLoggingUtil.h
../../examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp
../../examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
../../examples/OpenGLWindow/SimpleCamera.cpp
../../examples/OpenGLWindow/SimpleCamera.h
../../examples/TinyRenderer/geometry.cpp
../../examples/TinyRenderer/model.cpp
../../examples/TinyRenderer/tgaimage.cpp
../../examples/TinyRenderer/our_gl.cpp
../../examples/TinyRenderer/TinyRenderer.cpp
../../examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
../../examples/SharedMemory/IKTrajectoryHelper.cpp
../../examples/SharedMemory/IKTrajectoryHelper.h
../../examples/SharedMemory/PhysicsClient.h
../../examples/SharedMemory/PhysicsServer.cpp
../../examples/SharedMemory/PhysicsServer.h
../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
../../examples/SharedMemory/PhysicsServerSharedMemory.h
../../examples/SharedMemory/PhysicsDirect.cpp
../../examples/SharedMemory/PhysicsDirect.h
../../examples/SharedMemory/PhysicsDirectC_API.cpp
../../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
../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h
../../examples/SharedMemory/PhysicsClientC_API.cpp
../../examples/SharedMemory/PhysicsClientC_API.h
../../examples/SharedMemory/PhysicsLoopBack.cpp
../../examples/SharedMemory/PhysicsLoopBack.h
../../examples/SharedMemory/PhysicsLoopBackC_API.cpp
../../examples/SharedMemory/PhysicsLoopBackC_API.h
../../examples/SharedMemory/Win32SharedMemory.cpp
../../examples/SharedMemory/Win32SharedMemory.h
../../examples/SharedMemory/PosixSharedMemory.cpp
../../examples/SharedMemory/PosixSharedMemory.h
../../examples/Utils/b3ResourcePath.cpp
../../examples/Utils/b3ResourcePath.h
../../examples/Utils/b3Clock.cpp
../../examples/Utils/b3Clock.h
../../examples/Utils/ChromeTraceUtil.cpp
../../examples/Utils/ChromeTraceUtil.h
../../examples/Utils/RobotLoggingUtil.cpp
../../examples/Utils/RobotLoggingUtil.h
../../examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp
../../examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
../../examples/OpenGLWindow/SimpleCamera.cpp
../../examples/OpenGLWindow/SimpleCamera.h
../../examples/TinyRenderer/geometry.cpp
../../examples/TinyRenderer/model.cpp
../../examples/TinyRenderer/tgaimage.cpp
../../examples/TinyRenderer/our_gl.cpp
../../examples/TinyRenderer/TinyRenderer.cpp
../../examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
)

16
test/SharedMemory/premake4.lua
View File

@ -170,6 +170,10 @@ project ("Test_PhysicsServerLoopBack")
files {
"test.c",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h",
"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
"../../examples/SharedMemory/IKTrajectoryHelper.h",
"../../examples/SharedMemory/PhysicsClient.cpp",
@ -186,6 +190,8 @@ project ("Test_PhysicsServerLoopBack")
"../../examples/SharedMemory/PhysicsLoopBack.h",
"../../examples/SharedMemory/PhysicsLoopBackC_API.cpp",
"../../examples/SharedMemory/PhysicsLoopBackC_API.h",
"../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp",
"../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h",
"../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
"../../examples/SharedMemory/PhysicsClientSharedMemory.h",
"../../examples/SharedMemory/PhysicsClientC_API.cpp",
@ -224,7 +230,7 @@ project ("Test_PhysicsServerLoopBack")
"../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",
"../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
"../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
}
if (_OPTIONS["enable_static_plugins"]) then
@ -256,6 +262,10 @@ end
files {
"test.c",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h",
"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
"../../examples/SharedMemory/IKTrajectoryHelper.h",
"../../examples/SharedMemory/PhysicsClient.cpp",
@ -360,6 +370,10 @@ project ("Test_PhysicsServerInProcessExampleBrowser")
files {
"test.c",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h",
"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
"../../examples/SharedMemory/IKTrajectoryHelper.h",
"../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",