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

Merge pull request #2106 from erwincoumans/master

Browse Source 
fast createMultiBody batch creation, more deep_mimic work
This commit is contained in:
erwincoumans
2019-02-12 12:10:10 -08:00
committed by GitHub
parent b5a707de1d 85ee4c2934
commit e7e46154bb
30 changed files with 393 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
build_cmake_pybullet_double.sh
View File

@@ -12,4 +12,16 @@ cd pybullet
if [ -e pybullet.dylib ]; then
ln -f -s pybullet.dylib pybullet.so
fi
if [ -e pybullet_envs ]; then
rm pybullet_envs
fi
if [ -e pybullet_data ]; then
rm pybullet_data
fi
if [ -e pybullet_utils ]; then
rm pybullet_utils
fi
ln -s ../../../examples/pybullet/gym/pybullet_envs .
ln -s ../../../examples/pybullet/gym/pybullet_data .
ln -s ../../../examples/pybullet/gym/pybullet_utils .
echo "Completed build of Bullet."

5
examples/ExampleBrowser/InProcessExampleBrowser.cpp
View File

@@ -217,6 +217,11 @@ void ExampleBrowserThreadFunc(void* userPtr, void* lsMemory)
ExampleBrowserArgs* args = (ExampleBrowserArgs*)userPtr;
//int workLeft = true;
b3CommandLineArgs args2(args->m_argc, args->m_argv);
int minUpdateMs = 4000;
if (args2.GetCmdLineArgument("minGraphicsUpdateTimeMs", minUpdateMs))
{
gMinUpdateTimeMicroSecs = minUpdateMs;
}
b3Clock clock;
ExampleEntriesPhysicsServer examples;

19
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1604,11 +1604,30 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3Physics
command->m_createMultiBodyArgs.m_bodyName[0] = 0;
command->m_createMultiBodyArgs.m_baseLinkIndex = -1;
command->m_createMultiBodyArgs.m_numLinks = 0;
command->m_createMultiBodyArgs.m_numBatchObjects = 0;
return (b3SharedMemoryCommandHandle)command;
}
return 0;
}
//batch creation is an performance feature to create a large number of multi bodies in one command
B3_SHARED_API int b3CreateMultiBodySetBatchPositions(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, double* batchPositions, int numBatchObjects)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_MULTI_BODY);
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl && command->m_type == CMD_CREATE_MULTI_BODY)
{
command->m_createMultiBodyArgs.m_numBatchObjects = numBatchObjects;
cl->uploadBulletFileToSharedMemory((const char*)batchPositions, sizeof(double) * 3 * numBatchObjects);
}
return 0;
}
B3_SHARED_API int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, const double basePosition[3], const double baseOrientation[4], const double baseInertialFramePosition[3], const double baseInertialFrameOrientation[4])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -511,6 +511,9 @@ extern "C"
int linkJointType,
const double linkJointAxis[/*3*/]);
//batch creation is an performance feature to create a large number of multi bodies in one command
B3_SHARED_API int b3CreateMultiBodySetBatchPositions(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, double* batchPositions, int numBatchObjects);
//useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet
B3_SHARED_API void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle);
B3_SHARED_API void b3CreateMultiBodySetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);

72
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2621,7 +2621,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
#endif
//Workaround: in a VR application, where we avoid synchronizing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(32768);
m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(128 * 1024);
m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
@@ -4401,7 +4401,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
btVector3 v2(vertexUpload[i2*3+0],
vertexUpload[i2*3+1],
vertexUpload[i2*3+2]);
meshInterface->addTriangle(v0, v1, v2);
meshInterface->addTriangle(v0*meshScale, v1*meshScale, v2*meshScale);
}
}
@@ -4434,7 +4434,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
btVector3 pt(vertexUpload[v*3+0],
vertexUpload[v*3+1],
vertexUpload[v*3+2]);
convexHull->addPoint(pt, false);
convexHull->addPoint(pt*meshScale, false);
}
convexHull->recalcLocalAabb();
@@ -7058,6 +7058,36 @@ bool PhysicsServerCommandProcessor::processLoadSDFCommand(const struct SharedMem
bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
if (clientCmd.m_createMultiBodyArgs.m_numBatchObjects > 0)
{
//batch of objects, to speed up creation time
bool result = false;
SharedMemoryCommand clientCmd2 = clientCmd;
int baseLinkIndex = clientCmd.m_createMultiBodyArgs.m_baseLinkIndex;
double* basePositionAndOrientations = (double*)bufferServerToClient;
for (int i = 0; i < clientCmd2.m_createMultiBodyArgs.m_numBatchObjects; i++)
{
clientCmd2.m_createMultiBodyArgs.m_linkPositions[baseLinkIndex * 3 + 0] = basePositionAndOrientations[0 + i * 3];
clientCmd2.m_createMultiBodyArgs.m_linkPositions[baseLinkIndex * 3 + 1] = basePositionAndOrientations[1 + i * 3];
clientCmd2.m_createMultiBodyArgs.m_linkPositions[baseLinkIndex * 3 + 2] = basePositionAndOrientations[2 + i * 3];
if (i == (clientCmd2.m_createMultiBodyArgs.m_numBatchObjects - 1))
{
result = processCreateMultiBodyCommandSingle(clientCmd2, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
}
else
{
result = processCreateMultiBodyCommandSingle(clientCmd2, serverStatusOut, 0, 0);
}
}
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
return result;
}
return processCreateMultiBodyCommandSingle(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
}
bool PhysicsServerCommandProcessor::processCreateMultiBodyCommandSingle(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
BT_PROFILE("processCreateMultiBodyCommand2");
bool hasStatus = true;
serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_FAILED;
@@ -7082,10 +7112,16 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S
bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b);
bool ok = 0;
{
BT_PROFILE("processImportedObjects");
ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b);
}
if (ok)
{
BT_PROFILE("post process");
int bodyUniqueId = -1;
if (m_data->m_sdfRecentLoadedBodies.size() == 1)
@@ -7095,15 +7131,23 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S
m_data->m_sdfRecentLoadedBodies.clear();
if (bodyUniqueId >= 0)
{
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_COMPLETED;
if (bufferSizeInBytes>0 && serverStatusOut.m_numDataStreamBytes==0)
{
{
BT_PROFILE("autogenerateGraphicsObjects");
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
}
BT_PROFILE("createBodyInfoStream");
int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
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());
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());
}
}
}
@@ -8209,6 +8253,11 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
}
}
if (clientCmd.m_updateFlags & SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE)
{
m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = clientCmd.m_physSimParamArgs.m_minimumSolverIslandSize;
}
if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_COLLISION_FILTER_MODE)
{
m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode;
@@ -10977,6 +11026,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = processCreateVisualShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_CREATE_MULTI_BODY:
{
hasStatus = processCreateMultiBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);

2
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -41,6 +41,8 @@ protected:
bool processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processLoadSDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processCreateMultiBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processCreateMultiBodyCommandSingle(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processLoadURDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processCreateSensorCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);

10
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -980,14 +980,8 @@ struct b3CreateMultiBodyArgs
int m_linkJointTypes[MAX_CREATE_MULTI_BODY_LINKS];
double m_linkJointAxis[3 * MAX_CREATE_MULTI_BODY_LINKS];
int m_flags;
#if 0
std::string m_name;
std::string m_sourceFile;
btTransform m_rootTransformInWorld;
btHashMap<btHashString, UrdfMaterial*> m_materials;
btHashMap<btHashString, UrdfLink*> m_links;
btHashMap<btHashString, UrdfJoint*> m_joints;
#endif
int m_numBatchObjects;
};
struct b3CreateMultiBodyResultArgs

15
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View File

@@ -2184,6 +2184,21 @@ int b3RobotSimulatorClientAPI_NoDirect::createMultiBody(struct b3RobotSimulatorC
command = b3CreateMultiBodyCommandInit(sm);
if (args.m_useMaximalCoordinates)
{
b3CreateMultiBodyUseMaximalCoordinates(command);
}
if (args.m_batchPositions.size())
{
btAlignedObjectArray<double> positionArray;
for (int i = 0; i < args.m_batchPositions.size(); i++)
{
positionArray.push_back(args.m_batchPositions[i][0]);
positionArray.push_back(args.m_batchPositions[i][1]);
positionArray.push_back(args.m_batchPositions[i][2]);
}
b3CreateMultiBodySetBatchPositions(sm, command, &positionArray[0], args.m_batchPositions.size());
}
baseIndex = b3CreateMultiBodyBase(command, args.m_baseMass, args.m_baseCollisionShapeIndex, args.m_baseVisualShapeIndex,
doubleBasePosition, doubleBaseOrientation, doubleBaseInertialFramePosition, doubleBaseInertialFrameOrientation);

2
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
View File

@@ -418,7 +418,7 @@ struct b3RobotSimulatorCreateMultiBodyArgs
int *m_linkParentIndices;
int *m_linkJointTypes;
btVector3 *m_linkJointAxes;
btAlignedObjectArray<btVector3> m_batchPositions;
int m_useMaximalCoordinates;
b3RobotSimulatorCreateMultiBodyArgs()

16
examples/pybullet/CMakeLists.txt
View File

@@ -13,8 +13,24 @@ IF(BUILD_PYBULLET_NUMPY)
)
ENDIF()
ADD_DEFINITIONS(-DSTATIC_LINK_SPD_PLUGIN)
SET(pybullet_SRCS
pybullet.c
../../examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp
../../examples/SharedMemory/plugins/stablePDPlugin/SpAlg.h
../../examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp
../../examples/SharedMemory/plugins/stablePDPlugin/Shape.h
../../examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp
../../examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.h
../../examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp
../../examples/SharedMemory/plugins/stablePDPlugin/RBDModel.h
../../examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp
../../examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h
../../examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp
../../examples/SharedMemory/plugins/stablePDPlugin/KinTree.h
../../examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp
../../examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.h
../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h

150
examples/pybullet/examples/createMultiBodyBatch.py Normal file
View File

@@ -0,0 +1,150 @@
import pybullet as p
import time
import math
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000")
p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024)
p.setTimeStep(1./120.)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json")
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
p.setPhysicsEngineParameter(contactBreakingThreshold=0.04)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
#disable tinyrenderer, software (CPU) renderer, we don't use it here
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
shift = [0,-0.02,0]
meshScale=[0.1,0.1,0.1]
vertices=[
[-1.000000,-1.000000,1.000000],
[1.000000,-1.000000,1.000000],
[1.000000,1.000000,1.000000],
[-1.000000,1.000000,1.000000],
[-1.000000,-1.000000,-1.000000],
[1.000000,-1.000000,-1.000000],
[1.000000,1.000000,-1.000000],
[-1.000000,1.000000,-1.000000],
[-1.000000,-1.000000,-1.000000],
[-1.000000,1.000000,-1.000000],
[-1.000000,1.000000,1.000000],
[-1.000000,-1.000000,1.000000],
[1.000000,-1.000000,-1.000000],
[1.000000,1.000000,-1.000000],
[1.000000,1.000000,1.000000],
[1.000000,-1.000000,1.000000],
[-1.000000,-1.000000,-1.000000],
[-1.000000,-1.000000,1.000000],
[1.000000,-1.000000,1.000000],
[1.000000,-1.000000,-1.000000],
[-1.000000,1.000000,-1.000000],
[-1.000000,1.000000,1.000000],
[1.000000,1.000000,1.000000],
[1.000000,1.000000,-1.000000]
]
normals=[
[0.000000,0.000000,1.000000],
[0.000000,0.000000,1.000000],
[0.000000,0.000000,1.000000],
[0.000000,0.000000,1.000000],
[0.000000,0.000000,-1.000000],
[0.000000,0.000000,-1.000000],
[0.000000,0.000000,-1.000000],
[0.000000,0.000000,-1.000000],
[-1.000000,0.000000,0.000000],
[-1.000000,0.000000,0.000000],
[-1.000000,0.000000,0.000000],
[-1.000000,0.000000,0.000000],
[1.000000,0.000000,0.000000],
[1.000000,0.000000,0.000000],
[1.000000,0.000000,0.000000],
[1.000000,0.000000,0.000000],
[0.000000,-1.000000,0.000000],
[0.000000,-1.000000,0.000000],
[0.000000,-1.000000,0.000000],
[0.000000,-1.000000,0.000000],
[0.000000,1.000000,0.000000],
[0.000000,1.000000,0.000000],
[0.000000,1.000000,0.000000],
[0.000000,1.000000,0.000000]
]
uvs=[
[0.750000,0.250000],
[1.000000,0.250000],
[1.000000,0.000000],
[0.750000,0.000000],
[0.500000,0.250000],
[0.250000,0.250000],
[0.250000,0.000000],
[0.500000,0.000000],
[0.500000,0.000000],
[0.750000,0.000000],
[0.750000,0.250000],
[0.500000,0.250000],
[0.250000,0.500000],
[0.250000,0.250000],
[0.000000,0.250000],
[0.000000,0.500000],
[0.250000,0.500000],
[0.250000,0.250000],
[0.500000,0.250000],
[0.500000,0.500000],
[0.000000,0.000000],
[0.000000,0.250000],
[0.250000,0.250000],
[0.250000,0.000000]
]
indices=[ 0, 1, 2, 0, 2, 3, #//ground face
6, 5, 4, 7, 6, 4, #//top face
10, 9, 8, 11, 10, 8,
12, 13, 14, 12, 14, 15,
18, 17, 16, 19, 18, 16,
20, 21, 22, 20, 22, 23]
#p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, vertices=vertices, indices=indices, uvs=uvs, normals=normals)
#visualShapeId = p.createVisualShape(shapeType=p.GEOM_BOX,rgbaColor=[1,1,1,1], halfExtents=[0.5,0.5,0.5],specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=[1,1,1], vertices=vertices, indices=indices)
#visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, fileName="duck.obj")
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_BOX, halfExtents=meshScale)#MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale)
texUid = p.loadTexture("tex256.png")
batchPositions=[]
for x in range (33):
for y in range (34):
for z in range (4):
batchPositions.append([x*meshScale[0]*5.5,y*meshScale[1]*5.5,(0.5+z)*meshScale[2]*2.5])
bodyUid = p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition =[0,0,2], batchPositions=batchPositions,useMaximalCoordinates=True)
p.changeVisualShape(bodyUid,-1, textureUniqueId = texUid)
#p.syncBodyInfo()
#print("numBodies=",p.getNumBodies())
p.stopStateLogging(logId)
p.setGravity(0,0,-10)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
colors = [[1,0,0,1],[0,1,0,1],[0,0,1,1],[1,1,1,1]]
currentColor = 0
while (1):
p.stepSimulation()
#time.sleep(1./120.)
#p.getCameraImage(320,200)

2
examples/pybullet/gym/pybullet_envs/deep_mimic/DeepMimic_Optimizer.py
View File

@@ -11,7 +11,7 @@ print("parentdir=",parentdir)
from pybullet_envs.deep_mimic.env.pybullet_deep_mimic_env import PyBulletDeepMimicEnv
from pybullet_envs.deep_mimic.learning.rl_world import RLWorld
from pybullet_utils.logger import Logger
from testrl import update_world, update_timestep, build_world
from pybullet_envs.deep_mimic.testrl import update_world, update_timestep, build_world
import pybullet_utils.mpi_util as MPIUtil
args = []

1
examples/pybullet/gym/pybullet_envs/deep_mimic/env/__init__.py vendored Normal file
View File

@@ -0,0 +1 @@

10
examples/pybullet/gym/pybullet_envs/deep_mimic/env/env.py vendored
View File

@@ -1,4 +1,10 @@
from abc import ABC, abstractmethod
from abc import abstractmethod
import sys, abc
if sys.version_info >= (3, 4):
ABC = abc.ABC
else:
ABC = abc.ABCMeta('ABC', (), {})
import numpy as np
from enum import Enum
@@ -10,4 +16,4 @@ class Env(ABC):
def __init__(self, args, enable_draw):
self.enable_draw = enable_draw
return
return

2
examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py vendored
View File

@@ -27,6 +27,8 @@ class PyBulletDeepMimicEnv(Env):
if not self._isInitialized:
if self.enable_draw:
self._pybullet_client = bullet_client.BulletClient(connection_mode=p1.GUI)
#disable 'GUI' since it slows down a lot on Mac OSX and some other platforms
self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_GUI,0)
else:
self._pybullet_client = bullet_client.BulletClient()

1
examples/pybullet/gym/pybullet_envs/deep_mimic/learning/__init__.py Normal file
View File

@@ -0,0 +1 @@

2
examples/pybullet/gym/pybullet_envs/deep_mimic/learning/agent_builder.py
View File

@@ -1,6 +1,6 @@
import json
import numpy as np
from learning.ppo_agent import PPOAgent
from pybullet_envs.deep_mimic.learning.ppo_agent import PPOAgent
import pybullet_data
AGENT_TYPE_KEY = "AgentType"

2
examples/pybullet/gym/pybullet_envs/deep_mimic/learning/nets/fc_2layers_1024units.py
View File

@@ -1,5 +1,5 @@
import tensorflow as tf
import learning.tf_util as TFUtil
import pybullet_envs.deep_mimic.learning.tf_util as TFUtil
NAME = "fc_2layers_1024units"

2
examples/pybullet/gym/pybullet_envs/deep_mimic/learning/nets/net_builder.py
View File

@@ -1,4 +1,4 @@
import learning.nets.fc_2layers_1024units as fc_2layers_1024units
import pybullet_envs.deep_mimic.learning.nets.fc_2layers_1024units as fc_2layers_1024units
def build_net(net_name, input_tfs, reuse=False):
net = None

14
examples/pybullet/gym/pybullet_envs/deep_mimic/learning/pg_agent.py
View File

@@ -2,12 +2,12 @@ import numpy as np
import tensorflow as tf
import copy
from learning.tf_agent import TFAgent
from learning.solvers.mpi_solver import MPISolver
import learning.tf_util as TFUtil
import learning.nets.net_builder as NetBuilder
from learning.tf_normalizer import TFNormalizer
import learning.rl_util as RLUtil
from pybullet_envs.deep_mimic.learning.tf_agent import TFAgent
from pybullet_envs.deep_mimic.learning.solvers.mpi_solver import MPISolver
import pybullet_envs.deep_mimic.learning.tf_util as TFUtil
import pybullet_envs.deep_mimic.learning.nets.net_builder as NetBuilder
from pybullet_envs.deep_mimic.learning.tf_normalizer import TFNormalizer
import pybullet_envs.deep_mimic.learning.rl_util as RLUtil
from pybullet_utils.logger import Logger
import pybullet_utils.mpi_util as MPIUtil
import pybullet_utils.math_util as MathUtil
@@ -350,4 +350,4 @@ class PGAgent(TFAgent):
def _build_replay_buffer(self, buffer_size):
super()._build_replay_buffer(buffer_size)
self.replay_buffer.add_filter_key(self.EXP_ACTION_FLAG)
return
return

10
examples/pybullet/gym/pybullet_envs/deep_mimic/learning/ppo_agent.py
View File

@@ -2,10 +2,10 @@ import numpy as np
import copy as copy
import tensorflow as tf
from learning.pg_agent import PGAgent
from learning.solvers.mpi_solver import MPISolver
import learning.tf_util as TFUtil
import learning.rl_util as RLUtil
from pybullet_envs.deep_mimic.learning.pg_agent import PGAgent
from pybullet_envs.deep_mimic.learning.solvers.mpi_solver import MPISolver
import pybullet_envs.deep_mimic.learning.tf_util as TFUtil
import pybullet_envs.deep_mimic.learning.rl_util as RLUtil
from pybullet_utils.logger import Logger
import pybullet_utils.mpi_util as MPIUtil
import pybullet_utils.math_util as MathUtil
@@ -365,4 +365,4 @@ class PPOAgent(PGAgent):
self._actor_stepsize_ph: stepsize,
}
self.sess.run(self._actor_stepsize_update_op, feed)
return
return

20
examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_agent.py
View File

@@ -2,14 +2,20 @@ import numpy as np
import copy
import os
import time
import sys
from abc import abstractmethod
import abc
if sys.version_info >= (3, 4):
ABC = abc.ABC
else:
ABC = abc.ABCMeta('ABC', (), {})
from abc import ABC, abstractmethod
from enum import Enum
from learning.path import *
from learning.exp_params import ExpParams
from learning.normalizer import Normalizer
from learning.replay_buffer import ReplayBuffer
from pybullet_envs.deep_mimic.learning.path import *
from pybullet_envs.deep_mimic.learning.exp_params import ExpParams
from pybullet_envs.deep_mimic.learning.normalizer import Normalizer
from pybullet_envs.deep_mimic.learning.replay_buffer import ReplayBuffer
from pybullet_utils.logger import Logger
import pybullet_utils.mpi_util as MPIUtil
import pybullet_utils.math_util as MathUtil
@@ -161,7 +167,7 @@ class RLAgent(ABC):
return self._enable_training
def set_enable_training(self, enable):
print("set_enable_training!=", enable)
print("set_enable_training=", enable)
self._enable_training = enable
if (self._enable_training):
self.reset()
@@ -593,4 +599,4 @@ class RLAgent(ABC):
self.logger.log_tabular("Exp_Rate", self.exp_params_curr.rate)
self.logger.log_tabular("Exp_Noise", self.exp_params_curr.noise)
self.logger.log_tabular("Exp_Temp", self.exp_params_curr.temp)
return
return

6
examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_world.py
View File

@@ -1,7 +1,7 @@
import numpy as np
import learning.agent_builder as AgentBuilder
import learning.tf_util as TFUtil
from learning.rl_agent import RLAgent
import pybullet_envs.deep_mimic.learning.agent_builder as AgentBuilder
import pybullet_envs.deep_mimic.learning.tf_util as TFUtil
from pybullet_envs.deep_mimic.learning.rl_agent import RLAgent
from pybullet_utils.logger import Logger
import pybullet_data

1
examples/pybullet/gym/pybullet_envs/deep_mimic/learning/solvers/__init__.py Normal file
View File

@@ -0,0 +1 @@

4
examples/pybullet/gym/pybullet_envs/deep_mimic/learning/solvers/mpi_solver.py
View File

@@ -1,12 +1,12 @@
from mpi4py import MPI
import tensorflow as tf
import numpy as np
import learning.tf_util as TFUtil
import pybullet_envs.deep_mimic.learning.tf_util as TFUtil
import pybullet_utils.math_util as MathUtil
import pybullet_utils.mpi_util as MPIUtil
from pybullet_utils.logger import Logger
from learning.solvers.solver import Solver
from pybullet_envs.deep_mimic.learning.solvers.solver import Solver
class MPISolver(Solver):
CHECK_SYNC_ITERS = 1000

9
examples/pybullet/gym/pybullet_envs/deep_mimic/learning/solvers/solver.py
View File

@@ -1,4 +1,9 @@
from abc import ABC, abstractmethod
from abc import abstractmethod
import sys, abc
if sys.version_info >= (3, 4):
ABC = abc.ABC
else:
ABC = abc.ABCMeta('ABC', (), {})
class Solver(ABC):
def __init__(self, vars):
@@ -7,4 +12,4 @@ class Solver(ABC):
@abstractmethod
def update(self, grads):
pass
pass

4
examples/pybullet/gym/pybullet_envs/deep_mimic/learning/tf_agent.py
View File

@@ -2,9 +2,9 @@ import numpy as np
import tensorflow as tf
from abc import abstractmethod
from learning.rl_agent import RLAgent
from pybullet_envs.deep_mimic.learning.rl_agent import RLAgent
from pybullet_utils.logger import Logger
from learning.tf_normalizer import TFNormalizer
from pybullet_envs.deep_mimic.learning.tf_normalizer import TFNormalizer
class TFAgent(RLAgent):
RESOURCE_SCOPE = 'resource'

2
examples/pybullet/gym/pybullet_envs/deep_mimic/learning/tf_normalizer.py
View File

@@ -1,7 +1,7 @@
import numpy as np
import copy
import tensorflow as tf
from learning.normalizer import Normalizer
from pybullet_envs.deep_mimic.learning.normalizer import Normalizer
class TFNormalizer(Normalizer):

2
examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py
View File

@@ -6,7 +6,7 @@ os.sys.path.insert(0,parentdir)
print("parentdir=",parentdir)
import json
from pybullet_envs.deep_mimic.learning.rl_world import RLWorld
from learning.ppo_agent import PPOAgent
from pybullet_envs.deep_mimic.learning.ppo_agent import PPOAgent
import pybullet_data
from pybullet_utils.arg_parser import ArgParser

56
examples/pybullet/pybullet.c
View File

@@ -1501,9 +1501,32 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
int minimumSolverIslandSize = -1;
int physicsClientId = 0;
static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching", "restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "solverResidualThreshold", "contactSlop", "enableSAT", "constraintSolverType", "globalCFM", "minimumSolverIslandSize", "physicsClientId", NULL};
static char* kwlist[] = {"fixedTimeStep",
"numSolverIterations",
"useSplitImpulse",
"splitImpulsePenetrationThreshold",
"numSubSteps",
"collisionFilterMode",
"contactBreakingThreshold",
"maxNumCmdPer1ms",
"enableFileCaching",
"restitutionVelocityThreshold",
"erp",
"contactERP",
"frictionERP",
"enableConeFriction",
"deterministicOverlappingPairs",
"allowedCcdPenetration",
"jointFeedbackMode",
"solverResidualThreshold",
"contactSlop",
"enableSAT",
"constraintSolverType",
"globalCFM",
"minimumSolverIslandSize",
"physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiiddi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
&collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &minimumSolverIslandSize, &physicsClientId))
{
return NULL;
@@ -7656,18 +7679,19 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
PyObject* linkJointAxisObj = 0;
PyObject* linkInertialFramePositionObj = 0;
PyObject* linkInertialFrameOrientationObj = 0;
PyObject* objBatchPositions = 0;
static char* kwlist[] = {
"baseMass", "baseCollisionShapeIndex", "baseVisualShapeIndex", "basePosition", "baseOrientation",
"baseInertialFramePosition", "baseInertialFrameOrientation", "linkMasses", "linkCollisionShapeIndices",
"linkVisualShapeIndices", "linkPositions", "linkOrientations", "linkInertialFramePositions", "linkInertialFrameOrientations", "linkParentIndices",
"linkJointTypes", "linkJointAxis", "useMaximalCoordinates", "flags", "physicsClientId", NULL};
"linkJointTypes", "linkJointAxis", "useMaximalCoordinates", "flags", "batchPositions", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOOOOOOOOOOOOOiii", kwlist,
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOOOOOOOOOOOOOiiOi", kwlist,
&baseMass, &baseCollisionShapeIndex, &baseVisualShapeIndex, &basePosObj, &baseOrnObj,
&baseInertialFramePositionObj, &baseInertialFrameOrientationObj, &linkMassesObj, &linkCollisionShapeIndicesObj,
&linkVisualShapeIndicesObj, &linkPositionsObj, &linkOrientationsObj, &linkInertialFramePositionObj, &linkInertialFrameOrientationObj, &linkParentIndicesObj,
&linkJointTypesObj, &linkJointAxisObj, &useMaximalCoordinates, &flags, &physicsClientId))
&linkJointTypesObj, &linkJointAxisObj, &useMaximalCoordinates, &flags, &objBatchPositions, &physicsClientId))
{
return NULL;
}
@@ -7690,6 +7714,7 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
int numLinkJoinAxis = linkJointAxisObj ? PySequence_Size(linkJointAxisObj) : 0;
int numLinkInertialFramePositions = linkInertialFramePositionObj ? PySequence_Size(linkInertialFramePositionObj) : 0;
int numLinkInertialFrameOrientations = linkInertialFrameOrientationObj ? PySequence_Size(linkInertialFrameOrientationObj) : 0;
int numBatchPositions = objBatchPositions ? PySequence_Size(objBatchPositions) : 0;
PyObject* seqLinkMasses = linkMassesObj ? PySequence_Fast(linkMassesObj, "expected a sequence") : 0;
PyObject* seqLinkCollisionShapes = linkCollisionShapeIndicesObj ? PySequence_Fast(linkCollisionShapeIndicesObj, "expected a sequence") : 0;
@@ -7702,6 +7727,8 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
PyObject* seqLinkInertialFramePositions = linkInertialFramePositionObj ? PySequence_Fast(linkInertialFramePositionObj, "expected a sequence") : 0;
PyObject* seqLinkInertialFrameOrientations = linkInertialFrameOrientationObj ? PySequence_Fast(linkInertialFrameOrientationObj, "expected a sequence") : 0;
PyObject* seqBatchPositions = objBatchPositions ? PySequence_Fast(objBatchPositions, "expected a sequence") : 0;
if ((numLinkMasses == numLinkCollisionShapes) &&
(numLinkMasses == numLinkVisualShapes) &&
(numLinkMasses == numLinkPositions) &&
@@ -7728,6 +7755,18 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
baseIndex = b3CreateMultiBodyBase(commandHandle, baseMass, baseCollisionShapeIndex, baseVisualShapeIndex, basePosition, baseOrientation, baseInertialFramePosition, baseInertialFrameOrientation);
if (numBatchPositions > 0)
{
double* batchPositions = malloc(sizeof(double) * 3 * numBatchPositions);
for (i = 0; i < numBatchPositions; i++)
{
pybullet_internalGetVector3FromSequence(seqBatchPositions, i, &batchPositions[3*i]);
}
b3CreateMultiBodySetBatchPositions(sm, commandHandle, batchPositions, numBatchPositions);
free(batchPositions);
}
for (i = 0; i < numLinkMasses; i++)
{
double linkMass = pybullet_internalGetFloatFromSequence(seqLinkMasses, i);
@@ -7742,7 +7781,7 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
int linkJointType;
pybullet_internalGetVector3FromSequence(seqLinkInertialFramePositions, i, linkInertialFramePosition);
pybullet_internalGetVector4FromSequence(linkInertialFrameOrientationObj, i, linkInertialFrameOrientation);
pybullet_internalGetVector4FromSequence(seqLinkInertialFrameOrientations, i, linkInertialFrameOrientation);
pybullet_internalGetVector3FromSequence(seqLinkPositions, i, linkPosition);
pybullet_internalGetVector4FromSequence(seqLinkOrientations, i, linkOrientation);
pybullet_internalGetVector3FromSequence(seqLinkJoinAxis, i, linkJointAxis);
@@ -7782,6 +7821,8 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
Py_DECREF(seqLinkInertialFramePositions);
if (seqLinkInertialFrameOrientations)
Py_DECREF(seqLinkInertialFrameOrientations);
if (seqBatchPositions)
Py_DECREF(seqBatchPositions);
if (useMaximalCoordinates > 0)
{
@@ -7822,7 +7863,8 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
Py_DECREF(seqLinkInertialFramePositions);
if (seqLinkInertialFrameOrientations)
Py_DECREF(seqLinkInertialFrameOrientations);
if (seqBatchPositions)
Py_DECREF(seqBatchPositions);
PyErr_SetString(SpamError, "All link arrays need to be same size.");
return NULL;
}