From 0818112ede6aa9f40a48d6484a0c184e3285ac6d Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 28 Jan 2019 16:21:52 -0800 Subject: [PATCH 1/4] fix some gym envs --- .../pybullet/gym/pybullet_envs/env_bases.py | 3 +- .../gym/pybullet_envs/examples/laikago.py | 80 +++++++++---------- .../gym/pybullet_envs/examples/testBike.py | 2 +- .../minitaur/envs/minitaur_gym_env_example.py | 7 +- .../minitaur_raibert_controller_example.py | 7 ++ .../envs/minitaur_reactive_env_example.py | 10 +++ .../pybullet/gym/pybullet_envs/robot_bases.py | 4 +- 7 files changed, 65 insertions(+), 48 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index 24d903a7d..861363348 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -69,7 +69,7 @@ class MJCFBaseBulletEnv(gym.Env): self.potential = self.robot.calc_potential() return s - def render(self, mode='human'): + def render(self, mode='human', close=False): if mode == "human": self.isRender = True if mode != "rgb_array": @@ -125,7 +125,6 @@ class MJCFBaseBulletEnv(gym.Env): _render = render _reset = reset _seed = seed - _step = step class Camera: def __init__(self): diff --git a/examples/pybullet/gym/pybullet_envs/examples/laikago.py b/examples/pybullet/gym/pybullet_envs/examples/laikago.py index 9317d2085..b256f2a04 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/laikago.py +++ b/examples/pybullet/gym/pybullet_envs/examples/laikago.py @@ -10,23 +10,23 @@ plane = p.loadURDF("plane.urdf") p.setGravity(0,0,-9.8) p.setTimeStep(1./500) #p.setDefaultContactERP(0) -#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS +#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS urdfFlags = p.URDF_USE_SELF_COLLISION quadruped = p.loadURDF("laikago/laikago.urdf",[0,0,.5],[0,0.5,0.5,0], flags = urdfFlags,useFixedBase=False) #enable collision between lower legs for j in range (p.getNumJoints(quadruped)): - print(p.getJointInfo(quadruped,j)) + print(p.getJointInfo(quadruped,j)) #2,5,8 and 11 are the lower legs lower_legs = [2,5,8,11] for l0 in lower_legs: - for l1 in lower_legs: - if (l1>l0): - enableCollision = 1 - print("collision for pair",l0,l1, p.getJointInfo(quadruped,l0)[12],p.getJointInfo(quadruped,l1)[12], "enabled=",enableCollision) - p.setCollisionFilterPair(quadruped, quadruped, 2,5,enableCollision) + for l1 in lower_legs: + if (l1>l0): + enableCollision = 1 + print("collision for pair",l0,l1, p.getJointInfo(quadruped,l0)[12],p.getJointInfo(quadruped,l1)[12], "enabled=",enableCollision) + p.setCollisionFilterPair(quadruped, quadruped, 2,5,enableCollision) jointIds=[] paramIds=[] @@ -35,9 +35,9 @@ jointDirections=[-1,1,1,1,1,1,-1,1,1,1,1,1] jointAngles=[0,0,0,0,0,0,0,0,0,0,0,0] for i in range (4): - jointOffsets.append(0) - jointOffsets.append(-0.7) - jointOffsets.append(0.7) + jointOffsets.append(0) + jointOffsets.append(-0.7) + jointOffsets.append(0.7) maxForceId = p.addUserDebugParameter("maxForce",0,100,20) @@ -50,35 +50,35 @@ for j in range (p.getNumJoints(quadruped)): if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): jointIds.append(j) - + p.getCameraImage(480,320) p.setRealTimeSimulation(0) joints=[] with open(pd.getDataPath()+"/laikago/data1.txt","r") as filestream: - for line in filestream: - print("line=",line) - maxForce = p.readUserDebugParameter(maxForceId) - currentline = line.split(",") - #print (currentline) - #print("-----") - frame = currentline[0] - t = currentline[1] - #print("frame[",frame,"]") - joints=currentline[2:14] - #print("joints=",joints) - for j in range (12): - targetPos = float(joints[j]) - p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce) - p.stepSimulation() - for lower_leg in lower_legs: - #print("points for ", quadruped, " link: ", lower_leg) - pts = p.getContactPoints(quadruped,-1, lower_leg) - #print("num points=",len(pts)) - #for pt in pts: - # print(pt[9]) - time.sleep(1./500.) + for line in filestream: + print("line=",line) + maxForce = p.readUserDebugParameter(maxForceId) + currentline = line.split(",") + #print (currentline) + #print("-----") + frame = currentline[0] + t = currentline[1] + #print("frame[",frame,"]") + joints=currentline[2:14] + #print("joints=",joints) + for j in range (12): + targetPos = float(joints[j]) + p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce) + p.stepSimulation() + for lower_leg in lower_legs: + #print("points for ", quadruped, " link: ", lower_leg) + pts = p.getContactPoints(quadruped,-1, lower_leg) + #print("num points=",len(pts)) + #for pt in pts: + # print(pt[9]) + time.sleep(1./500.) for j in range (p.getNumJoints(quadruped)): @@ -95,10 +95,10 @@ for j in range (p.getNumJoints(quadruped)): p.setRealTimeSimulation(1) while (1): - - for i in range(len(paramIds)): - c = paramIds[i] - targetPos = p.readUserDebugParameter(c) - maxForce = p.readUserDebugParameter(maxForceId) - p.setJointMotorControl2(quadruped,jointIds[i],p.POSITION_CONTROL,jointDirections[i]*targetPos+jointOffsets[i], force=maxForce) - + + for i in range(len(paramIds)): + c = paramIds[i] + targetPos = p.readUserDebugParameter(c) + maxForce = p.readUserDebugParameter(maxForceId) + p.setJointMotorControl2(quadruped,jointIds[i],p.POSITION_CONTROL,jointDirections[i]*targetPos+jointOffsets[i], force=maxForce) + diff --git a/examples/pybullet/gym/pybullet_envs/examples/testBike.py b/examples/pybullet/gym/pybullet_envs/examples/testBike.py index b62a8668e..e36922fdc 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/testBike.py +++ b/examples/pybullet/gym/pybullet_envs/examples/testBike.py @@ -24,7 +24,7 @@ for i in range (1): p.setJointMotorControl2(bike,1,p.VELOCITY_CONTROL,targetVelocity=5, force=0) p.setJointMotorControl2(bike,2,p.VELOCITY_CONTROL,targetVelocity=15, force=20) - p.changeDynamics(plane,-1, mass=20,lateralFriction=1, linearDamping=0, angularDamping=0) + p.changeDynamics(plane,-1, mass=0,lateralFriction=1, linearDamping=0, angularDamping=0) p.changeDynamics(bike,1,lateralFriction=1,linearDamping=0, angularDamping=0) p.changeDynamics(bike,2,lateralFriction=1,linearDamping=0, angularDamping=0) #p.resetJointState(bike,1,0,100) diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env_example.py index 197560cfd..c05022648 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env_example.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env_example.py @@ -4,10 +4,11 @@ import csv import math - -import os, inspect +import os +import inspect currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) -parentdir = os.path.dirname(os.path.dirname(currentdir)) +parentdir = os.path.dirname(os.path.dirname(os.path.dirname(currentdir))) +print("parentdir=",parentdir) os.sys.path.insert(0,parentdir) import argparse diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_raibert_controller_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_raibert_controller_example.py index 90a89a3ac..87958dde5 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_raibert_controller_example.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_raibert_controller_example.py @@ -5,6 +5,13 @@ from __future__ import absolute_import from __future__ import division from __future__ import print_function +import os +import inspect +currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) +parentdir = os.path.dirname(os.path.dirname(os.path.dirname(currentdir))) +os.sys.path.insert(0,parentdir) + + import tensorflow as tf from pybullet_envs.minitaur.envs import minitaur_raibert_controller from pybullet_envs.minitaur.envs import minitaur_gym_env diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env_example.py index 5057d9cf4..2c2be7106 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env_example.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env_example.py @@ -6,11 +6,21 @@ from __future__ import print_function import os import time + +import inspect +currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) +parentdir = os.path.dirname(os.path.dirname(os.path.dirname(currentdir))) +print("parentdir=",parentdir) +os.sys.path.insert(0,parentdir) + + + import tensorflow as tf from pybullet_envs.minitaur.agents.scripts import utility import pybullet_data from pybullet_envs.minitaur.envs import simple_ppo_agent + flags = tf.app.flags FLAGS = tf.app.flags.FLAGS LOG_DIR = os.path.join(pybullet_data.getDataPath(), "policies/ppo/minitaur_reactive_env") diff --git a/examples/pybullet/gym/pybullet_envs/robot_bases.py b/examples/pybullet/gym/pybullet_envs/robot_bases.py index 40c104999..18fd3defe 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_bases.py +++ b/examples/pybullet/gym/pybullet_envs/robot_bases.py @@ -22,9 +22,9 @@ class XmlBasedRobot: self.robot_body = None high = np.ones([action_dim]) - self.action_space = gym.spaces.Box(-high, high, dtype=np.float32) + self.action_space = gym.spaces.Box(-high, high) high = np.inf * np.ones([obs_dim]) - self.observation_space = gym.spaces.Box(-high, high, dtype=np.float32) + self.observation_space = gym.spaces.Box(-high, high) #self.model_xml = model_xml self.robot_name = robot_name From 63683e8f02f68e277193ee4309ff9ea06f2a110a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 28 Jan 2019 16:24:44 -0800 Subject: [PATCH 2/4] fix Issue 2039 --- examples/pybullet/gym/pybullet_envs/robot_bases.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/pybullet/gym/pybullet_envs/robot_bases.py b/examples/pybullet/gym/pybullet_envs/robot_bases.py index 18fd3defe..36e5b8c76 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_bases.py +++ b/examples/pybullet/gym/pybullet_envs/robot_bases.py @@ -235,6 +235,8 @@ class BodyPart: (x, y, z), (a, b, c, d), _, _, _, _ = self._p.getLinkState(body_id, link_id) return np.array([x, y, z, a, b, c, d]) + def get_position(self): return self.current_position() + def get_pose(self): return self.state_fields_of_pose_of(self.bodies[self.bodyIndex], self.bodyPartIndex) From b257bd731b4baa00b0b1c290f2fe675ffcc89545 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 29 Jan 2019 12:03:11 -0800 Subject: [PATCH 3/4] PyBullet: allow createVisualShape to pass vertices, indices, normals and uv coordinates. This can be combined with changeVisualShape to set the texture. --- .../ImportURDFDemo/BulletUrdfImporter.cpp | 47 +++++ .../Importers/ImportURDFDemo/UrdfParser.h | 6 + examples/SharedMemory/PhysicsClientC_API.cpp | 79 ++++++++ examples/SharedMemory/PhysicsClientC_API.h | 3 + .../PhysicsServerCommandProcessor.cpp | 69 ++++++- examples/SharedMemory/SharedMemoryCommands.h | 2 + .../TinyRendererVisualShapeConverter.cpp | 43 +++++ .../examples/createTexturedMeshVisualShape.py | 168 ++++++++++++++++++ examples/pybullet/pybullet.c | 117 +++++++++++- 9 files changed, 522 insertions(+), 12 deletions(-) create mode 100644 examples/pybullet/examples/createTexturedMeshVisualShape.py diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 034ca3d6f..f295458a8 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -969,6 +969,53 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu { switch (visual->m_geometry.m_meshFileType) { + case UrdfGeometry::MEMORY_VERTICES: + { + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + glmesh->m_vertices->resize(visual->m_geometry.m_vertices.size()); + glmesh->m_indices->resize(visual->m_geometry.m_indices.size()); + + for (int i = 0; i < visual->m_geometry.m_vertices.size(); i++) + { + glmesh->m_vertices->at(i).xyzw[0] = visual->m_geometry.m_vertices[i].x(); + glmesh->m_vertices->at(i).xyzw[1] = visual->m_geometry.m_vertices[i].y(); + glmesh->m_vertices->at(i).xyzw[2] = visual->m_geometry.m_vertices[i].z(); + glmesh->m_vertices->at(i).xyzw[3] = 1; + btVector3 normal(visual->m_geometry.m_vertices[i]); + if (visual->m_geometry.m_normals.size() == visual->m_geometry.m_vertices.size()) + { + normal = visual->m_geometry.m_normals[i]; + } + else + { + normal.safeNormalize(); + } + + btVector3 uv(0.5, 0.5, 0); + if (visual->m_geometry.m_uvs.size() == visual->m_geometry.m_vertices.size()) + { + uv = visual->m_geometry.m_uvs[i]; + } + glmesh->m_vertices->at(i).normal[0] = normal[0]; + glmesh->m_vertices->at(i).normal[1] = normal[1]; + glmesh->m_vertices->at(i).normal[2] = normal[2]; + glmesh->m_vertices->at(i).uv[0] = uv[0]; + glmesh->m_vertices->at(i).uv[1] = uv[1]; + + } + for (int i = 0; i < visual->m_geometry.m_indices.size(); i++) + { + glmesh->m_indices->at(i) = visual->m_geometry.m_indices[i]; + + } + glmesh->m_numIndices = visual->m_geometry.m_indices.size(); + glmesh->m_numvertices = visual->m_geometry.m_vertices.size(); + + break; + } case UrdfGeometry::FILE_OBJ: { diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 63ad3e30e..4edf850b2 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -87,6 +87,12 @@ struct UrdfGeometry std::string m_meshFileName; btVector3 m_meshScale; + btArray m_vertices; + btArray m_uvs; + btArray m_normals; + btArray m_indices; + + UrdfMaterial m_localMaterial; bool m_hasLocalMaterial; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index f41d37254..01371d357 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1377,6 +1377,9 @@ B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3PhysicsClientHandle phy { indexUpload[i]=indices[i]; } + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numNormals = 0; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numUVs = 0; + command->m_createUserShapeArgs.m_numUserShapes++; cl->uploadBulletFileToSharedMemory(data, totalUploadSizeInBytes); delete [] data; @@ -1386,6 +1389,82 @@ B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3PhysicsClientHandle phy return -1; } +B3_SHARED_API int b3CreateVisualShapeAddMesh2(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices, const double* normals, int numNormals, const double* uvs, int numUVs) +{ + if (numUVs == 0 && numNormals == 0) + { + return b3CreateCollisionShapeAddConcaveMesh(physClient, commandHandle, meshScale, vertices, numVertices, indices, numIndices); + } + + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE)); + if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE)) + { + int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes; + if (shapeIndex < MAX_COMPOUND_COLLISION_SHAPES && numVertices >= 0 && numIndices >= 0) + { + int i = 0; + if (numVertices>B3_MAX_NUM_VERTICES) + numVertices = B3_MAX_NUM_VERTICES; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = GEOM_FORCE_CONCAVE_TRIMESH; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0]; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1]; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2]; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileType = 0; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileName[0] = 0; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numVertices = numVertices; + + int totalUploadSizeInBytes = numVertices * sizeof(double) * 3 + numIndices * sizeof(int) + numNormals*sizeof(double)*3+numUVs*sizeof(double)*2; + char* data = new char[totalUploadSizeInBytes]; + double* vertexUpload = (double*)data; + int* indexUpload = (int*)(data + numVertices * sizeof(double) * 3); + double* normalUpload = (double*)(data + numVertices * sizeof(double) * 3 + numIndices * sizeof(int)); + double* uvUpload = (double*)(data + numVertices * sizeof(double) * 3 + numIndices * sizeof(int)+ numNormals * sizeof(double) * 3); + for (i = 0; iB3_MAX_NUM_INDICES) + numIndices = B3_MAX_NUM_INDICES; + + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = numIndices; + for (i = 0; im_createUserShapeArgs.m_shapes[shapeIndex].m_numNormals = numNormals; + for (i = 0; i < numNormals; i++) + { + normalUpload[i * 3 + 0] = normals[i * 3 + 0]; + normalUpload[i * 3 + 1] = normals[i * 3 + 1]; + normalUpload[i * 3 + 2] = normals[i * 3 + 2]; + } + + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numUVs = numUVs; + for (i = 0; i < numUVs; i++) + { + uvUpload[i * 2 + 0] = uvs[i * 2 + 0]; + uvUpload[i * 2 + 1] = uvs[i * 2 + 1]; + } + command->m_createUserShapeArgs.m_numUserShapes++; + cl->uploadBulletFileToSharedMemory(data, totalUploadSizeInBytes); + delete[] data; + return shapeIndex; + } + } + return -1; +} + + B3_SHARED_API int b3CreateVisualShapeAddMesh(b3SharedMemoryCommandHandle commandHandle, const char* fileName, const double meshScale[/*3*/]) { return b3CreateCollisionShapeAddMesh(commandHandle, fileName, meshScale); diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 28fb2d7c9..0545c899d 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -486,6 +486,9 @@ extern "C" B3_SHARED_API int b3CreateVisualShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle, double radius, double height); B3_SHARED_API int b3CreateVisualShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, const double planeNormal[/*3*/], double planeConstant); B3_SHARED_API int b3CreateVisualShapeAddMesh(b3SharedMemoryCommandHandle commandHandle, const char* fileName, const double meshScale[/*3*/]); + B3_SHARED_API int b3CreateVisualShapeAddMesh2(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices, const double* normals, int numNormals, const double* uvs, int numUVs); + + B3_SHARED_API void b3CreateVisualSetFlag(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, int flags); B3_SHARED_API void b3CreateVisualShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, const double childPosition[/*3*/], const double childOrientation[/*4*/]); B3_SHARED_API void b3CreateVisualShapeSetSpecularColor(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, const double specularColor[/*3*/]); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 43d18ad91..3c995149f 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2520,7 +2520,8 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface if (m_data->m_pluginManager.getRenderInterface()) { CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); - m_data->m_pluginManager.getRenderInterface()->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, &link, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId, fileIO); + int visualShapeUniqueid = m_data->m_pluginManager.getRenderInterface()->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, &link, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId, fileIO); + colObj->setUserIndex3(visualShapeUniqueid); } } virtual void setBodyUniqueId(int bodyId) @@ -4706,18 +4707,68 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct case URDF_GEOM_MESH: { std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshFileName; - const std::string& error_message_prefix = ""; - std::string out_found_filename; - int out_type; - if (fileIO->findResourcePath(fileName.c_str(), relativeFileName, 1024)) + if (fileName.length()) { - b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); + const std::string& error_message_prefix = ""; + std::string out_found_filename; + int out_type; + if (fileIO->findResourcePath(fileName.c_str(), relativeFileName, 1024)) + { + b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); + } + + bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); + if (foundFile) + { + visualShape.m_geometry.m_meshFileType = out_type; + visualShape.m_geometry.m_meshFileName = fileName; + } + else + { + + } } + else + { + visualShape.m_geometry.m_meshFileType = UrdfGeometry::MEMORY_VERTICES; + int numVertices = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_numVertices; + int numIndices = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_numIndices; + int numUVs = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_numUVs; + int numNormals = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_numNormals; - bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); - visualShape.m_geometry.m_meshFileType = out_type; - visualShape.m_geometry.m_meshFileName = fileName; + if (numVertices > 0 && numIndices >0) + { + char* data = bufferServerToClient; + double* vertexUpload = (double*)data; + int* indexUpload = (int*)(data + numVertices * sizeof(double) * 3); + double* normalUpload = (double*)(data + numVertices * sizeof(double) * 3 + numIndices * sizeof(int)); + double* uvUpload = (double*)(data + numVertices * sizeof(double) * 3 + numIndices * sizeof(int)+numNormals*sizeof(double)*3); + for (int i = 0; i < numIndices; i++) + { + visualShape.m_geometry.m_indices.push_back(indexUpload[i]); + } + for (int i = 0; i < numVertices; i++) + { + btVector3 v0(vertexUpload[i * 3 + 0], + vertexUpload[i * 3 + 1], + vertexUpload[i * 3 + 2]); + visualShape.m_geometry.m_vertices.push_back(v0); + } + for (int i = 0; i < numNormals; i++) + { + btVector3 normal(normalUpload[i * 3 + 0], + normalUpload[i * 3 + 1], + normalUpload[i * 3 + 2]); + visualShape.m_geometry.m_normals.push_back(normal); + } + for (int i = 0; i < numUVs; i++) + { + btVector3 uv(uvUpload[i * 2 + 0], uvUpload[i * 2 + 1],0); + visualShape.m_geometry.m_uvs.push_back(uv); + } + } + } visualShape.m_geometry.m_meshScale.setValue(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[0], clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[1], clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[2]); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 2737ac30f..adab74ee6 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -932,6 +932,8 @@ struct b3CreateUserShapeData int m_visualFlags; int m_numVertices; int m_numIndices; + int m_numUVs; + int m_numNormals; double m_rgbaColor[4]; double m_specularColor[3]; }; diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp index 6fb538089..359a251eb 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp @@ -361,6 +361,49 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa { case UrdfGeometry::MEMORY_VERTICES: { + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + glmesh->m_vertices->resize(visual->m_geometry.m_vertices.size()); + glmesh->m_indices->resize(visual->m_geometry.m_indices.size()); + + for (int i = 0; i < visual->m_geometry.m_vertices.size(); i++) + { + glmesh->m_vertices->at(i).xyzw[0] = visual->m_geometry.m_vertices[i].x(); + glmesh->m_vertices->at(i).xyzw[1] = visual->m_geometry.m_vertices[i].y(); + glmesh->m_vertices->at(i).xyzw[2] = visual->m_geometry.m_vertices[i].z(); + glmesh->m_vertices->at(i).xyzw[3] = 1; + btVector3 normal(visual->m_geometry.m_vertices[i]); + if (visual->m_geometry.m_normals.size() == visual->m_geometry.m_vertices.size()) + { + normal = visual->m_geometry.m_normals[i]; + } + else + { + normal.safeNormalize(); + } + + btVector3 uv(0.5, 0.5, 0); + if (visual->m_geometry.m_uvs.size() == visual->m_geometry.m_vertices.size()) + { + uv = visual->m_geometry.m_uvs[i]; + } + glmesh->m_vertices->at(i).normal[0] = normal[0]; + glmesh->m_vertices->at(i).normal[1] = normal[1]; + glmesh->m_vertices->at(i).normal[2] = normal[2]; + glmesh->m_vertices->at(i).uv[0] = uv[0]; + glmesh->m_vertices->at(i).uv[1] = uv[1]; + + } + for (int i = 0; i < visual->m_geometry.m_indices.size(); i++) + { + glmesh->m_indices->at(i) = visual->m_geometry.m_indices[i]; + + } + glmesh->m_numIndices = visual->m_geometry.m_indices.size(); + glmesh->m_numvertices = visual->m_geometry.m_vertices.size(); + break; } case UrdfGeometry::FILE_OBJ: diff --git a/examples/pybullet/examples/createTexturedMeshVisualShape.py b/examples/pybullet/examples/createTexturedMeshVisualShape.py new file mode 100644 index 000000000..3dbc10c2e --- /dev/null +++ b/examples/pybullet/examples/createTexturedMeshVisualShape.py @@ -0,0 +1,168 @@ +import pybullet as p +import time +import math + +def getRayFromTo(mouseX,mouseY): + width, height, viewMat, projMat, cameraUp, camForward, horizon,vertical, _,_,dist, camTarget = p.getDebugVisualizerCamera() + camPos = [camTarget[0] - dist*camForward[0],camTarget[1] - dist*camForward[1],camTarget[2] - dist*camForward[2]] + farPlane = 10000 + rayForward = [(camTarget[0]-camPos[0]),(camTarget[1]-camPos[1]),(camTarget[2]-camPos[2])] + invLen = farPlane*1./(math.sqrt(rayForward[0]*rayForward[0]+rayForward[1]*rayForward[1]+rayForward[2]*rayForward[2])) + rayForward = [invLen*rayForward[0],invLen*rayForward[1],invLen*rayForward[2]] + rayFrom = camPos + oneOverWidth = float(1)/float(width) + oneOverHeight = float(1)/float(height) + dHor = [horizon[0] * oneOverWidth,horizon[1] * oneOverWidth,horizon[2] * oneOverWidth] + dVer = [vertical[0] * oneOverHeight,vertical[1] * oneOverHeight,vertical[2] * oneOverHeight] + rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[2]] + rayTo = [rayFrom[0]+rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0], + rayFrom[1]+rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1]+float(mouseX)*dHor[1]-float(mouseY)*dVer[1], + rayFrom[2]+rayForward[2] - 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]] + return rayFrom,rayTo + +cid = p.connect(p.SHARED_MEMORY) +if (cid<0): + p.connect(p.GUI) +p.setPhysicsEngineParameter(numSolverIterations=10) +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.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] + +#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=[1,1,1], 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_MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale) + +texUid = p.loadTexture("tex256.png") + +rangex = 1 +rangey = 1 +for i in range (rangex): + for j in range (rangey ): + bodyUid = p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [((-rangex/2)+i)*meshScale[0]*2,(-rangey/2+j)*meshScale[1]*2,1], useMaximalCoordinates=True) + p.changeVisualShape(bodyUid,-1, textureUniqueId = texUid) +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) +p.stopStateLogging(logId) +p.setGravity(0,0,-10) +p.setRealTimeSimulation(1) + +colors = [[1,0,0,1],[0,1,0,1],[0,0,1,1],[1,1,1,1]] +currentColor = 0 + +while (1): + p.getCameraImage(320,200) + mouseEvents = p.getMouseEvents() + for e in mouseEvents: + if ((e[0] == 2) and (e[3]==0) and (e[4]& p.KEY_WAS_TRIGGERED)): + mouseX = e[1] + mouseY = e[2] + rayFrom,rayTo=getRayFromTo(mouseX,mouseY) + rayInfo = p.rayTest(rayFrom,rayTo) + #p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3) + for l in range(len(rayInfo)): + hit = rayInfo[l] + objectUid = hit[0] + if (objectUid>=1): + #p.removeBody(objectUid) + p.changeVisualShape(objectUid,-1,rgbaColor=colors[currentColor]) + currentColor+=1 + if (currentColor>=len(colors)): + currentColor=0 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index befb0086d..7452fec60 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -185,6 +185,34 @@ static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) return 0; } +// vector - double[2] which will be set by values from obVec +static int pybullet_internalSetVector2d(PyObject* obVec, double vector[2]) +{ + int i, len; + PyObject* seq; + if (obVec == NULL) + return 0; + + seq = PySequence_Fast(obVec, "expected a sequence"); + if (seq) + { + len = PySequence_Size(obVec); + assert(len == 2); + if (len == 2) + { + for (i = 0; i < len; i++) + { + vector[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + Py_DECREF(seq); + return 1; + } + Py_DECREF(seq); + } + PyErr_Clear(); + return 0; +} + // vector - double[3] which will be set by values from obVec static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) { @@ -6773,6 +6801,45 @@ static int extractVertices(PyObject* verticesObj, double* vertices, int maxNumVe return numVerticesOut; } + +static int extractUVs(PyObject* uvsObj, double* uvs, int maxNumVertices) +{ + int numUVOut = 0; + + if (uvsObj) + { + PyObject* seqVerticesObj = PySequence_Fast(uvsObj, "expected a sequence of uvs"); + if (seqVerticesObj) + { + int numVerticesSrc = PySequence_Size(seqVerticesObj); + { + int i; + + if (numVerticesSrc > B3_MAX_NUM_VERTICES) + { + PyErr_SetString(SpamError, "Number of uvs exceeds the maximum."); + Py_DECREF(seqVerticesObj); + return 0; + } + for (i = 0; i < numVerticesSrc; i++) + { + PyObject* vertexObj = PySequence_GetItem(seqVerticesObj, i); + double uv[2]; + if (pybullet_internalSetVector2d(vertexObj, uv)) + { + if (uvs) + { + uvs[numUVOut * 2 + 0] = uv[0]; + uvs[numUVOut * 2 + 1] = uv[1]; + } + numUVOut++; + } + } + } + } + } + return numUVOut; +} static int extractIndices(PyObject* indicesObj, int* indices, int maxNumIndices) { int numIndicesOut=0; @@ -7183,9 +7250,14 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb PyObject* halfExtentsObj = 0; - static char* kwlist[] = {"shapeType", "radius", "halfExtents", "length", "fileName", "meshScale", "planeNormal", "flags", "rgbaColor", "specularColor", "visualFramePosition", "visualFrameOrientation", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOi", kwlist, - &shapeType, &radius, &halfExtentsObj, &length, &fileName, &meshScaleObj, &planeNormalObj, &flags, &rgbaColorObj, &specularColorObj, &visualFramePositionObj, &visualFrameOrientationObj, &physicsClientId)) + PyObject* verticesObj = 0; + PyObject* indicesObj = 0; + PyObject* normalsObj = 0; + PyObject* uvsObj = 0; + + static char* kwlist[] = {"shapeType", "radius", "halfExtents", "length", "fileName", "meshScale", "planeNormal", "flags", "rgbaColor", "specularColor", "visualFramePosition", "visualFrameOrientation", "vertices", "indices", "normals", "uvs", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOOOOOi", kwlist, + &shapeType, &radius, &halfExtentsObj, &length, &fileName, &meshScaleObj, &planeNormalObj, &flags, &rgbaColorObj, &specularColorObj, &visualFramePositionObj, &visualFrameOrientationObj, &verticesObj, &indicesObj, &normalsObj, &uvsObj, &physicsClientId)) { return NULL; } @@ -7228,6 +7300,45 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb pybullet_internalSetVectord(meshScaleObj, meshScale); shapeIndex = b3CreateVisualShapeAddMesh(commandHandle, fileName, meshScale); } + + if (shapeType == GEOM_MESH && verticesObj && indicesObj) + { + int numVertices = extractVertices(verticesObj, 0, B3_MAX_NUM_VERTICES); + int numIndices = extractIndices(indicesObj, 0, B3_MAX_NUM_INDICES); + int numNormals = extractVertices(normalsObj, 0, B3_MAX_NUM_VERTICES); + int numUVs = extractUVs(uvsObj, 0, B3_MAX_NUM_VERTICES); + + double* vertices = numVertices ? malloc(numVertices * 3 * sizeof(double)) : 0; + int* indices = numIndices ? malloc(numIndices * sizeof(int)) : 0; + double* normals = numNormals ? malloc(numNormals * 3 * sizeof(double)) : 0; + double* uvs = numUVs ? malloc(numUVs * 2 * sizeof(double)) : 0; + + numVertices = extractVertices(verticesObj, vertices, B3_MAX_NUM_VERTICES); + pybullet_internalSetVectord(meshScaleObj, meshScale); + + if (indicesObj) + { + numIndices = extractIndices(indicesObj, indices, B3_MAX_NUM_INDICES); + } + if (numNormals) + { + extractVertices(normalsObj, normals, numNormals); + } + if (numUVs) + { + extractUVs(uvsObj, uvs, numUVs); + } + + if (numIndices) + { + shapeIndex = b3CreateVisualShapeAddMesh2(sm, commandHandle, meshScale, vertices, numVertices, indices, numIndices, normals, numNormals, uvs, numUVs); + } + free(uvs); + free(normals); + free(vertices); + free(indices); + } + if (shapeType == GEOM_PLANE) { double planeConstant = 0; From 9392b05d5370a4136913e339de9cd05300d0b198 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 29 Jan 2019 12:05:15 -0800 Subject: [PATCH 4/4] bump up to PyBullet 2.4.3 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index c84a0fe53..760d3c452 100644 --- a/setup.py +++ b/setup.py @@ -454,7 +454,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name = 'pybullet', - version='2.4.2', + version='2.4.3', 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',