Merge pull request #1445 from erwincoumans/master

pybullet: move RGB,depth, segmentation mask preview windows
This commit is contained in:
erwincoumans
2017-11-18 20:35:04 -08:00
committed by GitHub
11 changed files with 51 additions and 297 deletions

View File

@@ -4,7 +4,7 @@
struct Common2dCanvasInterface
{
virtual ~Common2dCanvasInterface() {}
virtual int createCanvas(const char* canvasName, int width, int height)=0;
virtual int createCanvas(const char* canvasName, int width, int height, int xPos, int yPos)=0;
virtual void destroyCanvas(int canvasId)=0;
virtual void setPixel(int canvasId, int x, int y, unsigned char red, unsigned char green,unsigned char blue, unsigned char alpha)=0;
virtual void getPixel(int canvasId, int x, int y, unsigned char& red, unsigned char& green,unsigned char& blue, unsigned char& alpha)=0;

View File

@@ -732,12 +732,11 @@ struct QuickCanvas : public Common2dCanvasInterface
MyGraphWindow* m_gw[MAX_GRAPH_WINDOWS];
GraphingTexture* m_gt[MAX_GRAPH_WINDOWS];
int m_curNumGraphWindows;
int m_curXpos;
QuickCanvas(GL3TexLoader* myTexLoader)
:m_myTexLoader(myTexLoader),
m_curNumGraphWindows(0),
m_curXpos(0)
m_curNumGraphWindows(0)
{
for (int i=0;i<MAX_GRAPH_WINDOWS;i++)
{
@@ -746,7 +745,7 @@ struct QuickCanvas : public Common2dCanvasInterface
}
}
virtual ~QuickCanvas() {}
virtual int createCanvas(const char* canvasName, int width, int height)
virtual int createCanvas(const char* canvasName, int width, int height, int xPos, int yPos)
{
if (m_curNumGraphWindows<MAX_GRAPH_WINDOWS)
{
@@ -761,9 +760,8 @@ struct QuickCanvas : public Common2dCanvasInterface
MyGraphInput input(gui2->getInternalData());
input.m_width=width;
input.m_height=height;
input.m_xPos = m_curXpos;//GUI will clamp it to the right//300;
m_curXpos+=width+20;
input.m_yPos = 10000;//GUI will clamp it to bottom
input.m_xPos = xPos;
input.m_yPos = yPos;
input.m_name=canvasName;
input.m_texName = canvasName;
m_gt[slot] = new GraphingTexture;
@@ -778,7 +776,6 @@ struct QuickCanvas : public Common2dCanvasInterface
}
virtual void destroyCanvas(int canvasId)
{
m_curXpos = 0;
btAssert(canvasId>=0);
delete m_gt[canvasId];
m_gt[canvasId] = 0;
@@ -1329,7 +1326,7 @@ void OpenGLExampleBrowser::update(float deltaTime)
float camPos[3];
s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraPosition(camPos);
s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraTargetPosition(camTarget);
sprintf(msg,"camPos=%f,%f,%f, dist=%f, pitch=%f, yaw=%f,target=%f,%f,%f", camPos[0],camPos[1],camPos[2],camDist,pitch,yaw,camTarget[0],camTarget[1],camTarget[2]);
sprintf(msg,"camTargetPos=%2.2f,%2.2f,%2.2f, dist=%2.2f, pitch=%2.2f, yaw=%2.2f", camPos[0],camPos[1],camPos[2],camDist,pitch,yaw);
gui2->setStatusBarMessage(msg, true);
}

View File

@@ -142,7 +142,7 @@ void RaytracerPhysicsSetup::initPhysics()
if (m_internalData->m_canvas)
{
m_internalData->m_canvasIndex = m_internalData->m_canvas->createCanvas("raytracer",m_internalData->m_width,m_internalData->m_height);
m_internalData->m_canvasIndex = m_internalData->m_canvas->createCanvas("raytracer",m_internalData->m_width,m_internalData->m_height, 15,55);
for (int i=0;i<m_internalData->m_width;i++)
{
for (int j=0;j<m_internalData->m_height;j++)

View File

@@ -78,7 +78,7 @@ TimeSeriesCanvas::TimeSeriesCanvas(struct Common2dCanvasInterface* canvasInterfa
if (canvasInterface)
{
m_internalData->m_canvasIndex = m_internalData->m_canvasInterface->createCanvas(windowTitle,m_internalData->m_width,m_internalData->m_height);
m_internalData->m_canvasIndex = m_internalData->m_canvasInterface->createCanvas(windowTitle,m_internalData->m_width,m_internalData->m_height,20,50);
}
}

View File

@@ -28,7 +28,7 @@ struct TestCanvasInterface2 : public Common2dCanvasInterface
virtual ~TestCanvasInterface2()
{}
virtual int createCanvas(const char* canvasName, int width, int height)
virtual int createCanvas(const char* canvasName, int width, int height,int posX,int posY)
{
return 0;
}

View File

@@ -22,8 +22,9 @@ struct MyMotorInfo2
int m_qIndex;
};
static int camVisualizerWidth = 320;//1024/3;
static int camVisualizerHeight = 240;//768/3;
static int camVisualizerWidth = 228;//1024/3;
static int camVisualizerHeight = 192;//768/3;
enum CustomCommands
{
@@ -758,10 +759,10 @@ void PhysicsClientExample::initPhysics()
m_canvas = m_guiHelper->get2dCanvasInterface();
if (m_canvas)
{
m_canvasRGBIndex = m_canvas->createCanvas("Synthetic Camera RGB data",camVisualizerWidth, camVisualizerHeight);
m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",camVisualizerWidth, camVisualizerHeight);
m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",camVisualizerWidth, camVisualizerHeight);
m_canvasRGBIndex = m_canvas->createCanvas("Synthetic Camera RGB data",camVisualizerWidth, camVisualizerHeight, 8,55);
m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",camVisualizerWidth, camVisualizerHeight,8,75+camVisualizerHeight);
m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",camVisualizerWidth, camVisualizerHeight,8,95+camVisualizerHeight*2);
for (int i=0;i<camVisualizerWidth;i++)
{

View File

@@ -60,9 +60,6 @@
#endif
//@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
btVector3 gLastPickPos(0, 0, 0);
int gInternalSimFlags = 0;
bool gResetSimulation = 0;
@@ -1604,49 +1601,6 @@ struct PhysicsServerCommandProcessorInternalData
m_userVisualShapeHandles.exitHandles();
m_userVisualShapeHandles.initHandles();
#if 0
btAlignedObjectArray<int> bla;
for (int i=0;i<1024;i++)
{
int handle = allocHandle();
bla.push_back(handle);
InternalBodyHandle* body = getHandle(handle);
InternalBodyData* body2 = body;
}
for (int i=0;i<bla.size();i++)
{
freeHandle(bla[i]);
}
bla.resize(0);
for (int i=0;i<1024;i++)
{
int handle = allocHandle();
bla.push_back(handle);
InternalBodyHandle* body = getHandle(handle);
InternalBodyData* body2 = body;
}
for (int i=0;i<bla.size();i++)
{
freeHandle(bla[i]);
}
bla.resize(0);
for (int i=0;i<1024;i++)
{
int handle = allocHandle();
bla.push_back(handle);
InternalBodyHandle* body = getHandle(handle);
InternalBodyData* body2 = body;
}
for (int i=0;i<bla.size();i++)
{
freeHandle(bla[i]);
}
#endif
}
btInverseDynamics::MultiBodyTree* findOrCreateTree(btMultiBody* multiBody)
@@ -2747,11 +2701,8 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
bool loadOk = u2b.loadURDF(fileName, useFixedBase);
if (loadOk)
{
#if 1
btTransform rootTrans;
rootTrans.setOrigin(pos);
rootTrans.setRotation(orn);
@@ -2762,188 +2713,11 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
if (m_data->m_sdfRecentLoadedBodies.size()==1)
{
*bodyUniqueIdPtr = m_data->m_sdfRecentLoadedBodies[0];
}
m_data->m_sdfRecentLoadedBodies.clear();
}
return ok;
#else
//get a body index
int bodyUniqueId = m_data->m_bodyHandles.allocHandle();
if (bodyUniqueIdPtr)
*bodyUniqueIdPtr= bodyUniqueId;
//quick prototype of 'save world' for crude world editing
{
SaveWorldObjectData sd;
sd.m_fileName = fileName;
sd.m_bodyUniqueIds.push_back(bodyUniqueId);
m_data->m_saveWorldBodyData.push_back(sd);
}
u2b.setBodyUniqueId(bodyUniqueId);
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
{
btScalar mass = 0;
bodyHandle->m_rootLocalInertialFrame.setIdentity();
btVector3 localInertiaDiagonal(0,0,0);
int urdfLinkIndex = u2b.getRootLinkIndex();
u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,bodyHandle->m_rootLocalInertialFrame);
}
if (m_data->m_verboseOutput)
{
b3Printf("loaded %s OK!", fileName);
}
btTransform tr;
tr.setIdentity();
tr.setOrigin(pos);
tr.setRotation(orn);
//int rootLinkIndex = u2b.getRootLinkIndex();
// printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_data->m_guiHelper);
flags |= URDF_ORDER_TYPED_CONSTRAINT;
ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags);
for (int i=0;i<u2b.getNumAllocatedCollisionShapes();i++)
{
btCollisionShape* shape =u2b.getAllocatedCollisionShape(i);
m_data->m_collisionShapes.push_back(shape);
}
btMultiBody* mb = creation.getBulletMultiBody();
btRigidBody* rb = creation.getRigidBody();
bodyHandle->m_bodyName = u2b.getBodyName();
if (useMultiBody)
{
if (mb)
{
mb->setUserIndex2(bodyUniqueId);
bodyHandle->m_multiBody = mb;
if (flags & URDF_USE_SELF_COLLISION)
{
mb->setHasSelfCollision(true);
}
createJointMotors(mb);
#ifdef B3_ENABLE_TINY_AUDIO
{
SDFAudioSource audioSource;
int urdfRootLink = u2b.getRootLinkIndex();//LinkIndex = creation.m_mb2urdfLink[-1];
if (u2b.getLinkAudioSource(urdfRootLink,audioSource))
{
int flags = mb->getBaseCollider()->getCollisionFlags();
mb->getBaseCollider()->setCollisionFlags(flags | btCollisionObject::CF_HAS_COLLISION_SOUND_TRIGGER);
audioSource.m_userIndex = m_data->m_soundEngine.loadWavFile(audioSource.m_uri.c_str());
if (audioSource.m_userIndex>=0)
{
bodyHandle->m_audioSources.insert(-1, audioSource);
}
}
}
#endif
//serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire
UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil;
m_data->m_urdfLinkNameMapper.push_back(util);
util->m_mb = mb;
for (int i = 0; i < bufferSizeInBytes; i++)
{
bufferServerToClient[i] = 0;//0xcc;
}
util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient);
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(),0);
util->m_memSerializer->startSerialization();
bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
for (int i=0;i<mb->getNumLinks();i++)
{
int link=i;
//disable serialization of the collision objects
util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider,0);
int urdfLinkIndex = creation.m_mb2urdfLink[i];
btScalar mass;
btVector3 localInertiaDiagonal(0,0,0);
btTransform localInertialFrame;
u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame);
bodyHandle->m_linkLocalInertialFrames.push_back(localInertialFrame);
std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
m_data->m_strings.push_back(linkName);
util->m_memSerializer->registerNameForPointer(linkName->c_str(),linkName->c_str());
mb->getLink(i).m_linkName = linkName->c_str();
std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex).c_str());
m_data->m_strings.push_back(jointName);
util->m_memSerializer->registerNameForPointer(jointName->c_str(),jointName->c_str());
mb->getLink(i).m_jointName = jointName->c_str();
#ifdef B3_ENABLE_TINY_AUDIO
{
SDFAudioSource audioSource;
int urdfLinkIndex = creation.m_mb2urdfLink[link];
if (u2b.getLinkAudioSource(urdfLinkIndex,audioSource))
{
int flags = mb->getLink(link).m_collider->getCollisionFlags();
mb->getLink(i).m_collider->setCollisionFlags(flags | btCollisionObject::CF_HAS_COLLISION_SOUND_TRIGGER);
audioSource.m_userIndex = m_data->m_soundEngine.loadWavFile(audioSource.m_uri.c_str());
if (audioSource.m_userIndex>=0)
{
bodyHandle->m_audioSources.insert(link, audioSource);
}
}
}
#endif
}
std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
m_data->m_strings.push_back(baseName);
mb->setBaseName(baseName->c_str());
util->m_memSerializer->registerNameForPointer(baseName->c_str(),baseName->c_str());
int len = mb->calculateSerializeBufferSize();
btChunk* chunk = util->m_memSerializer->allocate(len,1);
const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer);
util->m_memSerializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
return true;
} else
{
b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later.");
return false;
}
} else
{
if (rb)
{
bodyHandle->m_rigidBody = rb;
rb->setUserIndex2(bodyUniqueId);
return true;
}
}
#endif
}
return false;
}
@@ -3017,14 +2791,7 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
for (int i=0;i<bodyHandle->m_rigidBodyJoints.size();i++)
{
const btGeneric6DofSpring2Constraint* con = bodyHandle->m_rigidBodyJoints.at(i);
#if 0
const btRigidBody& bodyA = con->getRigidBodyA();
const btRigidBody& bodyB = con->getRigidBodyB();
int len = bodyA.calculateSerializeBufferSize();
btChunk* chunk = util->m_memSerializer->allocate(len,1);
const char* structType = bodyA.serialize(chunk->m_oldPtr, util->m_memSerializer);
util->m_memSerializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)&bodyA);
#endif
util->m_memSerializer->registerNameForPointer(con,bodyHandle->m_rigidBodyJointNames[i].c_str());
util->m_memSerializer->registerNameForPointer(&con->getRigidBodyB(),bodyHandle->m_rigidBodyLinkNames[i].c_str());
@@ -3037,27 +2804,7 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
}
streamSizeInBytes = util->m_memSerializer->getCurrentBufferSize();
#if 0
util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(),0);
if (mb->getBaseName())
{
util->m_memSerializer->registerNameForPointer(mb->getBaseName(),mb->getBaseName());
}
bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
for (int i=0;i<mb->getNumLinks();i++)
{
//disable serialization of the collision objects
util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider,0);
util->m_memSerializer->registerNameForPointer(mb->getLink(i).m_linkName,mb->getLink(i).m_linkName);
util->m_memSerializer->registerNameForPointer(mb->getLink(i).m_jointName,mb->getLink(i).m_jointName);
}
util->m_memSerializer->registerNameForPointer(mb->getBaseName(),mb->getBaseName());
int len = mb->calculateSerializeBufferSize();
btChunk* chunk = util->m_memSerializer->allocate(len,1);
const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer);
util->m_memSerializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
streamSizeInBytes = util->m_memSerializer->getCurrentBufferSize();
#endif
}
}
@@ -8847,7 +8594,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
{
btVector3 pickPos = rayCallback.m_hitPointWorld;
gLastPickPos = pickPos;
btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject);
if (body)
{

View File

@@ -30,8 +30,8 @@ bool gActivedVRRealTimeSimulation = false;
bool gEnableSyncPhysicsRendering= true;
bool gEnableUpdateDebugDrawLines = true;
static int gCamVisualizerWidth = 320;
static int gCamVisualizerHeight = 240;
static int gCamVisualizerWidth = 228;
static int gCamVisualizerHeight = 192;
static bool gEnableDefaultKeyboardShortcuts = true;
static bool gEnableDefaultMousePicking = true;
@@ -1751,9 +1751,9 @@ void PhysicsServerExample::initPhysics()
{
m_canvasRGBIndex = m_canvas->createCanvas("Synthetic Camera RGB data",gCamVisualizerWidth, gCamVisualizerHeight);
m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",gCamVisualizerWidth, gCamVisualizerHeight);
m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",gCamVisualizerWidth, gCamVisualizerHeight);
m_canvasRGBIndex = m_canvas->createCanvas("Synthetic Camera RGB data",gCamVisualizerWidth, gCamVisualizerHeight, 8,55);
m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",gCamVisualizerWidth, gCamVisualizerHeight,8,75+gCamVisualizerHeight);
m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",gCamVisualizerWidth, gCamVisualizerHeight,8,95+gCamVisualizerHeight*2);
for (int i=0;i<gCamVisualizerWidth;i++)
{
@@ -1904,7 +1904,7 @@ void PhysicsServerExample::updateGraphics()
{
if (m_canvasRGBIndex<0)
{
m_canvasRGBIndex = m_canvas->createCanvas("Synthetic Camera RGB data",gCamVisualizerWidth, gCamVisualizerHeight);
m_canvasRGBIndex = m_canvas->createCanvas("Synthetic Camera RGB data",gCamVisualizerWidth, gCamVisualizerHeight, 8,55);
}
} else
{
@@ -1922,7 +1922,7 @@ void PhysicsServerExample::updateGraphics()
{
if (m_canvasDepthIndex<0)
{
m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",gCamVisualizerWidth, gCamVisualizerHeight);
m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",gCamVisualizerWidth, gCamVisualizerHeight,8,75+gCamVisualizerHeight);
}
} else
{
@@ -1940,7 +1940,7 @@ void PhysicsServerExample::updateGraphics()
{
if (m_canvasSegMaskIndex<0)
{
m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",gCamVisualizerWidth, gCamVisualizerHeight);
m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",gCamVisualizerWidth, gCamVisualizerHeight,8,95+gCamVisualizerHeight*2);
}
} else
{
@@ -2096,8 +2096,9 @@ void PhysicsServerExample::updateGraphics()
int startSegIndex = m_multiThreadedHelper->m_startPixelIndex;
int endSegIndex = startSegIndex + (*m_multiThreadedHelper->m_numPixelsCopied);
btScalar frustumZNear = m_multiThreadedHelper->m_projectionMatrix[14]/(m_multiThreadedHelper->m_projectionMatrix[10]-1);
btScalar frustumZFar = 20;//m_multiThreadedHelper->m_projectionMatrix[14]/(m_multiThreadedHelper->m_projectionMatrix[10]+1);
//btScalar frustumZNear = m_multiThreadedHelper->m_projectionMatrix[14]/(m_multiThreadedHelper->m_projectionMatrix[10]-1);
//btScalar frustumZFar = m_multiThreadedHelper->m_projectionMatrix[14]/(m_multiThreadedHelper->m_projectionMatrix[10]+1);
for (int i=0;i<gCamVisualizerWidth;i++)
{
@@ -2131,17 +2132,16 @@ void PhysicsServerExample::updateGraphics()
if (depthValue>-1e20)
{
int rgb = 0;
btScalar minDepthValue = 0.98;//todo: compute more reasonably min/max depth range
btScalar maxDepthValue = 1;
btScalar frustumZNear = 0.1;
btScalar frustumZFar = 30;
btScalar minDepthValue = frustumZNear;//todo: compute more reasonably min/max depth range
btScalar maxDepthValue = frustumZFar;
if (maxDepthValue!=minDepthValue)
{
rgb = (depthValue-minDepthValue)*(255. / (btFabs(maxDepthValue-minDepthValue)));
if (rgb<0 || rgb>255)
{
//printf("rgb=%d\n",rgb);
}
}
float depth = depthValue;
btScalar linearDepth = 255.*(2.0 * frustumZNear) / (frustumZFar + frustumZNear - depth * (frustumZFar - frustumZNear));
btClamp(linearDepth, btScalar(0),btScalar(255));
rgb = linearDepth;
m_canvas->setPixel(m_canvasDepthIndex,i,j,
rgb,
rgb,

View File

@@ -20,4 +20,4 @@ from __future__ import print_function
from . import train_ppo
from . import utility
from . import visualize
from . import visualize_ppo

View File

@@ -119,6 +119,15 @@ def pybullet_racecar():
return locals()
def pybullet_humanoid():
locals().update(default())
randomizer = (minitaur_env_randomizer.MinitaurEnvRandomizer())
env = 'HumanoidBulletEnv-v0'
max_length = 1000
steps = 3e7 # 30M
return locals()
def pybullet_minitaur():
"""Configuration specific to minitaur_gym_env.MinitaurBulletEnv class."""
locals().update(default())

View File

@@ -441,7 +441,7 @@ print("-----")
setup(
name = 'pybullet',
version='1.6.6',
version='1.6.9',
description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
url='https://github.com/bulletphysics/bullet3',