From a136098120de812830fe46de615c348f8505f8ed Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 22 Jun 2017 16:49:14 -0700 Subject: [PATCH 01/22] add a few more discrete actions to cartpole_bullet.py so it trains faster + add option to train without rendering, enjoy with rendering. --- .../pybullet/gym/envs/bullet/cartpole_bullet.py | 16 ++++++++++------ examples/pybullet/gym/train_pybullet_cartpole.py | 2 +- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/examples/pybullet/gym/envs/bullet/cartpole_bullet.py b/examples/pybullet/gym/envs/bullet/cartpole_bullet.py index f117e8f71..3e7f29daf 100644 --- a/examples/pybullet/gym/envs/bullet/cartpole_bullet.py +++ b/examples/pybullet/gym/envs/bullet/cartpole_bullet.py @@ -22,10 +22,14 @@ class CartPoleBulletEnv(gym.Env): 'video.frames_per_second' : 50 } - def __init__(self): + def __init__(self, renders=True): # start the bullet physics server - p.connect(p.GUI) - #p.connect(p.DIRECT) + self._renders = renders + if (renders): + p.connect(p.GUI) + else: + p.connect(p.DIRECT) + observation_high = np.array([ np.finfo(np.float32).max, np.finfo(np.float32).max, @@ -33,7 +37,7 @@ class CartPoleBulletEnv(gym.Env): np.finfo(np.float32).max]) action_high = np.array([0.1]) - self.action_space = spaces.Discrete(5) + self.action_space = spaces.Discrete(9) self.observation_space = spaces.Box(-observation_high, observation_high) self.theta_threshold_radians = 1 @@ -56,8 +60,8 @@ class CartPoleBulletEnv(gym.Env): self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] theta, theta_dot, x, x_dot = self.state - dv = 0.4 - deltav = [-2.*dv, -dv, 0, dv, 2.*dv][action] + dv = 0.1 + deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action] p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3])) diff --git a/examples/pybullet/gym/train_pybullet_cartpole.py b/examples/pybullet/gym/train_pybullet_cartpole.py index 353fa4a86..4b7f4839e 100644 --- a/examples/pybullet/gym/train_pybullet_cartpole.py +++ b/examples/pybullet/gym/train_pybullet_cartpole.py @@ -12,7 +12,7 @@ def callback(lcl, glb): def main(): - env = gym.make('CartPoleBulletEnv-v0') + env = CartPoleBulletEnv(renders=False) model = deepq.models.mlp([64]) act = deepq.learn( env, From 2ab56b4d62261bfc6a10e476f0a4c77c8f5b6d72 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 23 Jun 2017 14:43:28 -0700 Subject: [PATCH 02/22] Allow to create concave collision meshes. Note that this is only supported for static (mass 0) multibodies. --- examples/SharedMemory/PhysicsClientC_API.cpp | 8 ++ .../PhysicsServerCommandProcessor.cpp | 128 ++++++++++++------ .../pybullet/examples/createMultiBodyLinks.py | 3 + .../examples/createSphereMultiBodies.py | 8 +- examples/pybullet/pybullet.c | 4 +- 5 files changed, 111 insertions(+), 40 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index de1e2e2d9..1bd6504d6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -716,6 +716,7 @@ b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHan b3Assert(command); command->m_type = CMD_CREATE_COLLISION_SHAPE; command->m_updateFlags =0; + command->m_createCollisionShapeArgs = {0}; command->m_createCollisionShapeArgs.m_numCollisionShapes = 0; return (b3SharedMemoryCommandHandle) command; } @@ -733,6 +734,7 @@ int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,do if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_SPHERE; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_sphereRadius = radius; command->m_createCollisionShapeArgs.m_numCollisionShapes++; @@ -753,6 +755,7 @@ int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,doubl if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_BOX; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[0] = halfExtents[0]; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[1] = halfExtents[1]; @@ -775,6 +778,7 @@ int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,d if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_CAPSULE; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleRadius = radius; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleHeight = height; @@ -796,6 +800,7 @@ int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle, if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_CYLINDER; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleRadius = radius; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleHeight = height; @@ -818,6 +823,7 @@ int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, do if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_PLANE; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_planeNormal[0] = planeNormal[0]; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_planeNormal[1] = planeNormal[1]; @@ -841,6 +847,7 @@ int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,cons if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; strcpy(command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshFileName,fileName); command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0]; @@ -856,6 +863,7 @@ int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,cons void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags) { + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index cbddd1ddc..04610f6cd 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -14,7 +14,7 @@ #include "../Importers/ImportURDFDemo/UrdfParser.h" #include "../Utils/b3ResourcePath.h" #include "Bullet3Common/b3FileUtils.h" - +#include "../OpenGLWindow/GLInstanceGraphicsShape.h" #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" @@ -3704,50 +3704,102 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { if (out_type==UrdfGeometry::FILE_OBJ) { - std::vector shapes; - std::string err = tinyobj::LoadObj(shapes,out_found_filename.c_str()); //create a convex hull for each shape, and store it in a btCompoundShape - //shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale); - //static btCollisionShape* createConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale) - B3_PROFILE("createConvexHullFromShapes"); - if (compound==0) + if (clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH) { - compound = worldImporter->createCompoundShape(); - } - compound->setMargin(defaultCollisionMargin); - - for (int s = 0; s<(int)shapes.size(); s++) - { - btConvexHullShape* convexHull = worldImporter->createConvexHullShape(); - convexHull->setMargin(defaultCollisionMargin); - tinyobj::shape_t& shape = shapes[s]; - int faceCount = shape.mesh.indices.size(); - - for (int f = 0; fm_numvertices<=0) { + b3Warning("%s: cannot extract mesh from '%s'\n", pathPrefix, relativeFileName); + delete glmesh; + break; + } + btAlignedObjectArray convertedVerts; + convertedVerts.reserve(glmesh->m_numvertices); + btVector3 meshScale(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[0], + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[1], + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[2]); - btVector3 pt; - pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]); - - convexHull->addPoint(pt*meshScale,false); - - pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]); - convexHull->addPoint(pt*meshScale, false); - - pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]); - convexHull->addPoint(pt*meshScale, false); + for (int i=0; im_numvertices; i++) + { + convertedVerts.push_back(btVector3( + glmesh->m_vertices->at(i).xyzw[0]*meshScale[0], + glmesh->m_vertices->at(i).xyzw[1]*meshScale[1], + glmesh->m_vertices->at(i).xyzw[2]*meshScale[2])); } - convexHull->recalcLocalAabb(); - convexHull->optimizeConvexHull(); - compound->addChildShape(childTransform,convexHull); + BT_PROFILE("convert trimesh"); + btTriangleMesh* meshInterface = new btTriangleMesh(); + { + BT_PROFILE("convert vertices"); + + for (int i=0; im_numIndices/3; i++) + { + const btVector3& v0 = convertedVerts[glmesh->m_indices->at(i*3)]; + const btVector3& v1 = convertedVerts[glmesh->m_indices->at(i*3+1)]; + const btVector3& v2 = convertedVerts[glmesh->m_indices->at(i*3+2)]; + meshInterface->addTriangle(v0,v1,v2); + } + } + { + BT_PROFILE("create btBvhTriangleMeshShape"); + btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true); + //trimesh->setLocalScaling(collision->m_geometry.m_meshScale); + shape = trimesh; + if (compound) + { + compound->addChildShape(childTransform,shape); + } + } + } else + { + + std::vector shapes; + std::string err = tinyobj::LoadObj(shapes,out_found_filename.c_str()); + + //shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale); + //static btCollisionShape* createConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale) + B3_PROFILE("createConvexHullFromShapes"); + if (compound==0) + { + compound = worldImporter->createCompoundShape(); + } + compound->setMargin(defaultCollisionMargin); + + for (int s = 0; s<(int)shapes.size(); s++) + { + btConvexHullShape* convexHull = worldImporter->createConvexHullShape(); + convexHull->setMargin(defaultCollisionMargin); + tinyobj::shape_t& shape = shapes[s]; + int faceCount = shape.mesh.indices.size(); + + for (int f = 0; faddPoint(pt*meshScale,false); + + pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0], + shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1], + shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]); + convexHull->addPoint(pt*meshScale, false); + + pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0], + shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1], + shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]); + convexHull->addPoint(pt*meshScale, false); + } + + convexHull->recalcLocalAabb(); + convexHull->optimizeConvexHull(); + compound->addChildShape(childTransform,convexHull); + } } } } diff --git a/examples/pybullet/examples/createMultiBodyLinks.py b/examples/pybullet/examples/createMultiBodyLinks.py index 9c776cab6..bdb7f6739 100644 --- a/examples/pybullet/examples/createMultiBodyLinks.py +++ b/examples/pybullet/examples/createMultiBodyLinks.py @@ -49,5 +49,8 @@ for i in range (p.getNumJoints(sphereUid)): p.getJointInfo(sphereUid,i) while (1): + keys = p.getKeyboardEvents() + print(keys) + time.sleep(0.01) \ No newline at end of file diff --git a/examples/pybullet/examples/createSphereMultiBodies.py b/examples/pybullet/examples/createSphereMultiBodies.py index fb24febb1..f2b0fb59e 100644 --- a/examples/pybullet/examples/createSphereMultiBodies.py +++ b/examples/pybullet/examples/createSphereMultiBodies.py @@ -4,7 +4,11 @@ import time useMaximalCoordinates = 0 p.connect(p.GUI) -p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates) +#p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates) +monastryId = concaveEnv =p.createCollisionShape(p.GEOM_MESH,fileName="samurai_monastry.obj",flags=p.GEOM_FORCE_CONCAVE_TRIMESH) +orn = p.getQuaternionFromEuler([1.5707963,0,0]) +p.createMultiBody (0,monastryId, baseOrientation=orn) + sphereRadius = 0.05 colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius) colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius]) @@ -28,5 +32,7 @@ p.setGravity(0,0,-10) p.setRealTimeSimulation(1) while (1): + keys = p.getKeyboardEvents() + #print(keys) time.sleep(0.01) \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e7b83649c..c690d128a 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -5008,7 +5008,7 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P pybullet_internalSetVectord(planeNormalObj,planeNormal); shapeIndex = b3CreateCollisionShapeAddPlane(commandHandle, planeNormal, planeConstant); } - if (shapeIndex && flags) + if (shapeIndex>=0 && flags) { b3CreateCollisionSetFlag(commandHandle,shapeIndex,flags); } @@ -7275,6 +7275,8 @@ initpybullet(void) PyModule_AddIntConstant(m, "GEOM_PLANE", GEOM_PLANE); PyModule_AddIntConstant(m, "GEOM_CAPSULE", GEOM_CAPSULE); + PyModule_AddIntConstant(m, "GEOM_FORCE_CONCAVE_TRIMESH", GEOM_FORCE_CONCAVE_TRIMESH); + SpamError = PyErr_NewException("pybullet.error", NULL, NULL); From 9aed6b08f11329bef821ec6e90764701a2d43b85 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 23 Jun 2017 16:18:42 -0700 Subject: [PATCH 03/22] don't set the numSolverIterations that high, was a debug left-over --- examples/pybullet/examples/racecar_differential.py | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/pybullet/examples/racecar_differential.py b/examples/pybullet/examples/racecar_differential.py index 251ce0141..215f7667a 100644 --- a/examples/pybullet/examples/racecar_differential.py +++ b/examples/pybullet/examples/racecar_differential.py @@ -7,7 +7,6 @@ if (cid<0): p.resetSimulation() p.setGravity(0,0,-10) -p.setPhysicsEngineParameter(numSolverIterations=1000) useRealTimeSim = 1 #for video recording (works best on Mac and Linux, not well on Windows) From 8e9f5ef3f3d90eddfcd5ad5886f792c6f8117a88 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 23 Jun 2017 16:22:38 -0700 Subject: [PATCH 04/22] fix compile issue --- examples/SharedMemory/PhysicsClientC_API.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 1bd6504d6..8f743f08d 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -716,7 +716,6 @@ b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHan b3Assert(command); command->m_type = CMD_CREATE_COLLISION_SHAPE; command->m_updateFlags =0; - command->m_createCollisionShapeArgs = {0}; command->m_createCollisionShapeArgs.m_numCollisionShapes = 0; return (b3SharedMemoryCommandHandle) command; } From 65e22ba3e978276fd8b90e19d4ae681193547af4 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 23 Jun 2017 20:24:04 -0700 Subject: [PATCH 05/22] allow auxilary link to be used for gear btMultiBodyGearConstraint. --- data/racecar/racecar_differential.urdf | 30 +++++++++---------- examples/SharedMemory/PhysicsClientC_API.cpp | 14 +++++++++ examples/SharedMemory/PhysicsClientC_API.h | 2 +- .../PhysicsServerCommandProcessor.cpp | 4 +++ examples/SharedMemory/SharedMemoryCommands.h | 2 ++ examples/SharedMemory/SharedMemoryPublic.h | 1 + .../pybullet/examples/racecar_differential.py | 16 ++++++---- examples/pybullet/pybullet.c | 9 ++++-- .../Featherstone/btMultiBodyConstraint.h | 3 ++ .../btMultiBodyGearConstraint.cpp | 14 +++++++-- .../Featherstone/btMultiBodyGearConstraint.h | 7 ++++- 11 files changed, 74 insertions(+), 28 deletions(-) diff --git a/data/racecar/racecar_differential.urdf b/data/racecar/racecar_differential.urdf index 8f5f37519..c4f6ee42e 100644 --- a/data/racecar/racecar_differential.urdf +++ b/data/racecar/racecar_differential.urdf @@ -20,12 +20,12 @@ - + - + @@ -33,14 +33,14 @@ - + - + @@ -66,14 +66,14 @@ - + - + @@ -163,14 +163,14 @@ - + - + @@ -207,14 +207,14 @@ - + - + @@ -533,7 +533,7 @@ - + @@ -573,7 +573,7 @@ - + @@ -612,7 +612,7 @@ - + @@ -650,7 +650,7 @@ - + @@ -686,7 +686,7 @@ - + diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 8f743f08d..cb3e69de4 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1949,6 +1949,20 @@ int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHa return 0; } +int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_USER_CONSTRAINT); + b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT); + + command->m_updateFlags |=USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK; + command->m_userConstraintArguments.m_gearAuxLink = gearAuxLink; + + return 0; +} + + b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index efa45d495..da7bdac42 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -107,7 +107,7 @@ int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHan int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]); int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce); int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio); - +int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink); b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 04610f6cd..1c07479cc 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6450,6 +6450,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { userConstraintPtr->m_mbConstraint->setGearRatio(clientCmd.m_userConstraintArguments.m_gearRatio); } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK) + { + userConstraintPtr->m_mbConstraint->setGearAuxLink(clientCmd.m_userConstraintArguments.m_gearAuxLink); + } } if (userConstraintPtr->m_rbConstraint) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 9658d6e6c..103dd5f96 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -645,6 +645,8 @@ enum EnumUserConstraintFlags USER_CONSTRAINT_CHANGE_MAX_FORCE=32, USER_CONSTRAINT_REQUEST_INFO=64, USER_CONSTRAINT_CHANGE_GEAR_RATIO=128, + USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK=256, + }; enum EnumBodyChangeFlags diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 882db7a24..eaf3de675 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -235,6 +235,7 @@ struct b3UserConstraint double m_maxAppliedForce; int m_userConstraintUniqueId; double m_gearRatio; + int m_gearAuxLink; }; diff --git a/examples/pybullet/examples/racecar_differential.py b/examples/pybullet/examples/racecar_differential.py index 215f7667a..9abab35df 100644 --- a/examples/pybullet/examples/racecar_differential.py +++ b/examples/pybullet/examples/racecar_differential.py @@ -12,10 +12,10 @@ useRealTimeSim = 1 #for video recording (works best on Mac and Linux, not well on Windows) #p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4") p.setRealTimeSimulation(useRealTimeSim) # either this -#p.loadURDF("plane.urdf") -p.loadSDF("stadium.sdf") +p.loadURDF("plane.urdf") +#p.loadSDF("stadium.sdf") -car = p.loadURDF("racecar/racecar_differential.urdf")#, [0,0,2],useFixedBase=True) +car = p.loadURDF("racecar/racecar_differential.urdf") #, [0,0,2],useFixedBase=True) for i in range (p.getNumJoints(car)): print (p.getJointInfo(car,i)) for wheel in range(p.getNumJoints(car)): @@ -26,7 +26,6 @@ wheels = [8,15] print("----------------") #p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10) - c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c,gearRatio=1, maxForce=10000) @@ -39,12 +38,17 @@ p.changeConstraint(c,gearRatio=-1, maxForce=10000) c = p.createConstraint(car,16,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c,gearRatio=1, maxForce=10000) -c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) -p.changeConstraint(c,gearRatio=-1, maxForce=10000) c = p.createConstraint(car,16,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c,gearRatio=-1, maxForce=10000) +c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, maxForce=10000) + +c = p.createConstraint(car,1,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15, maxForce=10000) +c = p.createConstraint(car,3,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15,maxForce=10000) steering = [0,2] diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index c690d128a..056838df8 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4804,11 +4804,12 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, PyObject* keywds) { - static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "gearRatio", "physicsClientId", NULL}; + static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "gearRatio", "gearAuxLink", "physicsClientId", NULL}; int userConstraintUniqueId = -1; b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; int statusType; + int gearAuxLink = -1; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; PyObject* jointChildPivotObj = 0; @@ -4817,7 +4818,7 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P double jointChildFrameOrn[4]; double maxForce = -1; double gearRatio = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOddi", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &gearRatio, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOddii", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &gearRatio, &gearAuxLink, &physicsClientId)) { return NULL; } @@ -4847,6 +4848,10 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P { b3InitChangeUserConstraintSetGearRatio(commandHandle,gearRatio); } + if (gearAuxLink>=0) + { + b3InitChangeUserConstraintSetGearAuxLink(commandHandle,gearAuxLink); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); Py_INCREF(Py_None); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 27e1ccc05..8c28bbf4c 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -184,6 +184,9 @@ public: virtual void debugDraw(class btIDebugDraw* drawer)=0; virtual void setGearRatio(btScalar ratio) {} + virtual void setGearAuxLink(int gearAuxLink) {} + + }; #endif //BT_MULTIBODY_CONSTRAINT_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp index 2e242b1f7..3fdd51815 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp @@ -22,7 +22,8 @@ subject to the following restrictions: btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,1,false), - m_gearRatio(1) + m_gearRatio(1), + m_gearAuxLink(-1) { } @@ -121,11 +122,18 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& int dof = 0; btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; - + btScalar auxVel = 0; + + if (m_gearAuxLink>=0) + { + auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof]; + } + currentVelocity += auxVel; + //btScalar positionStabiliationTerm = erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; //btScalar velocityError = (m_desiredVelocity - currentVelocity); - btScalar desiredRelativeVelocity = 0; + btScalar desiredRelativeVelocity = auxVel; fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h index 947166ae7..711a73e46 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h @@ -31,10 +31,11 @@ protected: btMatrix3x3 m_frameInA; btMatrix3x3 m_frameInB; btScalar m_gearRatio; + int m_gearAuxLink; public: - btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); + //btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); virtual ~btMultiBodyGearConstraint(); @@ -97,6 +98,10 @@ public: { m_gearRatio = gearRatio; } + virtual void setGearAuxLink(int gearAuxLink) + { + m_gearAuxLink = gearAuxLink; + } }; From c777e61d482d3f48b9017e1c700419339fc4def9 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 23 Jun 2017 20:36:19 -0700 Subject: [PATCH 06/22] fix pybullet compilation on some WIN32 version of MSVC --- examples/pybullet/pybullet.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 056838df8..882de968c 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -5185,14 +5185,16 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje double linkJointAxis[3]; double linkInertialFramePosition[3]; double linkInertialFrameOrientation[4]; - + int linkParentIndex; + int linkJointType; + pybullet_internalGetVector3FromSequence(seqLinkInertialFramePositions,i,linkInertialFramePosition); pybullet_internalGetVector4FromSequence(linkInertialFrameOrientationObj,i,linkInertialFrameOrientation); pybullet_internalGetVector3FromSequence(seqLinkPositions,i,linkPosition); pybullet_internalGetVector4FromSequence(seqLinkOrientations,i,linkOrientation); pybullet_internalGetVector3FromSequence(seqLinkJoinAxis,i,linkJointAxis); - int linkParentIndex = pybullet_internalGetIntFromSequence(seqLinkParentIndices,i); - int linkJointType = pybullet_internalGetIntFromSequence(seqLinkJointTypes,i); + linkParentIndex = pybullet_internalGetIntFromSequence(seqLinkParentIndices,i); + linkJointType = pybullet_internalGetIntFromSequence(seqLinkJointTypes,i); b3CreateMultiBodyLink(commandHandle, linkMass, From a651cb9ab4c2974e4937da44bc149e8e0929ff56 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 24 Jun 2017 13:41:33 -0700 Subject: [PATCH 07/22] Implement first pass of transparent graphics object for GLInstancingRenderer remove 'enableBlend' from API, graphics instances use alpha component instead fix forward axis for SimpleCamera --- .../CommonInterfaces/CommonRenderInterface.h | 2 +- .../InverseKinematicsExample.cpp | 1 - .../MultiThreading/MultiThreadingExample.cpp | 2 - .../OpenGLWindow/GLInstancingRenderer.cpp | 85 +++++++++++++++-- examples/OpenGLWindow/GLInstancingRenderer.h | 7 +- examples/OpenGLWindow/SimpleCamera.cpp | 15 +-- .../OpenGLWindow/SimpleOpenGL2Renderer.cpp | 3 - examples/OpenGLWindow/SimpleOpenGL2Renderer.h | 3 +- .../CoordinateSystemDemo.cpp | 1 - .../DynamicTexturedCubeDemo.cpp | 1 - .../RenderInstancingDemo.cpp | 1 - .../RenderingExamples/TinyRendererSetup.cpp | 2 - .../RoboticsLearning/GripperGraspExample.cpp | 1 - .../RoboticsLearning/KukaGraspExample.cpp | 1 - .../RoboticsLearning/R2D2GraspExample.cpp | 1 - examples/Tutorial/Tutorial.cpp | 12 ++- .../examples/vr_racecar_differential.py | 93 +++++++++++++++++++ 17 files changed, 192 insertions(+), 39 deletions(-) create mode 100644 examples/pybullet/examples/vr_racecar_differential.py diff --git a/examples/CommonInterfaces/CommonRenderInterface.h b/examples/CommonInterfaces/CommonRenderInterface.h index a01533358..851db8d5f 100644 --- a/examples/CommonInterfaces/CommonRenderInterface.h +++ b/examples/CommonInterfaces/CommonRenderInterface.h @@ -86,7 +86,7 @@ struct CommonRenderInterface virtual int getTotalNumInstances() const = 0; virtual void writeTransforms()=0; - virtual void enableBlend(bool blend)=0; + virtual void clearZBuffer()=0; //This is internal access to OpenGL3+ features, mainly used for OpenCL-OpenGL interop diff --git a/examples/InverseKinematics/InverseKinematicsExample.cpp b/examples/InverseKinematics/InverseKinematicsExample.cpp index 9f2eae900..b224b8b51 100644 --- a/examples/InverseKinematics/InverseKinematicsExample.cpp +++ b/examples/InverseKinematics/InverseKinematicsExample.cpp @@ -198,7 +198,6 @@ public: } virtual ~InverseKinematicsExample() { - m_app->m_renderer->enableBlend(false); } diff --git a/examples/MultiThreading/MultiThreadingExample.cpp b/examples/MultiThreading/MultiThreadingExample.cpp index 92dea68c5..d0ad1ba51 100644 --- a/examples/MultiThreading/MultiThreadingExample.cpp +++ b/examples/MultiThreading/MultiThreadingExample.cpp @@ -176,14 +176,12 @@ public: //int numBodies = 1; m_app->setUpAxis(1); - m_app->m_renderer->enableBlend(true); } virtual ~MultiThreadingExample() { - m_app->m_renderer->enableBlend(false); } diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index c7fffa36b..a1eb298e0 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -115,6 +115,10 @@ static InternalDataRenderer* sData2; GLint lineWidthRange[2]={1,1}; +enum +{ + eGfxTransparency=1 +}; struct b3GraphicsInstance { GLuint m_cube_vao; @@ -124,6 +128,7 @@ struct b3GraphicsInstance int m_numIndices; int m_numVertices; + int m_numGraphicsInstances; b3AlignedObjectArray m_tempObjectUids; int m_instanceOffset; @@ -131,6 +136,7 @@ struct b3GraphicsInstance int m_primitiveType; float m_materialShinyNess; b3Vector3 m_materialSpecularColor; + int m_flags;//transparency etc b3GraphicsInstance() :m_cube_vao(-1), @@ -143,7 +149,8 @@ struct b3GraphicsInstance m_vertexArrayOffset(0), m_primitiveType(B3_GL_TRIANGLES), m_materialShinyNess(41), - m_materialSpecularColor(b3MakeVector3(.5,.5,.5)) + m_materialSpecularColor(b3MakeVector3(.5,.5,.5)), + m_flags(0) { } @@ -324,8 +331,7 @@ GLInstancingRenderer::GLInstancingRenderer(int maxNumObjectCapacity, int maxShap m_textureinitialized(false), m_screenWidth(0), m_screenHeight(0), - m_upAxis(1), - m_enableBlend(false) + m_upAxis(1) { m_data = new InternalDataRenderer; @@ -865,6 +871,10 @@ int GLInstancingRenderer::registerGraphicsInstanceInternal(int newUid, const flo m_data->m_instance_scale_ptr[index*3+1] = scaling[1]; m_data->m_instance_scale_ptr[index*3+2] = scaling[2]; + if (color[3]<1 && color[3]>0) + { + gfxObj->m_flags |= eGfxTransparency; + } gfxObj->m_numGraphicsInstances++; m_data->m_totalNumInstances++; } else @@ -1898,7 +1908,24 @@ void GLInstancingRenderer::drawLine(const float from[4], const float to[4], cons glUseProgram(0); } +struct SortableTransparentInstance +{ + int m_shapeIndex; + int m_instanceId; + b3Vector3 m_centerPosition; +}; +struct TransparentDistanceSortPredicate +{ + b3Vector3 m_camForwardVec; + + inline bool operator() (const SortableTransparentInstance& a, const SortableTransparentInstance& b) const + { + b3Scalar projA = a.m_centerPosition.dot(m_camForwardVec); + b3Scalar projB = b.m_centerPosition.dot(m_camForwardVec); + return (projA > projB); + } +}; void GLInstancingRenderer::renderSceneInternal(int renderMode) { @@ -2218,8 +2245,9 @@ b3Assert(glGetError() ==GL_NO_ERROR); case B3_USE_SHADOWMAP_RENDERMODE: { - if (m_enableBlend) + if ( gfxObj->m_flags&eGfxTransparency) { + glDepthMask(false); glEnable (GL_BLEND); glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); } @@ -2249,10 +2277,55 @@ b3Assert(glGetError() ==GL_NO_ERROR); glActiveTexture(GL_TEXTURE1); glBindTexture(GL_TEXTURE_2D, m_data->m_shadowTexture); glUniform1i(useShadow_shadowMap,1); - glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances); - if (m_enableBlend) + + //sort transparent objects + + //gfxObj->m_instanceOffset + + if ( gfxObj->m_flags&eGfxTransparency) + { + b3AlignedObjectArray transparentInstances; + transparentInstances.reserve(gfxObj->m_numGraphicsInstances); + + for (int i=0;im_numGraphicsInstances;i++) + { + SortableTransparentInstance inst; + + inst.m_shapeIndex = -1; + + inst.m_instanceId = curOffset+i; + inst.m_centerPosition.setValue(m_data->m_instance_positions_ptr[inst.m_instanceId*4+0], + m_data->m_instance_positions_ptr[inst.m_instanceId*4+1], + m_data->m_instance_positions_ptr[inst.m_instanceId*4+2]); + transparentInstances.push_back(inst); + + } + TransparentDistanceSortPredicate sorter; + float fwd[3]; + m_data->m_activeCamera->getCameraForwardVector(fwd); + sorter.m_camForwardVec.setValue(fwd[0],fwd[1],fwd[2]); + transparentInstances.quickSort(sorter); + + + for (int i=0;im_maxShapeCapacityInBytes)); + glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE)); + glVertexAttribPointer(5, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE)); + glVertexAttribPointer(6, 3, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*3*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE)); + + glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0); + } + } else + { + glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances); + } + + if ( gfxObj->m_flags&eGfxTransparency) { glDisable (GL_BLEND); + glDepthMask(true); } glActiveTexture(GL_TEXTURE1); glBindTexture(GL_TEXTURE_2D,0); diff --git a/examples/OpenGLWindow/GLInstancingRenderer.h b/examples/OpenGLWindow/GLInstancingRenderer.h index 3716b32a6..a066197b5 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.h +++ b/examples/OpenGLWindow/GLInstancingRenderer.h @@ -39,7 +39,7 @@ class GLInstancingRenderer : public CommonRenderInterface int m_screenHeight; int m_upAxis; - bool m_enableBlend; + int registerGraphicsInstanceInternal(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling); void rebuildGraphicsInstances(); @@ -147,10 +147,7 @@ public: virtual int getTotalNumInstances() const; virtual void enableShadowMap(); - virtual void enableBlend(bool blend) - { - m_enableBlend = blend; - } + virtual void clearZBuffer(); virtual void setRenderFrameBuffer(unsigned int renderFrameBuffer); diff --git a/examples/OpenGLWindow/SimpleCamera.cpp b/examples/OpenGLWindow/SimpleCamera.cpp index 14c2fea5d..76992ef90 100644 --- a/examples/OpenGLWindow/SimpleCamera.cpp +++ b/examples/OpenGLWindow/SimpleCamera.cpp @@ -234,8 +234,15 @@ void SimpleCamera::update() b3Vector3 eyePos = b3MakeVector3(0,0,0); eyePos[forwardAxis] = -m_data->m_cameraDistance; + eyePos = b3Matrix3x3(eyeRot)*eyePos; - m_data->m_cameraForward = b3MakeVector3(eyePos[0],eyePos[1],eyePos[2]); + m_data->m_cameraPosition = eyePos; + + + + m_data->m_cameraPosition+= m_data->m_cameraTargetPosition; + + m_data->m_cameraForward = m_data->m_cameraTargetPosition-m_data->m_cameraPosition; if (m_data->m_cameraForward.length2() < B3_EPSILON) { m_data->m_cameraForward.setValue(1.f,0.f,0.f); @@ -243,12 +250,6 @@ void SimpleCamera::update() { m_data->m_cameraForward.normalize(); } - - eyePos = b3Matrix3x3(eyeRot)*eyePos; - - m_data->m_cameraPosition = eyePos; - m_data->m_cameraPosition+= m_data->m_cameraTargetPosition; - } void SimpleCamera::getCameraProjectionMatrix(float projectionMatrix[16]) const diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp index 715f58721..6431789b2 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp @@ -676,9 +676,6 @@ void SimpleOpenGL2Renderer::updateShape(int shapeIndex, const float* vertices) } } -void SimpleOpenGL2Renderer::enableBlend(bool blend) -{ -} void SimpleOpenGL2Renderer::clearZBuffer() { diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.h b/examples/OpenGLWindow/SimpleOpenGL2Renderer.h index a438f4cd5..f57c98a03 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.h +++ b/examples/OpenGLWindow/SimpleOpenGL2Renderer.h @@ -83,8 +83,7 @@ public: virtual void updateShape(int shapeIndex, const float* vertices); - virtual void enableBlend(bool blend); - + virtual void clearZBuffer(); diff --git a/examples/RenderingExamples/CoordinateSystemDemo.cpp b/examples/RenderingExamples/CoordinateSystemDemo.cpp index 148db5674..7176298ff 100644 --- a/examples/RenderingExamples/CoordinateSystemDemo.cpp +++ b/examples/RenderingExamples/CoordinateSystemDemo.cpp @@ -37,7 +37,6 @@ public: } virtual ~CoordinateSystemDemo() { - m_app->m_renderer->enableBlend(false); } diff --git a/examples/RenderingExamples/DynamicTexturedCubeDemo.cpp b/examples/RenderingExamples/DynamicTexturedCubeDemo.cpp index 44d005f18..5b2b2b6d9 100644 --- a/examples/RenderingExamples/DynamicTexturedCubeDemo.cpp +++ b/examples/RenderingExamples/DynamicTexturedCubeDemo.cpp @@ -66,7 +66,6 @@ public: virtual ~DynamicTexturedCubeDemo() { delete m_tinyVrGUI; - m_app->m_renderer->enableBlend(false); } diff --git a/examples/RenderingExamples/RenderInstancingDemo.cpp b/examples/RenderingExamples/RenderInstancingDemo.cpp index 069ac46bb..c85824dc9 100644 --- a/examples/RenderingExamples/RenderInstancingDemo.cpp +++ b/examples/RenderingExamples/RenderInstancingDemo.cpp @@ -68,7 +68,6 @@ public: } virtual ~RenderInstancingDemo() { - m_app->m_renderer->enableBlend(false); } diff --git a/examples/RenderingExamples/TinyRendererSetup.cpp b/examples/RenderingExamples/TinyRendererSetup.cpp index 80584c9a8..a896552ac 100644 --- a/examples/RenderingExamples/TinyRendererSetup.cpp +++ b/examples/RenderingExamples/TinyRendererSetup.cpp @@ -154,7 +154,6 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui) m_app = gui->getAppInterface(); m_internalData = new TinyRendererSetupInternalData(gui->getAppInterface()->m_window->getWidth(),gui->getAppInterface()->m_window->getHeight()); - m_app->m_renderer->enableBlend(true); const char* fileName = "textured_sphere_smooth.obj"; fileName = "cube.obj"; @@ -225,7 +224,6 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui) TinyRendererSetup::~TinyRendererSetup() { - m_app->m_renderer->enableBlend(false); delete m_internalData; } diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 075185d61..5b9d5e437 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -45,7 +45,6 @@ public: } virtual ~GripperGraspExample() { - m_app->m_renderer->enableBlend(false); } diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index 1bf00785d..99f24f7de 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -54,7 +54,6 @@ public: } virtual ~KukaGraspExample() { - m_app->m_renderer->enableBlend(false); } diff --git a/examples/RoboticsLearning/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp index d5896ab4e..abe4e6ff5 100644 --- a/examples/RoboticsLearning/R2D2GraspExample.cpp +++ b/examples/RoboticsLearning/R2D2GraspExample.cpp @@ -41,7 +41,6 @@ public: } virtual ~R2D2GraspExample() { - m_app->m_renderer->enableBlend(false); } diff --git a/examples/Tutorial/Tutorial.cpp b/examples/Tutorial/Tutorial.cpp index f59209a7c..18a9f34eb 100644 --- a/examples/Tutorial/Tutorial.cpp +++ b/examples/Tutorial/Tutorial.cpp @@ -300,7 +300,6 @@ public: int numBodies = 1; m_app->setUpAxis(1); - m_app->m_renderer->enableBlend(true); switch (m_tutorialIndex) { @@ -405,7 +404,7 @@ public: { int width,height,n; - const char* filename = "data/cube.png"; + const char* filename = "data/checker_huge.gif"; const unsigned char* image=0; const char* prefix[]={"./","../","../../","../../../","../../../../"}; @@ -427,10 +426,16 @@ public: // int boxId = m_app->registerCubeShape(1,1,1,textureIndex); int boxId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_HIGH, textureIndex); - b3Vector4 color = b3MakeVector4(1,1,1,0.8); + + b3Vector3 scaling = b3MakeVector3(SPHERE_RADIUS,SPHERE_RADIUS,SPHERE_RADIUS); for (int i=0;im_collisionShape.m_sphere.m_radius = SPHERE_RADIUS; m_bodies[i]->m_collisionShape.m_type = LW_SPHERE_TYPE; @@ -467,7 +472,6 @@ public: m_timeSeriesCanvas0 = 0; m_timeSeriesCanvas1 = 0; - m_app->m_renderer->enableBlend(false); } diff --git a/examples/pybullet/examples/vr_racecar_differential.py b/examples/pybullet/examples/vr_racecar_differential.py new file mode 100644 index 000000000..5cdb8dc95 --- /dev/null +++ b/examples/pybullet/examples/vr_racecar_differential.py @@ -0,0 +1,93 @@ +import pybullet as p +import time +CONTROLLER_ID = 0 +POSITION=1 +ORIENTATION=2 +BUTTONS=6 + + +cid = p.connect(p.SHARED_MEMORY) +if (cid<0): + p.connect(p.GUI) + +p.resetSimulation() +p.setGravity(0,0,-10) +useRealTimeSim = 1 + +#for video recording (works best on Mac and Linux, not well on Windows) +#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4") +p.setRealTimeSimulation(useRealTimeSim) # either this +p.loadURDF("plane.urdf") +#p.loadSDF("stadium.sdf") + +car = p.loadURDF("racecar/racecar_differential.urdf") #, [0,0,2],useFixedBase=True) +for i in range (p.getNumJoints(car)): + print (p.getJointInfo(car,i)) +for wheel in range(p.getNumJoints(car)): + p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0) + p.getJointInfo(car,wheel) + +wheels = [8,15] +print("----------------") + +#p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10) +c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=1, maxForce=10000) + +c = p.createConstraint(car,10,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, maxForce=10000) + +c = p.createConstraint(car,9,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, maxForce=10000) + +c = p.createConstraint(car,16,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=1, maxForce=10000) + + +c = p.createConstraint(car,16,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, maxForce=10000) + +c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, maxForce=10000) + +c = p.createConstraint(car,1,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15, maxForce=10000) +c = p.createConstraint(car,3,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15,maxForce=10000) + + +steering = [0,2] + +targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-50,50,0) +maxForceSlider = p.addUserDebugParameter("maxForce",0,50,20) +steeringSlider = p.addUserDebugParameter("steering",-1,1,0) +activeController = -1 + +while (True): + + + maxForce = p.readUserDebugParameter(maxForceSlider) + targetVelocity = p.readUserDebugParameter(targetVelocitySlider) + steeringAngle = p.readUserDebugParameter(steeringSlider) + #print(targetVelocity) + events = p.getVREvents() + for e in events: + if (e[BUTTONS][33]&p.VR_BUTTON_WAS_TRIGGERED): + activeController = e[CONTROLLER_ID] + if (activeController == e[CONTROLLER_ID]): + orn = e[2] + eul = p.getEulerFromQuaternion(orn) + steeringAngle=eul[0] + + targetVelocity = 20.0*e[3] + + for wheel in wheels: + p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=maxForce) + + for steer in steering: + p.setJointMotorControl2(car,steer,p.POSITION_CONTROL,targetPosition=-steeringAngle) + + steering + if (useRealTimeSim==0): + p.stepSimulation() + time.sleep(0.01) From d5fe67cf57f2989748c4f7209c61dadc1b9c9a89 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 24 Jun 2017 19:38:31 -0700 Subject: [PATCH 08/22] Add pybullet transparent.py example, transparency with global per-object sort in GLInstancingRenderer --- data/sphere_transparent.urdf | 32 ++ .../Collision/CollisionTutorialBullet2.cpp | 2 - .../OpenGLWindow/GLInstancingRenderer.cpp | 479 ++++++++++-------- examples/Tutorial/Tutorial.cpp | 12 +- examples/pybullet/examples/transparent.py | 18 + 5 files changed, 324 insertions(+), 219 deletions(-) create mode 100644 data/sphere_transparent.urdf create mode 100644 examples/pybullet/examples/transparent.py diff --git a/data/sphere_transparent.urdf b/data/sphere_transparent.urdf new file mode 100644 index 000000000..d856b5a26 --- /dev/null +++ b/data/sphere_transparent.urdf @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/Collision/CollisionTutorialBullet2.cpp b/examples/Collision/CollisionTutorialBullet2.cpp index 8159f89ee..6e65ac0cd 100644 --- a/examples/Collision/CollisionTutorialBullet2.cpp +++ b/examples/Collision/CollisionTutorialBullet2.cpp @@ -79,7 +79,6 @@ public: gTotalPoints = 0; m_app->setUpAxis(1); - m_app->m_renderer->enableBlend(true); switch (m_tutorialIndex) { @@ -250,7 +249,6 @@ public: m_timeSeriesCanvas0 = 0; - m_app->m_renderer->enableBlend(false); } diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index a1eb298e0..3796bf371 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -2046,9 +2046,9 @@ void GLInstancingRenderer::renderSceneInternal(int renderMode) b3Vector3 center = b3MakeVector3(0,0,0); //float upf[3]; //m_data->m_activeCamera->getCameraUpVector(upf); - b3Vector3 up, fwd; + b3Vector3 up, lightFwd; b3Vector3 lightDir = m_data->m_lightPos.normalized(); - b3PlaneSpace1(lightDir,up,fwd); + b3PlaneSpace1(lightDir,up,lightFwd); // b3Vector3 up = b3MakeVector3(upf[0],upf[1],upf[2]); b3CreateLookAt(m_data->m_lightPos,center,up,&depthViewMatrix[0][0]); //b3CreateLookAt(lightPos,m_data->m_cameraTargetPosition,b3Vector3(0,1,0),(float*)depthModelViewMatrix2); @@ -2108,207 +2108,194 @@ b3Assert(glGetError() ==GL_NO_ERROR); { totalNumInstances+=m_graphicsInstances[i]->m_numGraphicsInstances; } - - int curOffset = 0; - //GLuint lastBindTexture = 0; - - for (int i=0;i transparentInstances; + { + int curOffset = 0; + //GLuint lastBindTexture = 0; - b3GraphicsInstance* gfxObj = m_graphicsInstances[i]; - if (gfxObj->m_numGraphicsInstances) + transparentInstances.reserve(totalNumInstances); + + for (int obj=0;objm_texturehandle) - curBindTexture = gfxObj->m_texturehandle; - else - curBindTexture = m_data->m_defaultTexturehandle; - -//disable lazy evaluation, it just leads to bugs - //if (lastBindTexture != curBindTexture) + b3GraphicsInstance* gfxObj = m_graphicsInstances[obj]; + if (gfxObj->m_numGraphicsInstances) { - glBindTexture(GL_TEXTURE_2D,curBindTexture); - } - //lastBindTexture = curBindTexture; - -b3Assert(glGetError() ==GL_NO_ERROR); - // int myOffset = gfxObj->m_instanceOffset*4*sizeof(float); - - int POSITION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4); - int ORIENTATION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4); - int COLOR_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4); -// int SCALE_BUFFER_SIZE = (totalNumInstances*sizeof(float)*3); - - glBindVertexArray(gfxObj->m_cube_vao); - - - int vertexStride = 9*sizeof(float); - PointerCaster vertex; - vertex.m_baseIndex = gfxObj->m_vertexArrayOffset*vertexStride; - - - glVertexAttribPointer(0, 4, GL_FLOAT, GL_FALSE, 9*sizeof(float), vertex.m_pointer); - glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(curOffset*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes)); - glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(curOffset*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE)); - - PointerCaster uv; - uv.m_baseIndex = 7*sizeof(float)+vertex.m_baseIndex; - - PointerCaster normal; - normal.m_baseIndex = 4*sizeof(float)+vertex.m_baseIndex; - - glVertexAttribPointer(3, 2, GL_FLOAT, GL_FALSE, 9*sizeof(float), uv.m_pointer); - glVertexAttribPointer(4, 3, GL_FLOAT, GL_FALSE, 9*sizeof(float), normal.m_pointer); - glVertexAttribPointer(5, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(curOffset*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE)); - glVertexAttribPointer(6, 3, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(curOffset*3*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE)); - - glEnableVertexAttribArray(0); - glEnableVertexAttribArray(1); - glEnableVertexAttribArray(2); - glEnableVertexAttribArray(3); - glEnableVertexAttribArray(4); - glEnableVertexAttribArray(5); - glEnableVertexAttribArray(6); - glVertexAttribDivisor(0, 0); - glVertexAttribDivisor(1, 1); - glVertexAttribDivisor(2, 1); - glVertexAttribDivisor(3, 0); - glVertexAttribDivisor(4, 0); - glVertexAttribDivisor(5, 1); - glVertexAttribDivisor(6, 1); - - - - - - - int indexCount = gfxObj->m_numIndices; - GLvoid* indexOffset = 0; - - glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, gfxObj->m_index_vbo); - { - B3_PROFILE("glDrawElementsInstanced"); - - if (gfxObj->m_primitiveType==B3_GL_POINTS) + SortableTransparentInstance inst; + + inst.m_shapeIndex = obj; + + + if ((gfxObj->m_flags&eGfxTransparency)==0) { - glUseProgram(instancingShaderPointSprite); - glUniformMatrix4fv(ProjectionMatrixPointSprite, 1, false, &m_data->m_projectionMatrix[0]); - glUniformMatrix4fv(ModelViewMatrixPointSprite, 1, false, &m_data->m_viewMatrix[0]); - glUniform1f(screenWidthPointSprite,float(m_screenWidth)); - - //glUniform1i(uniform_texture_diffusePointSprite, 0); - b3Assert(glGetError() ==GL_NO_ERROR); - glPointSize(20); - -#ifndef __APPLE__ - glEnable(GL_POINT_SPRITE_ARB); -// glTexEnvi(GL_POINT_SPRITE_ARB, GL_COORD_REPLACE_ARB, GL_TRUE); -#endif - - glEnable(GL_VERTEX_PROGRAM_POINT_SIZE); - glDrawElementsInstanced(GL_POINTS, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances); + inst.m_instanceId = curOffset; + inst.m_centerPosition.setValue(m_data->m_instance_positions_ptr[inst.m_instanceId*4+0], + m_data->m_instance_positions_ptr[inst.m_instanceId*4+1], + m_data->m_instance_positions_ptr[inst.m_instanceId*4+2]); + inst.m_centerPosition *= -1;//reverse sort opaque instances + transparentInstances.push_back(inst); } else { - switch (renderMode) + for (int i=0;im_numGraphicsInstances;i++) { - - case B3_DEFAULT_RENDERMODE: - { - glUseProgram(instancingShader); - glUniformMatrix4fv(ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]); - glUniformMatrix4fv(ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]); - - b3Vector3 gLightDir = m_data->m_lightPos; - gLightDir.normalize(); - glUniform3f(regularLightDirIn,gLightDir[0],gLightDir[1],gLightDir[2]); - - glUniform1i(uniform_texture_diffuse, 0); - glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances); - break; - } - case B3_CREATE_SHADOWMAP_RENDERMODE: - { - /*printf("createShadowMapInstancingShader=%d\n",createShadowMapInstancingShader); - printf("createShadow_depthMVP=%d\n",createShadow_depthMVP); - printf("indexOffset=%d\n",indexOffset); - printf("gfxObj->m_numGraphicsInstances=%d\n",gfxObj->m_numGraphicsInstances); - printf("indexCount=%d\n",indexCount); - */ - - glUseProgram(createShadowMapInstancingShader); - glUniformMatrix4fv(createShadow_depthMVP, 1, false, &depthMVP[0][0]); - glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances); - break; - } - - case B3_USE_SHADOWMAP_RENDERMODE: - { - if ( gfxObj->m_flags&eGfxTransparency) - { - glDepthMask(false); - glEnable (GL_BLEND); - glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); - } - - glUseProgram(useShadowMapInstancingShader); - glUniformMatrix4fv(useShadow_ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]); - glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]); - glUniformMatrix4fv(useShadow_ViewMatrixInverse, 1, false, &m_data->m_viewMatrixInverse[0]); - glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]); - - glUniform3f(useShadow_lightSpecularIntensity, m_data->m_lightSpecularIntensity[0],m_data->m_lightSpecularIntensity[1],m_data->m_lightSpecularIntensity[2]); - glUniform3f(useShadow_materialSpecularColor, gfxObj->m_materialSpecularColor[0],gfxObj->m_materialSpecularColor[1],gfxObj->m_materialSpecularColor[2]); - - - - float MVP[16]; - b3Matrix4x4Mul16(m_data->m_projectionMatrix,m_data->m_viewMatrix,MVP); - glUniformMatrix4fv(useShadow_MVP, 1, false, &MVP[0]); - //gLightDir.normalize(); - glUniform3f(useShadow_lightPosIn,m_data->m_lightPos[0],m_data->m_lightPos[1],m_data->m_lightPos[2]); - float camPos[3]; - m_data->m_activeCamera->getCameraPosition(camPos); - glUniform3f(useShadow_cameraPositionIn,camPos[0],camPos[1],camPos[2]); - glUniform1f(useShadow_materialShininessIn,gfxObj->m_materialShinyNess); - - glUniformMatrix4fv(useShadow_DepthBiasModelViewMatrix, 1, false, &depthBiasMVP[0][0]); - glActiveTexture(GL_TEXTURE1); - glBindTexture(GL_TEXTURE_2D, m_data->m_shadowTexture); - glUniform1i(useShadow_shadowMap,1); - - //sort transparent objects - - //gfxObj->m_instanceOffset - - if ( gfxObj->m_flags&eGfxTransparency) - { - b3AlignedObjectArray transparentInstances; - transparentInstances.reserve(gfxObj->m_numGraphicsInstances); - - for (int i=0;im_numGraphicsInstances;i++) - { - SortableTransparentInstance inst; - - inst.m_shapeIndex = -1; - - inst.m_instanceId = curOffset+i; - inst.m_centerPosition.setValue(m_data->m_instance_positions_ptr[inst.m_instanceId*4+0], + inst.m_instanceId = curOffset+i; + inst.m_centerPosition.setValue(m_data->m_instance_positions_ptr[inst.m_instanceId*4+0], m_data->m_instance_positions_ptr[inst.m_instanceId*4+1], m_data->m_instance_positions_ptr[inst.m_instanceId*4+2]); - transparentInstances.push_back(inst); + transparentInstances.push_back(inst); + } + } + curOffset+=gfxObj->m_numGraphicsInstances; + } + } + TransparentDistanceSortPredicate sorter; + float fwd[3]; + m_data->m_activeCamera->getCameraForwardVector(fwd); + sorter.m_camForwardVec.setValue(fwd[0],fwd[1],fwd[2]); + transparentInstances.quickSort(sorter); + } - } - TransparentDistanceSortPredicate sorter; - float fwd[3]; - m_data->m_activeCamera->getCameraForwardVector(fwd); - sorter.m_camForwardVec.setValue(fwd[0],fwd[1],fwd[2]); - transparentInstances.quickSort(sorter); + + //two passes: first for opaque instances, second for transparent ones. + for (int pass = 0; pass<2;pass++) + { + for (int i=0;im_flags&eGfxTransparency)==0); + + //transparent objects don't cast shadows (to simplify things) + if (gfxObj->m_flags&eGfxTransparency) + { + if (renderMode==B3_CREATE_SHADOWMAP_RENDERMODE) + drawThisPass = 0; + } + + if (drawThisPass && gfxObj->m_numGraphicsInstances) + { + glActiveTexture(GL_TEXTURE0); + GLuint curBindTexture = 0; + if (gfxObj->m_texturehandle) + curBindTexture = gfxObj->m_texturehandle; + else + curBindTexture = m_data->m_defaultTexturehandle; + + //disable lazy evaluation, it just leads to bugs + //if (lastBindTexture != curBindTexture) + { + glBindTexture(GL_TEXTURE_2D,curBindTexture); + } + //lastBindTexture = curBindTexture; + + b3Assert(glGetError() ==GL_NO_ERROR); + // int myOffset = gfxObj->m_instanceOffset*4*sizeof(float); + + int POSITION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4); + int ORIENTATION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4); + int COLOR_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4); + // int SCALE_BUFFER_SIZE = (totalNumInstances*sizeof(float)*3); + + glBindVertexArray(gfxObj->m_cube_vao); - for (int i=0;im_vertexArrayOffset*vertexStride; + + + glVertexAttribPointer(0, 4, GL_FLOAT, GL_FALSE, 9*sizeof(float), vertex.m_pointer); + glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(transparentInstances[i].m_instanceId*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes)); + glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(transparentInstances[i].m_instanceId*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE)); + + PointerCaster uv; + uv.m_baseIndex = 7*sizeof(float)+vertex.m_baseIndex; + + PointerCaster normal; + normal.m_baseIndex = 4*sizeof(float)+vertex.m_baseIndex; + + glVertexAttribPointer(3, 2, GL_FLOAT, GL_FALSE, 9*sizeof(float), uv.m_pointer); + glVertexAttribPointer(4, 3, GL_FLOAT, GL_FALSE, 9*sizeof(float), normal.m_pointer); + glVertexAttribPointer(5, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(transparentInstances[i].m_instanceId*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE)); + glVertexAttribPointer(6, 3, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(transparentInstances[i].m_instanceId*3*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE)); + + glEnableVertexAttribArray(0); + glEnableVertexAttribArray(1); + glEnableVertexAttribArray(2); + glEnableVertexAttribArray(3); + glEnableVertexAttribArray(4); + glEnableVertexAttribArray(5); + glEnableVertexAttribArray(6); + glVertexAttribDivisor(0, 0); + glVertexAttribDivisor(1, 1); + glVertexAttribDivisor(2, 1); + glVertexAttribDivisor(3, 0); + glVertexAttribDivisor(4, 0); + glVertexAttribDivisor(5, 1); + glVertexAttribDivisor(6, 1); + + + + + + + int indexCount = gfxObj->m_numIndices; + GLvoid* indexOffset = 0; + + glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, gfxObj->m_index_vbo); + { + B3_PROFILE("glDrawElementsInstanced"); + + if (gfxObj->m_primitiveType==B3_GL_POINTS) + { + glUseProgram(instancingShaderPointSprite); + glUniformMatrix4fv(ProjectionMatrixPointSprite, 1, false, &m_data->m_projectionMatrix[0]); + glUniformMatrix4fv(ModelViewMatrixPointSprite, 1, false, &m_data->m_viewMatrix[0]); + glUniform1f(screenWidthPointSprite,float(m_screenWidth)); + + //glUniform1i(uniform_texture_diffusePointSprite, 0); + b3Assert(glGetError() ==GL_NO_ERROR); + glPointSize(20); + + #ifndef __APPLE__ + glEnable(GL_POINT_SPRITE_ARB); + // glTexEnvi(GL_POINT_SPRITE_ARB, GL_COORD_REPLACE_ARB, GL_TRUE); + #endif + + glEnable(GL_VERTEX_PROGRAM_POINT_SIZE); + glDrawElementsInstanced(GL_POINTS, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances); + } else + { + switch (renderMode) + { + + case B3_DEFAULT_RENDERMODE: + { + if ( gfxObj->m_flags&eGfxTransparency) { + glDepthMask(false); + glEnable (GL_BLEND); + glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + } + + glUseProgram(instancingShader); + glUniformMatrix4fv(ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]); + glUniformMatrix4fv(ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]); + + b3Vector3 gLightDir = m_data->m_lightPos; + gLightDir.normalize(); + glUniform3f(regularLightDirIn,gLightDir[0],gLightDir[1],gLightDir[2]); + + glUniform1i(uniform_texture_diffuse, 0); + + if ( gfxObj->m_flags&eGfxTransparency) + { + int instanceId = transparentInstances[i].m_instanceId; glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes)); glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE)); @@ -2316,36 +2303,104 @@ b3Assert(glGetError() ==GL_NO_ERROR); glVertexAttribPointer(6, 3, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*3*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE)); glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0); + + } else + { + glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances); } - } else - { - glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances); - } - if ( gfxObj->m_flags&eGfxTransparency) - { - glDisable (GL_BLEND); - glDepthMask(true); - } - glActiveTexture(GL_TEXTURE1); - glBindTexture(GL_TEXTURE_2D,0); + if ( gfxObj->m_flags&eGfxTransparency) + { + glDisable (GL_BLEND); + glDepthMask(true); + } + + break; + } + case B3_CREATE_SHADOWMAP_RENDERMODE: + { + glUseProgram(createShadowMapInstancingShader); + glUniformMatrix4fv(createShadow_depthMVP, 1, false, &depthMVP[0][0]); + glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances); + break; + } - glActiveTexture(GL_TEXTURE0); - glBindTexture(GL_TEXTURE_2D,0); - break; - } - default: - { - // b3Assert(0); - } - }; + case B3_USE_SHADOWMAP_RENDERMODE: + { + if ( gfxObj->m_flags&eGfxTransparency) + { + glDepthMask(false); + glEnable (GL_BLEND); + glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + } + + glUseProgram(useShadowMapInstancingShader); + glUniformMatrix4fv(useShadow_ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]); + glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]); + glUniformMatrix4fv(useShadow_ViewMatrixInverse, 1, false, &m_data->m_viewMatrixInverse[0]); + glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]); + + glUniform3f(useShadow_lightSpecularIntensity, m_data->m_lightSpecularIntensity[0],m_data->m_lightSpecularIntensity[1],m_data->m_lightSpecularIntensity[2]); + glUniform3f(useShadow_materialSpecularColor, gfxObj->m_materialSpecularColor[0],gfxObj->m_materialSpecularColor[1],gfxObj->m_materialSpecularColor[2]); + + + + float MVP[16]; + b3Matrix4x4Mul16(m_data->m_projectionMatrix,m_data->m_viewMatrix,MVP); + glUniformMatrix4fv(useShadow_MVP, 1, false, &MVP[0]); + //gLightDir.normalize(); + glUniform3f(useShadow_lightPosIn,m_data->m_lightPos[0],m_data->m_lightPos[1],m_data->m_lightPos[2]); + float camPos[3]; + m_data->m_activeCamera->getCameraPosition(camPos); + glUniform3f(useShadow_cameraPositionIn,camPos[0],camPos[1],camPos[2]); + glUniform1f(useShadow_materialShininessIn,gfxObj->m_materialShinyNess); + + glUniformMatrix4fv(useShadow_DepthBiasModelViewMatrix, 1, false, &depthBiasMVP[0][0]); + glActiveTexture(GL_TEXTURE1); + glBindTexture(GL_TEXTURE_2D, m_data->m_shadowTexture); + glUniform1i(useShadow_shadowMap,1); + + //sort transparent objects + + //gfxObj->m_instanceOffset + + if ( gfxObj->m_flags&eGfxTransparency) + { + int instanceId = transparentInstances[i].m_instanceId; + glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes)); + glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE)); + glVertexAttribPointer(5, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE)); + glVertexAttribPointer(6, 3, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*3*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE)); + glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0); + } else + { + glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances); + } + + if ( gfxObj->m_flags&eGfxTransparency) + { + glDisable (GL_BLEND); + glDepthMask(true); + } + glActiveTexture(GL_TEXTURE1); + glBindTexture(GL_TEXTURE_2D,0); + + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D,0); + break; + } + default: + { + // b3Assert(0); + } + }; + } + + + //glDrawElementsInstanced(GL_LINE_LOOP, indexCount, GL_UNSIGNED_INT, (void*)indexOffset, gfxObj->m_numGraphicsInstances); } - - - //glDrawElementsInstanced(GL_LINE_LOOP, indexCount, GL_UNSIGNED_INT, (void*)indexOffset, gfxObj->m_numGraphicsInstances); } } - curOffset+= gfxObj->m_numGraphicsInstances; } { diff --git a/examples/Tutorial/Tutorial.cpp b/examples/Tutorial/Tutorial.cpp index 18a9f34eb..8d43daac2 100644 --- a/examples/Tutorial/Tutorial.cpp +++ b/examples/Tutorial/Tutorial.cpp @@ -425,21 +425,23 @@ public: } // int boxId = m_app->registerCubeShape(1,1,1,textureIndex); - int boxId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_HIGH, textureIndex); - + int sphereTransparent = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_HIGH, textureIndex); + int sphereOpaque= m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_HIGH, textureIndex); b3Vector3 scaling = b3MakeVector3(SPHERE_RADIUS,SPHERE_RADIUS,SPHERE_RADIUS); for (int i=0;im_collisionShape.m_sphere.m_radius = SPHERE_RADIUS; m_bodies[i]->m_collisionShape.m_type = LW_SPHERE_TYPE; - m_bodies[i]->m_graphicsIndex = m_app->m_renderer->registerGraphicsInstance(boxId,m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation,color,scaling); + m_bodies[i]->m_graphicsIndex = m_app->m_renderer->registerGraphicsInstance(gfxShape,m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation,color,scaling); m_app->m_renderer->writeSingleInstanceTransformToCPU(m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation, m_bodies[i]->m_graphicsIndex); } } diff --git a/examples/pybullet/examples/transparent.py b/examples/pybullet/examples/transparent.py new file mode 100644 index 000000000..5c6cc09c6 --- /dev/null +++ b/examples/pybullet/examples/transparent.py @@ -0,0 +1,18 @@ +import pybullet as p +import time +p.connect(p.GUI) +p.loadURDF("plane.urdf") +sphereUid = p.loadURDF("sphere_transparent.urdf",[0,0,2]) + +redSlider = p.addUserDebugParameter("red",0,1,1) +greenSlider = p.addUserDebugParameter("green",0,1,0) +blueSlider = p.addUserDebugParameter("blue",0,1,0) +alphaSlider = p.addUserDebugParameter("alpha",0,1,0.5) + +while (1): + red = p.readUserDebugParameter(redSlider) + green = p.readUserDebugParameter(greenSlider) + blue = p.readUserDebugParameter(blueSlider) + alpha = p.readUserDebugParameter(alphaSlider) + p.changeVisualShape(sphereUid,-1,rgbaColor=[red,green,blue,alpha]) + time.sleep(0.01) \ No newline at end of file From bf800e30d5d5fe688a2cd17a014f7068ad10886f Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 29 Jun 2017 17:54:04 -0700 Subject: [PATCH 09/22] Added manually converted widowx.urdf from https://github.com/RobotnikAutomation/widowx_arm Added simple pybullet file in Bullet/examples/pybullet/examples/widows.py (preliminary, both URDF and py file needs more work to be useful) https://github.com/RobotnikAutomation/widowx_arm/blob/master/widowx_arm_description/package.xml See also http://www.trossenrobotics.com/widowxrobotarm --- data/widowx/bsd_license.txt | 22 ++ data/widowx/meshes/base_link.stl | Bin 0 -> 26684 bytes data/widowx/meshes/biceps_link.stl | Bin 0 -> 18884 bytes data/widowx/meshes/forearm_link.stl | Bin 0 -> 21084 bytes .../widowx/meshes/gripper_hand_fixed_link.stl | Bin 0 -> 5284 bytes data/widowx/meshes/gripper_rail_link.stl | Bin 0 -> 17484 bytes data/widowx/meshes/shoulder_link.stl | Bin 0 -> 11884 bytes data/widowx/meshes/wrist_1_link.stl | Bin 0 -> 31884 bytes data/widowx/meshes/wrist_2_link.stl | Bin 0 -> 33084 bytes data/widowx/widowx.urdf | 279 ++++++++++++++++++ examples/pybullet/examples/widows.py | 21 ++ 11 files changed, 322 insertions(+) create mode 100644 data/widowx/bsd_license.txt create mode 100644 data/widowx/meshes/base_link.stl create mode 100644 data/widowx/meshes/biceps_link.stl create mode 100644 data/widowx/meshes/forearm_link.stl create mode 100644 data/widowx/meshes/gripper_hand_fixed_link.stl create mode 100644 data/widowx/meshes/gripper_rail_link.stl create mode 100644 data/widowx/meshes/shoulder_link.stl create mode 100644 data/widowx/meshes/wrist_1_link.stl create mode 100644 data/widowx/meshes/wrist_2_link.stl create mode 100644 data/widowx/widowx.urdf create mode 100644 examples/pybullet/examples/widows.py diff --git a/data/widowx/bsd_license.txt b/data/widowx/bsd_license.txt new file mode 100644 index 000000000..cf5bb8ce7 --- /dev/null +++ b/data/widowx/bsd_license.txt @@ -0,0 +1,22 @@ +Converted from https://github.com/RobotnikAutomation/widowx_arm/blob/master/widowx_arm_description/package.xml + +Copyright 2017 robotnik + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. +3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + + widowx_arm_description + 0.0.2 + The widowx_arm_description package + + Jose Rapado García + Román Navarro + Carlos Villar + + \ No newline at end of file diff --git a/data/widowx/meshes/base_link.stl b/data/widowx/meshes/base_link.stl new file mode 100644 index 0000000000000000000000000000000000000000..f9a35903780929e68d2cdd55b154171b5cc85cb5 GIT binary patch literal 26684 zcmb`OeXM0yS;mjF35g#Nv=tke+QAY;Vq4G>L+9KhW{53@D3LUfrdmMTI=N(O>R1?I z<`OZ{Xw`l|w2Gl<`~t0FY!kWX9?%38En<|^@CQOvlG+$sNl5re>U!SwKI{C}U3=e& zi6?32nf*S`de-N=*WP>0ou_wy;>WJ|zBk_a<5&E@{4ROM0>8c0&dJ^NcmKi1u6y`h zA6zE>u<(tq{OF0dKJyK0VF(s`s|W6V)rs3~xqYd~KYz_%zyJ1o-V%6T!3Y(lqW0@){3kvf9aa_$G>>_go2UX^=sC*-}7H5j_xaZ zQ1N$9zH$AEf4Vo+^R*(2qmMl%j}DCv!tc^uvairJes1k zX<26Ce66A^vPw@c%dCvC={*=Jqq!`zFIe>M6^u|(#%WnHMr4V31zCwB$MlM<)d}@U zwjK&nw8v`8H82MyTl9@znc%dHuOt&adQ_wOYUbRSz$J z?{Dw@Hm_h~U0*DUVrc~<#_G3Hl5*v`&a0>1_pF_lyz?F5VV24w%eZA<>2(V;Uy34yzuTj1_fEh)#KLJKDl$#*S{$gj2Nr#TwROBv4@_y`io27 zFeu0}t{xZt{FSR4t~ePAMvQZgDYs`k&VBl&tG6Bho^L@JubWC*43|_|Ll%t2_wexDDznNkMwxtN8Yk};9xT- z$TF@T=imI+)$1O+D-?_v%cHC`?F@3A|NgPJukQG>mktWDjH|~#K6ZZfji;U+xsDNI zcw}Zs#ab;lOJyYvmT~pSS{*=f-C;$xDYC?3p_c#fV|NV-vW%-o_HHN`F~*puM~>j2 z$Po;darMYqvQax2F_uTpvCKZ2iJy7!*q|WGxO(Jl4+SH}^2oK49=W0h1zE<`BUf!G z7%`SduKD!H%rGd(GOiw(eL}&Au{<)vrAKDCK|z*r^~fw53Pz0O!JTZ&)PsU7V|&Ei zAQX%kXFE=8cgUqy2PL-;ic+h_GOiw_$c#H<_-?ew#+O79+&+&(Bu?>3fk z^(Z5FP;z@H7%`Sd8Gr3+(xc3hP08(pf-K|eQD)+% zvW%-oSy7vk+e5*Ku{_Fp)cz{hd0DlalG_IbS;p0)WQI-2?U5N6F_uTkE83lykF3>B=5{F}Vix*pUpR96rsVcPku49FarMaF4Fw~{DmTX_J#qvG z1zE<`BWKAfb9?O16){U5Imgzu9h!+Hw+{-kjH^e^_E0clta5X$q(`o(K|z*r^~hBl z3Pz0Ok!wCZGBXScvW%-oW}i?nVl0o$UFne-ZcvbATs<<2hJq1ed3fh6yjXnvQ}5V( z;@>WgF@N;(cdrkwfAYlUS-*LmvC93*C+=T=>h3R|s2<<&io1&9U4w!wW0ka6{LbIs zyt(KPpAH2h#wz!ZpSW-RtDpHwdVJ~2uikw7)6a``n5DAFGOiwly(y|tlydeQTu=bV4!o6#j$|B3SdWh{2R5^+& zVytr229;JlR3pz4v!o!)xO%A7o{5YYt6cS;dZl`(XFc1^QdwjfSC3R2D8prBABugn zBk*F8wHg%Z*D0z;wmcMODPk;-?2+`y-rcBHW=TPoarMX%3`LIAS`lM;^f|1hlrYJRxw;x9<$kGbX>>m}3P*K{n_Kve34=TvY>l5R_{z)7=*`kPh z;bg5^go+%S>2@>)Svh+q6pT>eed$;$D#&sc9TkjF;eF}R3bMHV`|)6e3hzrt4=TuF zmgqgWw!Jr0xy-1&f)Of8N$m~myn-xd)Ly{|6{Q{Vwsvee6=Z2$YJH9hMyM$LrtfMO zP>{v--?xJiD#~ca+uE^KRFK6i(R*;ECXP3S^Q)E-Dl$tJ$~-oqV1$Zn#|0E* zWsgiK7@;EjZ8_V!Q5IP_7bX;B

6AUcm?zImaeEs30rn^n`*DDsrt{KtWcn<_QHO zRQN64*t=AaRj$XA%%q}4s3?Bzjb1<7Sr%EPN80mFuV93V(r@wJZ>$v+WR-EkQ_$uh3{-SSkVw+4R3iCxiQMdq0F&~t&FpViAOi!5XBsrRA{ zQZQnyx(ol*M{j-kw8D|Y3eCCN^0JriJPs*>mFjrjT`azE@wJ=s-%7ReI+uI)Td;h+ zdQ2%4p~Bd;Qt5l);Wa1^W3fKD`=8gAn#J1$vGuD8MyOD$r5HU9DH_X)+PiACTVCqh z-T`>#XD?v3PbuV1g|TZ@+o0B{uBw5`H5NR2~1=4yG& zl&o(&rDiV#%h#(%w!Bj)LWQww6>SMc_Pd{iH6ClRa-8D2*!tD^%ZSxdsMS_i{*0or ztY~Ad*&ekj^<9@%y0x(j%Q27ZwH;y~RwyE+N|3_1DqJtk-=`iwy?pbv=ayo9_628_ zzx5mE1OMX3UbMXP`i}(O@}($|wIe@t<(c)*y{9T3Jbrxr=X-xSpik!O>EvJI4`IeV)ZFeuNQe>jFR`J+ZVmZQ=v!uQqj8HK@OQ;}A z*UP+~XgMQPeBtVIGqZ#WvdW5Gm(@1m!3Y&)?XQ&`#=au!s`s34vwdE{2o)-6deu@v z7PC@69*j_-ew)rdRFK8o)q5~={=-dYTSiqsl^Rd<6(dyq>{aKM54~wBUr|Ap%2#Y0 z4@Rgsxc~g}BQLI*sBZ@qWGS!D&XN|PLVlloaw=c3EV9ZBUCz$-7NJ7@_WaAI@-E9F zOV_>T`<638g+}&skJLQhEBuV3et*WFo@zb1eD3bd2(m2QcU^aS_a2N;QSwTBZ5YR# z3bINbYkRxigLP+{)Q;&9WQ2;6gX8ILtQ8exu}yug7@?xv8Dcj(dQd?Yd!hH>eIj>Z zv5I&{?G=n1R;Y)^T2UcvmhLIWy$2)gU6#}<7@=Z*CQ?C`)}Ln3*bYXhP_KLc*w=~* zvXn=Z14a)%h~0l&)E(9 z-tRiMeCNNP4%~bE%dekb-f`?qL^20oK*6%8$m}}dK?Par5!ZIUR#N}yjc11%_6pX7 z3iZMzFFrR>D=Ns!F`rN{LdBO}bY}U3zjFQp3bJy(O(+gl=5`NP%4ncsUbLdE=wqJk`q&AlHu zHPLcLsK`6R#Hyu&ERBs? z3bJ&)WYP)6>;LG~`WIeMGhFd#X9Di~rrq@ek5A=9MyM#g(B5nG?Vy6J@BjSi_2<92 z=HOnzc1ZoUn`(wrJ}fhHq8*G-A&>98YAVCACT0BF8=d)DF+xR|$L+mu-wrCsQX70v z>=leqq0#dlZeBr_@U#cVoPEX7>^&Hvg5xx=AWLnSo{5Z5Av~>k`L~`~-+q6+vy{7! z-hRc*m>+XSs3>;{y(62LS5%P2`PR3bwR+*}&$ih|zh1!z6`%f&b8Y6&E67q0sI+kp z#|Rad-gCaK|K5WNveZxNiP3`*D&#kvOQ;}At3W+GdN4wT`fWOwP(c<~THkU;sL;rM z;7_LRYE+QL_20gj#5-h;IeXgIYmN90nGq_Ct0GTkhKJ{GU$68Pi!5QX zjCDOu(CLI}C>Sx8hih9$kmtzEQb}YP>$-pSoj&VsEC#bguV93Vg7qA=Hd}vgzZ)o` z`jTbb+M)BJ9znfBin2=G1B+GKr{}1gyF6`ZmR!j)*7X<w5nkr?f%usiMBFq0063d4(cW7*_>Pn3kT6XBkC|h4cc*4{yDabO` z^*Dj8v$gRpl@VjMqxKa~n3|=M+*&GhUBB&@IM+%nV=;tzuk96%99C#dq}K09MX*x6 z?Ao!CHtQZNW3jS*so*(ev!o`=Sl8Q$-yLB_jL{3}kz+n6Sa)NU+Gb+3Bj-<)Pv z*XI?AP+?pZxjw=p*Fp4^u{?6!WIOW2@1P*dxO(I|4+SH}Y=`h-ktdf2g< z$7?&VJW4-lf0cce=Sv0!S;o~P&#-t6Mp=rO#dZiU7I{u4T5gsUZY>qMuHW{wh}$8S zu^7V6e51mV!wQXw*!uMzhZMm|^}1`vM%t`fo?~EMsvZC${rGj2NRA z(u3!H%u-3LyRoj9(==bb`rtpVd3&_OJ4jvU&eB-dL$PB8BgU!~cd}AEwDUi=)_*f< zmP#VaSl1Ve_rC1P)uXqaj9M{bED!F;%ej@?POge-P>^L@JxYyMrDic6j2O#N`AUCd#MtApeKV20J1Eq5i^kyW4B{_3W@p3ER`fbvy64UwLDf7BgP(&?I|SYE3;IR`pqn3U2h{89*h{v zLnG_cTWyvM3bKstvCUVE7|VkvprTeSUzw${$TF@TxuT+0j2O#ZPdyvl|+`YuJ`$h5o3Aq+)2wmk>SizNn{!8dQ-$qWW?C**q&iw zzA{TC$w0VXMwk&}kH_|84fB;*Qmfz0GS>Arg5kl4 zu{<=gKJndVNhtiqT_y3$xUsH>qCF2XVk{4SC68LQd}WqOBFk9U+lq=>F=8wae)Ddt zc2JOITs<-~#F#T;EDxTBXxV2_kY!vw_&msnvD@KOHZ6-rhBHfLsa9qgR}Vf9GGgrU z*q+5H^GZ+KE0!y)-Zjg(dhmIW5o391WVdH=n6J!IS!5a8W4pg%#8@7hp{`+@?a>ai zR2Esr)gxC_C>Sx82hT9IRXZriGOiw(8Daz(F_s6Up=<} z-`nUZRc}>vjZei~X{0f@yn+!b^u}3lCs}^26%}O7x1*F3U-hKt9;LLp zeFdb?-i8q>bWhaojBMdvK~|PDuZRd0x@&8fIqMM-va)>>3Pz~N`8J{8tmbUj_-oCN zwJM{gZ!8hv8@BuwGNE9E3cbcG4P&RtoiX^gbMG%M-M8qvt0_k z#Zi>=*lhh7_w)7@nF_sU)O#kfM+I5RlNzg0!3Y(6UrJT1^HvsFWxU%r)?UE~6?|Vh zuOO?8cl%b{D;S}IV>7QHD@%%z?G;>WT!YFnde=*~aLreaP%(d3V_9S!J+JhxSdLWi zE~=P+btYOtmhL5<(`M_>y^Z&{`WxTqaObR;e|sB7sNk-~Z2j3Q$eQ0-GD1b}J|;$x z3bOKUH=$rQ;5)hb%)kg0ekaFP*OpU37H14qdmBcmU{B90$np~l*|k=TP{FxCRi6|6 zl}rq-u3o_i75=V9w(#DD3bN)`EhALu`>?+JjUH5xm3eGJ!3Y)l&ON=Ns30rz@q`Ci n{zk95&npJXf*KN0w$@d=g|P6N9Y(}mbNL0G zW^MPmYUd#DRy@|X<3(=1~I4aBN7L<5Iw6% z&r$65N;M)vrJ_c1`CX0qep|(w8=;z$pcS)32(fiL@}btHhD7X}NiP|dVl9(BcP-Y-OE6zS4#O#xN#Oy;2iI}^r&r?o}8O{i0 zI1;pyta|jas6~mgC^aOQgB2h8yU_eptCo)Kc+vN&y{5lt{MS#9n*8W?+l_zg&@)ZG z?w4EJSn3n6IDe99c01|CUXwEES;6MMd`+P#`Tu=aR{cCd4cm>s zu!QXdtvdT0Yo(fZ*=~(eQA2|5=6#T$)$u#eY4LI5C$~3^qJ;fFcJz++ToL1aP(ySM4T)UB+Kmq@ zRc`lMeM_s{PxjoGiW(C1*G|wXN9d4JQ9~lfn6;btk)xzml(409|JQCrSoU@a`(^Aa7p%ZX>q3?YR*Ww90awh#e04s1a*^fAI)4Bucx@xNh6%<0n7A zAQKZDL95d0iCFXdlu;ttxoXBd>yJ0TdtH^~O7gU~9&2s&&*^L94lv3s6EXYGJ9HF}TTh3InA zUiB^`es;u!LYRg`mMhVY=oYK@&OWLx{$8+gL0t$v4*{hrMAalpsYJV)cenZ5r3a}$ zCLQz5I?I*hm!A5to>!O7TVX`6bsa&gEZ2!Z4|b^`vE-zkwR)|t{J__uk5}d>nK*gA zBWRW7IuS85SnFOJGsCmnb!*i_86eA+KW_bNt>-yk#q48*vJVMbWjW`onBk02hNFhW zls6YvLZ}oWe%R7zFpSi*0XTHAEcz#~# zc?dIqoOTl@96zZLrXi8#O0>q3{?h3n`<#8_TbG39LA9=C$luJ`!{n2`z3cccf8V+% zC;Q?f?$xa9`2^Br?azFMqYv(DO5|%ya4+J0P(z}Ol{5zFg9NRH?s2*zIkRYPS1V{O z;%7DbpoWCzdC^+oB>U1;|%j54GFCofSNP<>cm&(WB z3Yi)b2k-p}Ba|h4JxI{%u4$*Y5X_0%C#WPEMa*{I2Q?(L0~p>`+N(*Uy_#0~k`=Ed zQ*T-Ohem%;LqdB)(SzfI1g*;R!EsIv3GH{}V{n|4pcQ8#-&WL+V7s*wwBn5F32I2N zXFb80W!v4ivf3(MITLw;8WNAcCK#=;(oWE-JxZt{@$6+!*{J=?jW28XAVDies4o?F z6U@%smw19267ejvH}AF+wDRlr#H2L?H6-F$rui7WyCXp>?lOF-s3FmwMM=<#yCv^~ z8WPOfo?u4i%841=6V#AsUtf`+70=$i4{Au188xl+6O($71g&`X?tM^0qO7&j{)0Y9 z&`PK3%2(-xo&+@{ikx;u|BH~7PSxdOFcPWZ?{dZIYt9*&cSVc^Pf$aG>)>{RRwaY~ z_Zf!-$GrE!-=Y;~J5NwUV(1#qb}d>{#~X!8>!^I|Q0dM?t*+;;Ka9}WE*e@Xujsjp z5+kT-5a#2*6N<6&yNXcB=>{v+J>6zdlW()CeB*BQb2si*D)mA6EtW9fG#cG&T_dO= zLEr5JtrVg1JyGm`=*_sm2$9rxc4%wt*9Xp zWg}8tjjh?cD^H&~?k!!eEV-KGiDw9ZU-=Rv_S^inF0Ha$X~mj}DxJ5}2Q?(*TX$h; zg?!c8Gpr7e9R8zn&t+?8t_CDWBCp$D^tW^eIq^Hw%Pom%NO0v8!mEF`%zPZR>z10o zMJvhjk@lQd?UK$i)}3|Y2sI?Q0t{jOqQ&N8;plCSpp|6#NNdp_U69T)7974qhZ+)G z$%gRJdFfQ`s1x5{Bdw`dZadXVb=m{RcBmo26?q8DHa{_Qyf7n$?}nQ zsrM~9--t^N8tqU+g1It;)rhV2*ZKrnCR%%@mw36KKkI{LD7;$9mL25|w1R;bNB}OPpNYF~M ze5AOFk(h`WiPVtbNk|AW+Ku>WjCK;Vk}Mx-e8drDghmuKB)CHkA&y!jG-^rEO0s;U z{dvp`Mkq5-LxOoMgjanr-A&vxBXmj7DrcX(Ki}}jH=2*xJHO6yqP#0Fk?=V&X1I>} zTv;^lqb!rLDM^>>PjB>Xh5cWZ$~fh z@s^1a?aJ-ckjP`D@*_&Qodm7&xXA=ZEj1+an6FZ%j=A0XL%E#=UT4H5;T%WM zitn9-ut%JERjNHdzN||P3I9$}By2=A30m=O7%Np8QL$9iknnBw^xR(Q*EXDqI_a*Gcsp5o}>>YYVB)CFcv~s z@YsPCC3ipj+Ai-3vRvu(G@tKR>E7Toi>Dc}`zw!j1g&@@5W@JDn=DE`bn!>J)R18P zLg;RpVZ_OIEO7*_e9h0j{hd~-E1sO(IeNB`{kB=MpEg{ ztH?UX=h?|nqHZJ7Ex?l#4zPN}|2w02|AOzZ*)kX<0PN& z*&7f=*6Cn5_bX?X14J#&Fe05mqEy{{-fHhHpy#*4b=IOMS<2;`y z+PfHhFN5cdewJ`~kO7F~RlJFk9! zP&153=b~}cT0Jz*`ECT?{qVO-Lx@^$Q;^_2RS37QNIq`ZGP#@YZYtgI@XZR|=?o?6 zCgE>>T&dDcmE~Ic<4u$0tA5RKw?)F2DrQle)s#hfua;$-OY)6b&h0T%8=*{1f>yj^ zi*Euho@S-eY(Nc(To0?C%{WGA#vwr~U-LZ=_{wdHF3p3SEm;pga}FVF9wfo~4Su_% zU8BxIc}AMwD2Qjs`f98B?UD$5;bbz;OGWr^mkL2E$;EdF&2N{Ah6K+`?dyTATB>}z zR0vv0*7M#M3PzM~mx_jjKYML{yHp5TN!IgpS{uK#FdyaHrJ^ChbMg4LVdLASLeNUG zo~K)Y=C?~lLxLyw_En0Ns(iat2wF+j^WK*)MwD-tiiQNw`t9o)Bg(f+g`kyWJx^ad zHNRad8WKF~525+(QXyz1S#_SKdV<=dq~&??_%=C*2nyCg!bNFw$PZho5IE){}SlJz|GdGp()q9GAu%zX4B zv7<3qzFjH=tt9JtB4V@~QNCR&8WM4=n2+9wGNOFDR0vv0*7HQfQENo`cByDc@E*^; zYBZvJyHp5T<-63JOPb#j`Q|a4*_U(26Iwo}h*V zzoBR+XvLFTPf$am^ix^4i%KJk1g&^->wQo|qR4}BHH?rIPj0;rYWTbDam76#vsxJ+ z?yX|Wi`0L-Ul@#qCZVF80488S2&)ah6KM#YA0wFe+m02)`J=nJd+#n(fci0`TZb$ zP(y;b%ljZft2nwW2M>K8uBRcv9PE8C+Ifn@IQ9fJBzS7nPSA??U!I_b1kZrl30m>Y HDTMz3Lh#{E literal 0 HcmV?d00001 diff --git a/data/widowx/meshes/forearm_link.stl b/data/widowx/meshes/forearm_link.stl new file mode 100644 index 0000000000000000000000000000000000000000..1aaf44e48689e33f1f58499f98eb9de0654864fe GIT binary patch literal 21084 zcmbuHf6S#tb;qx|CfH_O0Sz?PNF`D+f~l}7`#$Vd5QQZqCh{wq7hNhOL{thUiqjYeXV%4$j_O>n+v&iA>Wd7kI; zPv1Xa@|^RTGjnF<%$b=B@7j0MjeE~{?Y?*3@alKI?Tr8b->084D}P7k`ltT(opSru zwHJJG@y&fFA9wi&Cu;c<|NRf~U3x8H$rK?vh^7Ad)Y-`|E?OJ1k@ZG7?QN*0X~GiI zBvc;TC`4x)Dw`;_-Qw6&Gb}aoUOV!p84*oKkE& z{|oO9;#c>-xgn^fvTRgB{;hvUmY=!n>oIqSzS4L5(AxTUB`l$L>Gj#{UmiX*m;SeZ zwk_6#h@4kpPvyVd@xdtn#g_W9eb=o?+|dx!Qdu=s;?oao2Jy7JpNP6FA+hwCL5(SyVdj3x`v>Z%Cb?3 zZ~yF$Ac_Z;l#t*!g;#?pUTz3#wIlefmmXTGReKJeChu;@I@WfSpE&oX4Xu?2Kf9y! zt(?ovMv?cZK(+@H}5(o;;f4g zFDW5$`;|9-b!DRv7fun>Qh8;gv}(5qwTcoFrB5-h`kv24sOKc8rSi&G#k)bsyOfY9 zV-q&&2)_E>Hw1A}89@@%Qdu_oSrWvagLfXMgoL8Dd{uXYG82f?6ueMju6kC^1Dbv6qn0`j_A8&MB{|AM!E@YN;$6m5_G* zJEEPl^B8bE^j(C;#@hOKB`hJqaf)4SF8xQ!u2y!hs7t$-%Ce_&*%PDO@0>ws=OjTb zm1Vo%orBQsObLmyw}*{B0|-JH010ZTEE|26v0MF6mO%*#`cQev*+D3iAwjJv?;cso z&Y35@KO)%&51hC7*7g@gOK#nHbmQtvFWFM7o^$Ov3+d^v5|ohm=Usoa5V85SV_Q{! zilEkiz2b*qObNgz6%ZYqOh<3?`_RYd(o`{%9hD2-QU**Lhc?=rA&v zpjJkSBs?OByCKO~8|5b_WW#%{yz8>R*L_2FygOS$8@WGr)4Q_?YURG0g!Vt#@a`-^ zb&<&Zv76qVO;9WM-6XV+%Z7Jn5vq$s?vLH{?refux$h>Sd_*?9JBv_VByz9K)An$8 zHbJf2sgoFH`XZd^YXns;*_q;Q*XMB2D&=slYp5kcHBD#wmayKEKZcFzOy2~xRMz)C z-;Gu|)3<~~X6M~>rf-5;nZqYB%=AU5E)tn*uZSkoH$kn;;a9DSb?!`miYR^3&bjLO zT(iWPz6olntna;dgK(yA2?=H7t1%yD`X;EQvcC7TBy2d-w}eEQJz--y(>FmamG!+6 zWwwV6-x^5bTLZP-w+1Tr`EC;R*1$wrQPGlIU16hNwL$pSK!mkiZ&~v}_j!8|zBMpG zEtU1Xk3K>8*1!@HC3c04{`?WM#J2_}sHL*L_fa$m-x^p#qQu~^QTLDH)gTm8SQoWa zR!x=AEUbV1y^r?{P-Q%+1n>$NLlsz%%KBo@CH+LqerLw;F zyYo^ud~;_BiL$qcjs9#9gm3OlP)lWf@3V{`d~;_BiM)r49A$iSXM$QP>wCSqyYIJ$ z!^Um*-C4e#ulB|nIf;d|^t}-AeKvc__bPGdzSE9TLLzMhJ(~?}$huZ1TavbO)edcF zW(hbfbdGKc~4N*y5@bqloxuU z#I6n5t`UwzS*an>dd}T>p(s(-O?@lv5gWxVx$yhO1z4)q8istJ06sfSU=_@ zs8w1U_QtKEgv9!pNP=3NZ*4zvw_IoqYQK_ywC=P=P0!uj^<$-yws*&CkP;H>ZIGar zzmI)I2?@@Hw&#?PD4vcPI$pITs8vQbh#EyX+bJQDd*Y(*oa+c`Ydp;tlRYq3vG55g% z!G1(p_HxZn*q|h1iTlylWM9>ryYxwTkp#~o>j`SvS5NL;5=we0mKCI&&t6fqu;jGWPrjKR_NjM{l(VLvD#QEV%==Uv7+ zf?B18K{PQj`auZ^m&g5>BBYh^abjLEyU&;yGb5wx^t_7otcXlvy$#kyEwBG^KPVxw zzUL&U#i$ggiMhtadArUA&f8U1OSo?0G!caJc8%?j;M%Ms?zt<8GWti}{_|ssA1XT+ zRe7@K{TYCSqPB=8(~ntV8>)+1#S1}9+E}<%@|#;CUrpPfM3Gur*^j=aQO|n`3B_R1 zO-}88^aQosKlyNEqf=dykWkcKYojNqMNhXj=tad6)$7=<@}zgeh9o4)JdW?@~{DUKNk^z1~JuZ>=J=C5r7T zXTBTdanD&7ePypt$9B~#$rLdj55;y7c@mCto`j>^oP@(yitTQ@V!O&B6jyA!oh2lw zRazLtxaX9RaCzL1DI#agLaiFxP;8eZPhL^ZlUI}{=T$FJY!@v;F?iaBV!I@&i&~5; zt*hAAfTY zhkyO(aY{(=HFGw*;on~$#25eI*-H}CQdzd^Tk4*B&kEw;6&EcjA;I_0+3cE2wg++l zp{p8#S}My%c|R{Nqt_g~JMMSCw|D=N5)$oO>UZ|NF^G>m_dN|kEtO@XzP?`m$B)IU z?KwX^yrhH#-k^)_)4%k*xUGHGiQ5{2S}My%{dK}GF1bI5k39J2OG-%KZM)d`=9#ml z=G99+x!DlZQdu_YuM^Hb{bfP?>(5`Yq=bau)$1=HzWm|W1@WJoXEX%0RF;iO{NnIi zgLv$+?;odxgnu8PUlf$NJFCCBJLQ*u+Yr=JSvD&1!uQqt-4|@V_ZTHZLTjb|IswFm zQ-rkqy%K2EZc1!JTJBT*b%J~TKbL*KA*f}KRKni<$>tezN=RUAy4egP*bvmh$QA;# zWKIbQ%&|gXCN>1MFhdK0**>R)gx89G4KtfzMKuJqywWQ1-PNj{Q$hl3zSy{S6&V_W zT8J2hK=heYLc+1D{<7xsjBvMJaZy81%aN}Vh@x{!NH_-9-owg;KfWf)SMA^E^7}&HXS4s?eO3^QdoD5& z5)xTnh*qT1-|B6m?3|r;NVP;|W?=;xe=JX}j#6^#wC?#`d%w1SOd>#`(Nt z13@==;)tMD=8Wx0I0#BexZGr9Yl+Or!V67C76g5gXAa+y1;M;z>|GLhddW9>d39<$ zs8z;BnM{mR)i*({JV!;n>j_Fo3HEq@<-ml6`0^-atx64WXqs4P5YW!tJe z{lv{yo{D_?5#!Nh$T5O>@`^hE5_t-nm{%mIRazT1CVSpXNUWcUtczNlZ*9-%lRQ~O z+v&rWpoB!`@Dn{JL9IM9gb!OAl#s}?NQ}*Tf?9d@Y40p!1oJ!?(I?MszO#&3(wu99 z$TMJnBdEFLcN~?+>zo9AWv`Fp6(v)IXGuM2%LnqVvLZ>+(|uXreHT6^{DM*1-V-W2 z`m}_4Xd9*8zO3)-iQEn1rjYb>t6W$1DP9Cr*4{O3!;&GfzUKp?f0ZfU*Yw$|wjcBl36E1frLt8ds8znl zXx4MofaoP8F!NXEs2C5{MXloLsHr)51wjc3ui){2F+ze`#nWM9Jc5*vaEuxMcM~M2 z<@HhTMa!9UE`RCFnXT&bWU7|<#EPDmb8Qeh*Rmh!w;<@lIvUG_B-EiLt2HfqmlL$g{E&1%hjm9-{FxBZ}mgv<5Di#DbR+<0|O zEQx+-MX4paX=DqvR`k93Z?J-(gamE3{UAXttrgj5c7w1%2?>r(YXg$bT*9CVzi35y2^ix z{LG7&(!cD17tR@-%X(D3aXXaXyBbR-h>(!Ta;*PoP1sP$Hk5g}ta@$38Cl&sKy>Sf z+N$4d+Zi_WZ{`WMAv(WjW;sEGgv78_YLEWncVyvr2OD2ezyQb9=6SY-kwnsm-KDbAjP`&v*XZr*Z z5)!siTZI?Jc!N~5^e&?j#Oq@2I3WD%@m|Y>B9Y3|Z;-Nqvr}j}c1^!Qijai$X8sU+ z@c0eV1hpK4r{5q&NJ2s}Kl)K$6bEmRCa8s7qMUJtZ;;WdQWpuueA%db)bI_`1hu?N zOus>jkc5O{zHHRJ7Oyh#25Ev?*dsKoFMQiQc!ulrOp0QcN)kS3^QUsb~1^&6xJNl0L9O3r{0j5kOV)WVn- zV)zCrqKtV*0&}bon2GTQX@XjqyM@4Pj~7J|W$wm*d7o>d^IEAn-0%(31hu>dD=~b7 z6d?%-tohQa;Txn0Y9W>sV)zCrLjR`#5{_NdZ;&Ra<(N4A1}Q=k5{|+3#4>z?G(j!L z_DT%jAVo+*!h8Gpj8k^6hyco|FiV<9q&-=G^`_H=_B*aYe|Oe5CL|%@nO|c`m>Ad) zK`pkh>Z4V$L#kEmExoXwpjMj!uvL_h;AplsNKmWI+i8QDB|TdD6ywCqvVVi@B_yzd z7j3Ip7q#rSNgLr^Nl17-j?aUvi(131Pc5tcprj$1oPoX~fmN#;;b?8m`bbbh!edi2 znRNuUTvN^ZrU*$$c*Zn0+mVm5E^66_<5lZ=6-&?$-Ks^;n;WmNK?wE8`Q!q@m*h>UaJLUbzUO%pjEwfYL%51-OS8Cc^ErgEEEJoV4v%1er#f)pvXWv8*Ngl z!O9(7>VE^b`A^WNUYne59O*S4{> ze){YuvuQ$}F(DeFr#{+v;I?aEv%a1{1A=Ouh&syUwPT~9o)pPbFWfWk1HByG%0h#x z=$3WYs0RcSg>XGI^jo)7g@!7TjYiRVAUrPw8W8=SlS@^}{`}YF?1wAMZ2$Yv6VHWe z@+tAE6459MTXacyUf+*yy;(~HkMcU<`!Vc;TvSL;OkuLBTsu*zkJ)tR>Vs~bVCs_^ znIOu062TKsqR)z+HXna*i*=&)TQA&`Y7cWu>v_5%I-f*xP0+dXl2GSPU^6Qbdg>N+ zW~DxyUQ6UzL5%k!1oWY7Qkwqae7 zJay}y2_n#av@D|Te{gb^=tef{(wio%l0ZN^A+lb`^Yk~X8F{g(R_kc=ggP7(sj9FJ z5vS|9Jc@O%YDmcA1hx~gDtWrvbGO)u2o>lmEh~PeRqd4K|c4dwDe-8vzLF+~(JHHc; zB|=ov$kQ!|;PJfJ+$hx(DW9U}o!XB`xMzxp8h2mb4YT^=`tNhs@aZ~TRZh5Pg2>ZV zAA0J=^N+sPtZtmS{oSQMw;z7>mFC3Zy&q+Ne0`^Rss8Gw>=+yW){9?Gyq%-IpRY3g z*R402&BL9LX-LR(@^I%_Aq3(<6uXo+@q8 zIN^Hh36Hw)^E+#%>mDb33PTMiJWAD0cHZLA+lM`uO;5b9(5YLGa+(TD!aYt<;qin= zv4bVy9w#_&#}gjK=_v`OE06afTX&@-+~dUh={n(2>PdIA)NqdzoaspXcKXA`r+;r9 zX&fB zKXtFFh*B<(!bT&U#6Y+QTX}Vk`##cXF9?sqjvDDk1;Rbp($IZm(&1d`))s_EVMh&} Rbj?ehy9Zku{$8o7{{V6`EI$AM literal 0 HcmV?d00001 diff --git a/data/widowx/meshes/gripper_rail_link.stl b/data/widowx/meshes/gripper_rail_link.stl new file mode 100644 index 0000000000000000000000000000000000000000..e480ed586ab09dc677b178d0be3a4f6004a3ea29 GIT binary patch literal 17484 zcmbW8d5{&=8O5(_;$8ycO0f(n3-v^PsIS*OqT9B5dWl^R-tgZ*Fw&3c;q@GmAjfgvToEt3{Xxqiy3IYk#I6?! zMvy~@9$qIAdKftv0U_G1b38!h3AS7aqjQAyFmkYk$RXOUqkUPeas*p0gwaZbnlXnG zsYV}0(pGfI8Kc}rE)~Zf0!W_)U z>m;%f>_H?QK`WP|`Nf3RL}_hDSXHQrFDn+pG*&RK7iWn^JdIlA=pw-!Qg}R&>^ftW}-R{t+VUHxl(RhfCH3b(XuF z;0X4ob>eDUNOLG!z8HQn)pzxU{PFds%x zd)kl3_4ADAI<>P`BT5{3O;h~3uhpV!F&=~eHr0q}Umuj5w|s;}_FG$N)!xk$?&dR% z*l@(Vy&6$MN7lK{*&Rlly>LyAP?y%$xt7kIjo7&TiOD}E54Tz^pEIp)C~?yFhDTkV-u1YN>H2uAubK>g>iI|`2OK*h+WNhv@vYlUD9Lfa z6V+tl+=JRQhZ1k!G$LB!SMC1Yl{ClT1=VE88&~ECb)7!0DLV3#rSVVSk5Z!Z{jQ{= zZB3g-zCFJwdcp5F56m53Qmd^db|sHbcsG|riEX-@qG1C)z2&ep$3>&BNS1uCXS=pT zUAWG@61`i0LRLauyp9#@+=*?~1l&vAxAka53C-u+nmG%Mxbphxy-KKS(ludZ!#aQR zsJTWQx%<7n>e80yiDws{W5nWD*7a&c3GH>~7Oy+Oh`S%!IZ;Age%0E1!ye<@{TnXn zMa!F(%u96k={)u;$_T7~=RUZ2g5_AWYE+`->MY6UxO~wVBc3}j!aC2gE1|Q}x#t%Q zG-Akij?WS5%Gc_KZ8lmv4(dA-t2V2z61r}j+xpL|jd&;N$Pwz&9&zs8As1KNnagc= z*mvQm__uf7XWs|0xAR*VuZO3lF|S?jiD5Z{PXHuH7oV>_T{$-z(!ADk+|bt%eYSFw zX+CA@b0Wqtd{^UBsuI`FnHg<6-Q}KMi-fu`r&$u5{M0HMQDWcb8Bxc8ZAx-@SIq2u zdPUtm5tkDhQ38EcUMnTkwQAF}EH~SLe`^)3@5)Wnvi{Nbr zA#|23>FbDPw%fOf(%GIxl-ROzZp?L)Z->q$b*0y%hB+*v1oD;lRdse0_g-?t1DxTf zv^-+19ao`G@^u`V-3bb z33cfT&X1r*lt4btn8t*<8t)7`g6Qe)^IKc{k7hoHj+zpU=amxb!YU{qbB!p`cqVF1 z#_jiw_~`2%wRR|-Z-+*dxZso>;%7f`U$Q>bb>F|A?e5-VXbGW_{nnq`-F@&_b7}6z zwNm22qubUTzf)9_L(5W^zm9W;aw3Z;@zLSCv<@HC@+E}2x-MGY`sTvXC4@%q8vB0h z@nd#3*H^2R64!R@741@8Yol4{tE?UB!hU?q>g!4feac8{5@WXG`JsG=(})s{+fhPT z%l({gd^gAiZ33WBTt7$|D=mCB4nqrRr z1YM|M{a)*zMH6`zwL3kYAo*kgt@nB7{IjShvS-ok*+2>EVy!Uyrj6O%&adFYQy6lj zbF91g^r{ghnB&U{JO^nziiFlGU0sD5=Eq!Xq6Aj(h(RqzHzw4@J5jM#8c_oK%G8~r zk{n8?3%gx>@YoVUBiK>nJ%*Y~OUk!HBT8V`e&!Qbl0yk~;X22rn1c}|a4$Lbx<^Z9 ziI$})+_H8~F2*$hqYss4-um`Mw3U$`jo6)4jxYxRj2s?%On?NceMOd;0IT-0rAU98)8xwSaZrlz=;8KF^E4HIQ!Mz9 zFXmtbF3=c*G@q69#e{EdmXGi8$_c(VPy)JqJXjWM!ZGJNgklax$_VDDQ)z2CcY8ul z56jJCxbX=yg+$uBN|?rVbK7|dlqV2jkD4aUUM~{TKRsSYBv`{DAuZ3|#2`2Out+ch z7wEKik*_`;xppW)7ju-ioGpY)3FxmzNXzT9Jy$F3U6zY{jr$4_Dc6s`jk&!OF0YlW z+N|8PO;%E|ca`w9%JSiQwqNDi!HAYi&AD5DgM~HhKC)7+&IrVH&&j5qJj8GSz43ICJ2nQzQ(1;RxMw-u|gt~CIE6?HUZV{Zy z;++m$I5+nBOoy{qt(8WUXq-a{b!o5X+o6$uM1&gh?hGpQl@i)=9nE|WjVQrBEap%` zUA)fwdA(K|QG&WS=4Aw2{<`g`r8%;QmaF|%Xn97ci#fP6F5o**>zx6q{`J4-Vjt2^ax~M~r7iS)k+;+%F z+p{!6jU4)|&pEh3fu{>vUDUYl->Y zTc6dcowcG1C8>)V*RAChwp=6B$l*`F zXtpt@3ni(G8rL(zW}-%@*^c^IpYJEssz4wgUDP2*+VTqi(g-!jgYPV|-YpR5H@c`p z4%7%W!(SSqMh=W@{rv~lpPwZKLS59%VKdPPjZh;8<}quS&GrJJF6xjYT~S78gc>=p z(%93^VHSF-K&Xp4kG*gEhx?gF88aYr>{lqJ+ zmCaqcP?ox=LyolNiMmuHhn~IX_bV++UDP25)*otyzcfOP9De+>cL@3YiZ0OVqQ>>Y zer09hI$g-&$6wB=)7fsdq6>t&sBt}8QATKl8aeVO+qz%Tg_6`ojqCaSN+Z$ak>Q&*rhLe250uXDK3U;PR6 z8(q}!WOp1RG(wFW7}@guiY}C-E^1uQW}@ZL2sLtGhO&lME6nx+p)Tr>L-#9 z8KDtsl*&_8Xy1!(f8IM~&5Qz}F6xjYTKrJ&xQ!PWp%H47%2QQn=RUjXXT4KyytF{5 zi#p_ZeEzFFD{cJjZmXh zp5j6~x7XKZw_nvVzd)#qI^}KFSK(#Cmc}OXIy)MP#1N`vES0kl~ub8H9{lQD3zzf(9X@9 za6)D3@?8pqx~M~rz0N${0)Wwp*&M@yM7gf&xY(~Zepl~X?&ZG=XsQ4hYshITI6x9W*iZP0X~ zEOk+b94XNQLL<~H32(-sol9%g3r!c>K^JmEhaBslnp=5g@;ydqgc>EGr}5Sr+PSr_ z&#L_Bq?ZeXx~M~rbe1HbHA0Q+m}7Xi4eeKCn27~KUDP2*I@^uV2sKK@TEW|JXy?)u zRUp(w9de|r)(DMIqg1SByi14n`)=473WU0-LymO!F+wBMC>47b-nc{izrqezAk;-2 za-_Sc5gMUJsn}id#vR(Zbf+#5>Y@%gwuHNZ5gMUJsp-8W&Cwq2I0Zso)FB7%YR9aZ zVT4AgkpuTcyy1s-F1>RW2z6119O>QJ2#rvq)by^Mu8;Hypg^dLI^;;7GK|m&HFEIH zIJDm(zkhZzc=21d2I)ds>Y@%g_TQ~Dd3eyTjL--*O68kzXy<-8dqFbkk?93OUDP4R z$4^$1mg(QJ{YoR$C>0v-(xIJ83ELCtLRspf4mr|VwS(3OHLjygct;QIf2|%`ULe#( z9df+$?Cj+3-NGG5Bh)BWpBer7m@+fj_QE$T2VE#jUDP25$_SrYlFru{fBPp^`-LI2 zf0DIt^w9dLCHT&P@5s2S-(GlTspq2b)DnJ+$6x#w53MJx_?w?vqUG{C4x){7XhaG9 zUc&P6Zw>#CVs`ebE`AqLcOVeK??~_$2>eP5ns+;B{aB7RoRXtYN?PuBhUNC&nN)r} zu+H&|3q*Bn=*GXbL?cS1YcrjRbtX@!3uDfat^1KfBT8_-sT(>^s0*iG>^G%qiAI#r z)zz3#mmeqly^p$~Yl+@1_3oU`1wS6c`i+N=U<7TXbl=r|Uh~uv+%55o4O}hzHmPpt zTB0)pYlUN=xs}T1mDNfKKPwybu9l^)#(kwTT-SW#*{%^K(%#M9rLrxxL}AHnSYp)()5MERbm5hXCE@k??oAG($(p)QiL(5VZPrQnRMwGyc!Y|Lhm{1pTG`#erki#e1~mp-W}RZDb+D~;7P p)&HVV-O#l}BT9IUl~&S@enjSKJRTa+a7ClS{{bqbhQ$B? literal 0 HcmV?d00001 diff --git a/data/widowx/meshes/shoulder_link.stl b/data/widowx/meshes/shoulder_link.stl new file mode 100644 index 0000000000000000000000000000000000000000..636d414033e1878b6d0afc26401b6b582ea0fab0 GIT binary patch literal 11884 zcmbtZf5={C89vmA7z5cHXizfd(rsU~rDVw7_d8|l2Mo4aKS*hF)^rZmmSsU??aLfd zx`-57Qi^2%xG<^xA${-r9ivF1m4dQ?AYx(@i)t43hx?=Zx}WPl=Q{8699*=2c609g zdanC^?qBD;-xm)Z*uQtp$8J9Kr2~6!{KA_5``_QHs(NwrZS~Qo@0|PIxlQxSPyb|b z&x=ovmYuol5y-2)`IyVo7mgx%C277 zNux4CHM58=^0jA=&RPF8U?akJ2Kii$=aXA6oK{r{0gWP_J<;433i93292I689P za-f#vT=xo|AhTtJY9jL4nEGBRJ92RX0ou#Ka4mRN;{xRJhBJ&hnZ zAKk-Ygj(s1c+MNE67$Tfq|FpOL}jo_+RT;_ywM(_tE+<-;{2{kOfvz3J?}F4CMog~ zBB~_N$dk)nC5k|mgca^BWJF#@s74VX=Uyp0JJomJ`NrX0>xk-p-L)D=?<}zz_Nu25 z1oDr_(=3_c@cT)(+ga(2Fy|Mcep8j;EFj>a^+zkmO_vc-C5nKI=U%&Wx^Y6oo{uU) z&$CKqW@4*R1mf_MVwSWVsJ`HcIN%8~TSllx5h154QLmJpJs4HedRvY>@&QjgYa8_) zPC1OdJ4>vFz3OQMfjVyaX7~978hX`Q>5X{K>t*l7dqdhdfq;inG45^3Ii|uaf>qK( zXkFoS{9CB6zva|qam@$E@ao~+%cmDS{w|jp-zeoHStrEbw|#%~+^SDZp)vHx&@T6G zqyreo??1YF^tN~Jb}MoO8r&`mUCKZ@ui{58Y5es74Xy?IgVIb0|Ws%-czL z+ee%{HX5Hk_^M}c^Y-oIznnYavgW3bpvG_U>}ReU&tJ96Wujfq5N(yr+-_H)0$HfR z5mllHyXmoIvv606P^;IC!=qKip{2Kv|8w`n0~}GWYNdB5VOY+2?ih7N5B+|K_UC znVniU7jmw*xbx9VFme8e3@U=Nr0f;C*4fnx-p2Dh^3W)Pv#ZR}o~2e+W9`iEb1?EL zGR3-bnK$inAE6pWFb=PUVT4*SHw6yWC<1a+XrE&i5mwB3!7Hs%oz%wBZqYu6Y6^%# zT`7XHTbVo%hiVFl0*6+$_59R!lyVdh-OI;edI@n{fj<2^`2KRo9blv$h~WQ zuHbo98Dbm=)hL3!IZlzU+yDId)Gc3k4n?R{$cf|r=dPbW_NT9eS9l{ELlaig^W?bl zQ+Li^`N=P52==*Kg`7B^e(Tlfko(!Q@At#RN-VfCG{d%n_}%twK&5FHaBE4;=laBUBSIIIO$cci*ad z?c=xB|9s+DhES`J6UT}AtM#VcTOFaAkilV70`Dc9KA*n(YxN61Sdk&rD&)km?9Ac% zrFZ^$gveDBvT*q9swyHT0|>VwdNFa(t0h6WCamPHIDHs-hG67w6>{Rh?f&D5V~$Ww z$lzdZ$iWO|2(=11aj;4pp_-7v!RjFgE3rZ3VTH5QD&)k$YIlTcLIwwWWeV92P92pY z)GFk}!LD_LYC^`j?0Iro(B6>{R>R&#`ELI#I^x3`-!L#S2A z!QtE45vmE9ama4~=GD}8ZH6$fTC0!~hwa)W+qL19TR{_6%!m9kVYjbYs3v4^SpMY~7TdK;wrewlT7{fAtP&gBwT@6t$l$O##&;WYZo9UzU7I1) zD&)jr9o5*bb%bg{28Z<{zV66jUEA2M%@Ar8a^kSb(AchZgla+ths_my4{~1F^l5C@ zW(c(kIdRy8Yi!p#LNy_S!zLfz$jM<-w6R_5Q#7n_mRg0JIBZfkwrgYV`dPCGoAY>Y zCx>l=#&&InP^*v=hi#n3cC8~+6EfnkeT6rD?m^pXjqTbDp;jR$4%?iK?b_I!eik%g zB^=!6M8tNUMYt8wTM`Gon&++s!Zl$94#qTsY+FA?o*~pKeWN@&0$iYgixoiC!5ND}X$ccm1?g-U{3=a0nINcRHDnqDM$cckp z>j>3^jC0xZ!Y*sgViYC;AFXBRm*;WC6;g`7BS*J6q~LNy_SgEN>M zoYWaYtwK&5+y;(NO~~NjE+L2QT5OyQp;jR$4sJC^s3v4^==Ue8DgG@q%-R%Zu}UV8 z?WX<9D{+K9{w8Gz|E*Z89BudJe?!O-=1KdNYLVlw4*tc;?sC440UUZ35zaB}m1-10 zZb$byR1@zE5|ew_M<|CPc+&+AMW_|?UqGmamFQM_(y&)E2;b*}iE=1nIOmE`E7n3GAF5FV^It$H2kY3ac#_|AKXTP5!fwRB==0lEgj(4x`1g5^ zP>sdmU)ZqHlZL%ggzdy}?8jk*T2bbAZHHNC=)31xd!F7Fx>hxc7>+{`YQ>le9I8>o za2&?bZccWW=iE;6-$nC$s74XS(Y}3jH-&js`sEJivgZqxs2Y>o`AF_U&s{XF->gwt` z-F5CB z_o{#p9kqyJTPXszD$vkT7)J@h_bNaD?GTs;j7H`q8j!oaW{=O>XJ#FDM^$B)7kk#6 z+cgKes>_S3g|^S#%qfC7jF6Tpr6r3F5v-#{qfBDlDB*XIroaCd=FC)Y(>$LgD;?0JWqAc2P1q-DI@2$Sjx|43W7P_OY97C zOLg~(5#B>ne3#o|DR09R1Y6O2N#@_5K}L8FwdimhJcBIdd+QO`R-7IG?u)U}h`XQ6 zZeO`izG}lMnSUNOuj=lb_Rh~6IW)7nY;M)k*^TpmUvg?@!7X!SMV(SnFKO}VSVUB{0S=@%`F-8_xbhW z4?dg`HCga{WyhoU&cC#ISmu;3W>=i8+TQP?bpw$%W z%pfDe^&z89Wc1nVW#rrzOR>KhBco1a^jVCYIgH@-XpF4CGSrlvab$3HewDVLf`GUwPCWl zPp9me^M(ZomRf#uz1iaCq`tr6UqRed_eGA9w{qZpx5VXtJqW105| zbIu%Ymt(#$iJZ|0Krmh5o0>tQWJl>X=L=x@1F>4#R!fV#>lRj$nI%( zPHu~(!WsP2s~hw3Z&jxWGpu#7e;o9^v#we?^pfwMZx+A#{FLndFJ2lk2P0Pm+t8S| zuec>{Joeb^?+%$9AXtih+L$+weIUN4=K1W+TSf-H%Lul*F@qoaI)3A{b=l=(`vnM= z;72;F(aZC$ z38D`p!ugQgDv=y3EBRHd@#E;mKV&op@4ntt>3YmQxktEa^SWv$Hm_gEkU)nB@{(9R z18u3tj9Xv=v@w!f=^mBWy$J2<9tE0P3SxVW#`ZTpsaK`(90-l`pj`$aITkX2M5EgJ zOTNiy2C(aj?=x(XAj?RY!wAhdp8ouYjO1{&nM`0^mSRgAqd9}*S1~*syFSJN8d=6u z<4qyUz}y6RNvv50IE(;|tk3m|vc3|8O6i)1F9exo!W?cFG_p+C%^1n8Gblunmoo+$xqF zE0X*wVFXKQ41Q_DccO##+Jrfbz&L7sS8^-e&jSQY>0Z)c_}8L?a)yLCi~x;{W5yRi zP+n4kP$?j=Cw|ok2znC^qA1TGTRq5kgZa>126Ld9ETXnV>hj7&a;&W6S21%Kp?jCJ zF6D`d?&s*Q=7|`s?#|9ul#3?JVFX8T**PWOl^hHCZh&B^uve5*C(L04$9!WXw@M_( z%1V9}AXthmB5!w_MkbPDWhK7~Y{dwUF~-ozM1Wwaa0V&AO5hF1uWoww%M4pO^b*Of zV#%?PUs;6u4#z<}zOOA#7DnPIl`?N8VTg8%NA-@WIml14rV)Mj_H#V<}hMX&lXlk{mtz~ z$2B_+w}_)Io*p1r3N&;$gq-P@K?m+ymM&Ol?^wVAtx}A(=ZOUbbBYm)Hs+a)|r7c1WP(#S3~T%wa^wzg=&2 zh+dg};{Jgi&ZYi7b6M$8>HZgs5=LCF4DieM?wwK}9%c159A%wdF#sRlX+m}E9s5(66sxlU@6eh;Se%nix3MW%wfcX3)Wd3E>eqCEkdjuAXo}Cbd<(+ ziPSmxC^(>1iqT&sbYOd{yRfTS1a>t>uoP(MDD0dGbYSOX z4kKRvu9MYK*qto`yE7wL3N&;SG60J}2EZIf{A_18s{{Fiyg{y6)oEd{m$0&OwE zk35%dD@CxBx0^?BUgAB`+Q)l%!ER`&PH#^|$b5K2alArX8m}Ow@d|W$yi$aZD^|)! zB9ADJS3saGM)(+$OOIEIU@0GoJfb*W0fDv{;bTlLJzgn-rFuoTa%F~zwL5Ssg74m9@xUD(yECt_EVU8YhIva4096lgq8&wZ4`2-#6BIz9JM z1WSR&^Fjt-d!@OLav0(Bm0Wu6qX?GrS&c^&=RQE7Ek^k4qZSe5K8j!|pVid%%I-Lt z`=lXcOxSU1M;;+kYwn{6mI96E1?yUb=03_{1acIeO=F64A4RYfXgn`?iPfRGk8&8{ z^A+o3))PI#c@^3F5QT&br1lnSRe=o_U zzvC!^rTp8AM-;!~0D-m`ft7~&Kzwv>b(nKlfZJjz&}hjaIJQd$kO9Z#k z+TGbA^b9}|Eal(sYcl}52K5X82<+#KU@MfK0Vsl{*y?h}vTa5*_~`oCInOojh_frb zA5o-FtSEsxfcOj7}c4?NCOrl+vzdf-5t%G7mSrn2(7Y`z%sgXEv+~(@L;nDXj{_ z^AN!topVSvYw~vIf2~7n)r$3nI=Ed%Xq{YXuNc8nY!PGjcx|R#gEQ8&%=;5T8x4TQ zT0xeJWYQd8rQNFv9DzXk*5WZ@3hz z7OR)-?hx*VpVy`_;!h92ui#(oKRBD#dN}qsV;c6}bMd(^%#0qcE~{b=BS34NlrfWk zchr)uM_eA=Hh4s~7@<2cFvuA2Mk zl|elNTLB2IRFE5!s=bzOnRsEgZ-;3Xp&Ukpwp!hMZFRdIFJ$lQd|`lKDd^DZ6l3mw zCaG@y(8t+3e|4@!D2EXo$BfzQwp{g1t?Ne(-#sfpuoQG?b&4_HuWnaeGrvuA>J9xZ zLOG1!$S1XfAGD}G^{Fn=KYI2E5G(~9TB~SG^L@Tv(&oDX(btc+vIylcg6GMYGiF^- zeMkLa(dCyn4G=5^9kn%(6?+V>-qGs(=(EZ11$BMJh}tuDhe0IZ8tZ;)?Dia&q_P zmC?NiKAZ3P)?ERDr5x?*&95$Om?evluyZlt#PW zZeEACZWh*6DYV5>N;@5X1|!TMbCiY-KmWGM${6n@5xhjD&=yN6?R0ogjNpmPQ5rgU z*EYs`do)43U8T?#ODXMi_!SjlMKMQd=-_?cm<^! z*XApu<$w6719Oyy4vxWikH9}y*&SM?&=yN^6}wnBc{cBLEJ>4sju0W^h%t`G7TsLU z9HqT=?e{I9jS*YL5`AE+03o(2NGa_`F7pvf^ob<;EM<<;Xji{baXMtq6Nx^U^8mq8 zN_*?}=#fbDfnP0Uj?!pX{LOy(;&h1bCK7$%y8(iwly*8~eIycnus)VDM`^UHUwJqk zvIY~0K3IbRf~AypI%J(E5`D1Fmoi6bv@7dB2hVUiB>E&0eX8Dj^UeUlQc61=9+3xv zIZD$wd=&M)icfm>_5k5~Wu=sMIxupHK8U;8EBp0{N-4rJWAH zqOhM=Ge>FY@axg*@T)eF=o27VN@=IVM}|bA&l2V+4IMsWcpdOC+=SzQRSIpfl#fb2 z%kU8nIyAyrP8tIG3Q;tX=o4fz+OD^5A*(TF$B#2@kIwJ1x>9Q^@!sd@9B8eL!*?OZ zJhI?Qi+HBbrhvnUi`UxkKXRWo!h7v2UELl1V?CzZ91iOOJwe_kbO!W8y>0Dz?xcwr z$F9}a+uMwalcq$>VT9JRxZ3k&d%t55ohOb75G>{Ei*l~Mz4=&&IAHjgh&d?;TO%%2 z-Vf|)N513BGXsRyEur1=hut{d%VHe2JlW79hBO}$F^3Uak>tKg>9O3^8vJ^9uK>YP zN35`4zT~#{#L2|Ev$|Uy*Db4vn8OIIXm#Jpe9>T#MKrtfpa8*AzG5@yzbmRg%p#D5 zGbaV%zeMV{YK%qP_U7AJMrb7{+6}#AcgqPDaaQX!S>`Z8D?#yXjxi5xceiAZts4~} zSSs}0?ydKXPk(Dh^v2h(E`~4Zo2k;MRn+Q6cly!2^>y*~VN;_49d9aQP8!11Kvo>J zHooZ5@zJ1hGXsR8A*D6YPRH6gYvZ|hjgRJ*wz0=M&^qa`tAF(NhU)?ZODXMoCDO^_A6iErZvC%v<|qvvVcn}(HW3SFKi&CWxyBMm zDUEiyYStLBtRSw-uDSdDa^|EVT(88kiC8%M$~jvDgrXr8_DU?9h=sFHj^0|%oHT^f zA(lg2%9y@`YGb9?- za6;$l9B7S1uKJT=Vo+zv2#uBNcc5OBVq$<`DUC#~CUyLh-8PDzAQoUw3L@taa>ig+ zt(-Fi2#>Z>8i`z8IK{-EPM;APD^FjEO7cJT`MHZFKlBR_ETxgiRjXgPpu!@4oX|aC z4kI*Hp1pRL=%APwAXrKxk*m6YI2vJfd~#vKggGgQoI}W|ghj~7L{Jg$(N;=h7pmi> zUew3LU$rkT`YZiUd5z zIU}Cib5V=HxhQiO;qU1EnI+bSW=sB7CZqQ_XcshchguzsP|l{yKEjQI?Fndq2bpm< zqiG2A*WX}R9sU$IrB}>hgui3SxGD?Q!3dV}H$NFyGrl(l@?l;5iF(8OMAr}M$|nHMzEC3rX7d7apxIi4kOrZ;W&J? zkzH%Py2-B8x}y(s7$IxE4&P-2OGytcqSRK*VT4$urp^dgXp5!T^TWGs)d3$>TX`BS z<(%*G{kcb2P7xwgi}3Ri;lErv#5F!)Ee<32)|+pqeXPv68ZSmLhY>yo=Ym*bd&LNr z@?**c^I;Jz<$c$->pe6D!JITium-ITMS#PIur7l~&Ip#`$_OW~r<^G>XlFC5_hB8( z;fft@scs#N@DbJ5;;;^Gi>35=>3lGU5#HNlRigOYzKxGssniyn@wMslIEi2o59aUgz8v zOX>4c>oSKC>L|7@yp2!H63hQ2rb87WbMx(c= zLUOi}I)DUvG^bV$BS7Q1i=ulzdRsd=?~CND&wkIT-)*8@rSUdd`_1U`BlhpG{*x=B zgI}r$zFuYoX#L9A-ne)0H|`|}m4Xia_Su-P+wEJ{<(!_;yE8l6H(~t>6H@wRaj0Xj zcJFuacftvC7y(-K$^H4*r#tvN;kX!~Qem%N`J`3Zo0^wP&!;u;IL=tc^P!*^AN!tMdOTwYeMQqu#~ps>XsbF-Q6*V z5lTA**DZ0CmY{9^11U#8>23AX2q(prRV6h=SZlT|?qS_Jn8S#=$H54e3VX%2LUcy2 zP_ympsqI@M;v=4y#zbFXXftHwSCGQHEV3xj=^2@F7=ax%T?eaJ$n(u_=XAlCxzYHQiLssjj?@)fQ2jjYt$(N?kdVRy#!;AFyP0L(#t zrBd)1ZK-Y@j6miDgsYC`ad2BKrO!*P%k!aDfK=Tx$Q(u>t3j`Vx4S~_0}k3{DQzh= zGFXoThY`qw>Ku6i!BTAX(7L>G#0qu}u!`!o6>}KjcjuH9#T-T;f513`vn6XQMzECs zZYDfia(+eMzHyY0xv^f7x;_dVMzBRfTX9<~rO!+KiaCs68>Z{<-e>LOJ;wI7)UU)d z3LI{iJ-?tM0p;U|%^Pw zbufn!J_hFkFDXH=6jzmpBlq)czxm!aW>!biLGUKJ25r8-Kj>-Rg!ZQThZMvmQ5=A#mH5KHXHnd5g(i{O<}sU8&)_8m|xLFv9N)DeHp~EG0d( z-d<`e<}gAmV*U815iG@P1*32|JI)oIeYzKNA3a0FbD=YO`Vk_uF4UuK<%rVi?I8kf zp}$Vb(H>Of!}Xs-lapz%C?9Ln*1FCc(Uw^cC$QodJp z>gaAcs)G=9j-wUaTd@j9@9YNH}t?V)RvdmFS7S(UjW$HLA zf~B~(sR-L&M!?swF6!)+MX(glY^a0Rgw_$#oZH@p2(}3${M`Ht1WU2ijafK+RsOSA z{}#{dKPG~XQL}s7`n}J7-6((MXPMjqe{`NGXIZ}HZq?jb@$HMY#81o|X%Wg{#CoX& zd|g_)cmt~2?&{h#es{&*$xAN}4G=5^-De!!C+BMJ%D($;{KN-slB0GWZxPC2#MMvj zlh5`%Id}do(DB|4H|Iy(T9JHtW48doQqVEB$0@$%ZuRBg=AU?LTr%{T_1UU-8|9Du zXmIYn)92cCH}9Guw(b!KIa~AZhIl4)HZU3Hjgj$h;dO_ALzfE;PO@79t zS@F0JFSR4r8ARXVjqod;8#!$jX`p~$4kNT$$klQ_dfb-y;hSc~*}F#u2$tfxkzWL= zuAdAa*f~C}{TUH+7@^fdu9owdsXt3*FP{{jRoy#4uoPQFYEl=rN^UrKR(`-40|Hwy zLaT+GU%^A*Dew|*i>27o##Ed$CHdLB%iUnH&dl34u;2%Ue> zYPY4Y;}=Qcw~O2w%1muvfGX z#?0Y%K_jl99@ZG_XxQ0gS1UoN6cB2WV8=;x2S9x*q_9Wf{X=)6+rRostV_FE!W>3` z#%|{-WwAeDpOQVQ1ff#823uW?-;_n(gZxKwpP*N)Pxlg6DT}-Z`H$p2S}AM2gb|?* z-z@!S}Ln2XpeaW`QOBiPdNmuMt&?I0PfWUm2&rNTLqBB_rbdVV1%Y3}rc<4P*R z#wf%pMR?jix927HED`%4IC{RLv_CZxbnQ(;?O(5RZ57Uztdu{KDYW#{I_T60xBCA^ zuT;wSu=eCG7>Dc`8AKUK3BvBkN*7MN0tB}U8v7s+y8D!lL;9{~eHd+l#`AO?%F({7 zb%8FNss+6&M))%_JEn9U+F$5U@AD(C!@4|jMgWzrgApo)|JFSY$D8K**RfZXe_vj@tsQQlQm)!m|wFh(A4!n#Q)vEg7?QXUFKsiW8EP z?_O2TZQVG&EHm={LAmMe=2eA=8Kd`%Hnuq{d8*}&<;-EknakT}PAMCld;KH(-QA^+ zXV(m#l#DyGSAbwC?yWHw?3fun@WErbHvhGc{tA>mTbO-V_soeWACvq38}}O!u(tE| zvO}jw=MOwOexP#S0s z7WuuUMx&NIH+4$#?znmOH*1u`2)48_eeW4qjc;u6eJ!`eQh1J?$bG~6%Khh5=zmj(E?)Zk6 zMlG{u3-1`YH9#mDQem%PSy=c< z`M#DpX$YqSmW74aUi?~sP_)(It>6%scUaNE`>w4K=4?r6c#NJ0*!9uDuMdk*4)zO0 zfL0GRM%G|^zXt6I3nNrYz0&onXRkLp`1O&1qwRvmn!tNB>$6d`%tukJL$+_ii~y}? zHF7Tc{lR4;M;(^@#~a6&AXExEGy{~oiA|l#@Vf)@n*$c19Bvo1K9|2s_C-{NzaoHN z+vK)b3eWMc+iR!RKfcVzM2k=kBS6=_70o)oUzv}IB?y%QLR&)Kj(-Z3PKAt2BOH1H jX`LG%G?=*4}$Pdsuti z=n=z*)af&J__*;c>->NCd#r?lziIl-)ismb?|G!)p+D<~uRdAVTa(v2lj*s0N+sv? z*k^tRfSRU}gNOm%I|Ut^mP<0)MWBw0PNXNjpHzxI7lG*_qU*Yh<=f@!)p_f8By8Rql;d1!6I-4$SY8scYID zS<~3$9n0q;kb{V&Q`h7R`pgKFBHabW*A?cMEWbc3A8W+bfgD6st+>B=i?U5jUpX@Z zrLd5wSFn_Id^LN1qTi#fgJ0*gNTeWrKw+Tl(+s=;a_#s-Sm$(>Aso>krim>K`BL zH~qQ2Mh+rY+?nBR>e|j&k5XJ}Ep>1^?A?GM+x)LZwvZ>;)< zu!WIgYoGFQb3@x%?Sff%`K<>}2(n)O-B_Pk^$nlY_QuW4G8T@#K&hbH2mbd9ZVmR0 z|IO(+h&XhAU2n!kJ>ve+v|)*$`m(GbtuBEMj^N_j_xi0X-y9(a5m+NiaT&NQTn?U|_X8Pau+`i?IPmTF{PH=SgMT;qEkX_=P@hs=%N+5~ySYKI@WmQV zA4Dm%W2FX{Y!w{KD-rZ;pq!Bb5m;|Zb^M@h@ZN~^{_`K4?;ubLEng`vHJ6@CjvPd| z*3P3ekJda=BLbz|9#uH!ObfbZRZX6HV_Rmq-77O2Pft(QT9ISujOpnf(FOc_p}bPl z0^}g#mv34ok8XXjT`Hpaea{vwc&xU^oMYd#^hhcn=wc8kg=o}$=3F6Sbm<=px*a;( z=sq(7rLaa^1eP$SO*#IzTp=Q^xBsnnjTGg|qjo9=fgDUXF2`RWP-;x+A2X@0lBq3- z97M$R_7@10y7azhGszZ;K_CYaaclks0;T3Y&?NK46U~Z2Ag2hT;I9#cJ}t%G8I9n@ z=!nbVaV^IB3?0{@p+<1Sb$WPrR4!R??uiN^&r3|Zh+$Rk)=U1e zBD4OE&i=DUDw!GjKG&QkAA(8+nV) z3Z^`BeXw-h-@-{Bb@iUfxGewZpudIe-7hRrW8=j=%Z3FFuZ~Mzap#-i=k-7HzZ!5$ zfE+}yKK7Qo-94M?YX8D5QQ7tD6I%}67WASP>T^y~-JS;fmTD5lvq#f4?~z z^zg>S$OU%=$Uy|_;FQk&e8;6q)y^si7w^yY^Y`58AW+K9RiCPXo|nBfp>ooL&tI)* z(t2k6jrrLAKklv&w#xKPjVLwe&{Vx^aS4A{&9l8Zn=6GK*0ncn+x=#1OWfwJT}3|) z)h|q>wm8%!frwg*Durck>|iXsL`J3Xovs~YD&#w~u+y-~Q7mr#-OM+`9TLn5>UxRri0~Tf;{VB5+($ zs#nJm`aJcdfAr)S2Z2)9PD+g%HAqi9(L6C?+6n)KH|=SUy6c#a97OD%Ss~=FqU;XK39+a26?*UC`oa3wr#T3ea&33vb4v@J+H+Pg;prR9 zm?&cgxB2D*8b5O87a4tWFK<&Yre^7&@0$M3=z|ERWpq_4dhnBQ^}Xi@OJ{aVL5LKN zd`eZ`Un81-W1FD*=;qFti0R^ZtkjhMdn)R(xks?F<#8X=La8pll?$`J{fFtB^!@S2 zOVOGwHwPWpEc20r2()RXsE=#v>!zP00;SyA9nrLb4)te=dkfk-vl`B1*qTbEO|Pc& zAD!p-DsfeS97JsCSU%)6nV}z?Pi_A0zv}3TvrhU8>!&#gl*0B`YEi|idUNXr!B+=Q z_{c%Tn6>4@S4($~Es<3=nBfP+9OER#}uA1;Whls)ASY}qA14kFNclv>*R zNVM_KM!~_yt2+pk!dg)3>q-ZsVad+HOFKXJk%I`d9_k;>s_A2||2-Jry}yG%sUo#o zWt^Vy$_JUX7c6u9D;&FUv{P!(u-o-SH8%0~p)Edg5P{>EQrFEKrr(}C$j|-vs}2IC z+);GJ(d+b`&%W(1KEBzR2N8kem{L?~O{F&_$Fxw&ExGl>rv>~!uVu(;6psu-%kS(B zh~Mran1cwREdu8ANOJX3?RufiJ|Cym4JB80eHX^vNcsn_Q%N~iZm(Aa0X@n=V7g*G zjCMSLi1W*SLXDR7Lh%M_Qx5rep4;b^y@U|t9qZ5Mmhj0#F4!D<+sqMfiSsT#ku3-L zB$|AacI1~tW68f_?~Zkd#juX+yV&3D>LB^L`myY>iJx!Z7a<1`(*Bc5@Cqc2>zc-Q zpT>4ABixv)8-}0Fw0>#HFg;Sj8aarNamB9C z!_29m_fc8wfNmua>I z-gTgPaDQ$@s04ZXDF~6m8gahWPJffJ#c{gQYnd*zUGOYJ@g^A|b1nBl@hq+F!dI%p zK?L?LXFiW)tmS@=QixWHykkvXvQHjzh&>8>5snfxwikYXIO7K*%#a3oTSJn)D-@qoN=2Z2&p>q^o0jjnrQWd5kJ zot@DKYZ}KDrRe)cQ_g()#)*)F2)8|Gi8oKX~|#KW#ozY&1}jD19-6*E67>C{k%I`Vb){%M zM$@{CPiry`0;O=QREoUHh`dYoES)hC5m<*xk>?tb2g}~8gFq=9^Qp}rofp-ZUM$Rti!Qif*eGkKBdUljmYN>$lrAkDCLe<U zF?&M4Aiqi(VB4gWjQD7HW{@_dSqeg=SVv+1V8j!< z>jW<}>TD3g!E_lR{v69I^-bcEpw-!(3_>`FU|LQpSj6m7rGtm_`==m8igk!TXAv7# zlnQ?O)gpw0=`t<9Tg3bmYyF>xjxjofgHo(R{5gwQw_vS*^CXK94kDPAlOq=KLCfa; znzmDn4n&9)Bjk75;mx{TFI%wHXY7e-mw12s_@;ux8I8hTt7>|u?tY{o+}S9^vtP8P z|LCo{V(^JUpw#QLYI;>qK2p%}b=-$5X!1kzb>cx8j2n`JKw8hy8v@_qHEa1yd`hhj`LWsY|y`(v{l} zHVEV(;^oTqy@SaP1;Km#ZLsc&@p}0Ei&79Mg}GI#){kTK_zEwkl-xn=tm{45uV=wE zf3S`tQ%38nzy45XN2vd>VnAVU!t4?vdH-H?FIiPmI^c znk+B~Xdm8g#c_^lgU} z1WIA8(;9NOd-Sl!z9X#L9*B5lK!!K?#&PZLH+A)A-yD7N!begND21)5)YV(>(9JSe zn_OZ2U}?`UQOEltoRZma0-seo>+o>Br{WHSKn^0@GpRWZhv)~c_$CE`QvG_Rc@xV# znc3Ce(rxo-ev)3&t2$T}q>Ug>8V^MmgI}HLkLUX!u zuG8x1{LkLe!?#UNL7eU5Q*XtC?&MjLAmlJSI7~X z)1@+*6Q))NmE4z-ixj7YQbJoDRJ*>^E^>tCbmcs()j=&7NDGP-r-f2NTODL2fmjK0 zgywYR6uZ?ymKca7iWH}XQbJoD6PsrR2kKQe?SUMjIbE6!%-MRY$D@ ziqk?Vp{Uh3F?V#4ezZ(Q{gywYREVtFsV_AjZt`66wAW%wZtK+6V zJN!@g3^xem2+irriEpdpqrb2A4{o_51%XmRTOIxX)7Y;zY>Gi3M`%u0&WKwb{r~fP z!mF8+f7oDTi;pTo+S7DSHFoNnyHXIdR!j~J-uPgW@ilybd{Zx2ijdbB9% zb06gI82-oSVT9)I^Q!N*@P1m--di}oCY{CEb;rcuNQdbLfgD8OX=9~+95gI=Z_ln2 z1WJ`_(cG(CzoWP9oaLk=t$TJ*`=Rd)0y&7l^T|rJuh>0kcBS=}5P?$XT$}FwKCQcV z=$}6AJJ)&34@SM(IN*{a2N8HaS*a0iFACNMT~ZJzHE>*A@A~ck@D4Q3B^|S0sT?eP zr>8-n6rOK(5nO66J?9EJh`@8`N{!yJ!M{Hom4ZO2-*=^XZ3p)Dj#d7UOJ1v!zq8TZ z27w$zu*JyE2Ax))|3KpI@zz^H1WE~wyOvyvX@Q(i6uat`_V{w4#oky_{-y;ymIy~q z0{(Y|SRYFvLe5|E?~*?MKP^B6O3C@lR0MM5MC1RSD@4ed%TyhhE0n@Ir0lmjY5K+; zmk)C8X5e=le#t0RE$>@{cz;H=Mh1+@$?5eHtn;dT_xZmfq=h-ng z@52iJ-k*1|DS4mAI%(t}0(&s+@E)>=CEZ;FO5wdJrDo=pGCFqu(%-pJg$TEw&p%;% z)WJ)qnAd5)%eS`S@*Wr#1M7S$=@23535KphZ(LtaT_D5O=f8!ogS2-)UI`^(*3})XarjvGW z`;{iv5@+|kNaOf{)`$Iu#;aF)8y)#~R1_;Qqf9XfV?DIW`OZ*-7(8!(glVC@9)82z zc1!Zv7W&SloeUlmc{c+&h`?K7cDIBPlWE5U5h&HFitz@M_ACzX$S{I;Wsrjix2_It zY*UVX2KF7;GT58Ib4yI4FQL?!3nrGUIv^*Q)9T>_a(Epd(Y(LIC*?Um^yS#&uHc$; zCiz7XB86U*MZ8~SLNK`T93MHXLl8{MDP}s^e{QY3vb6KF{FD7D2$5n0@9)?%^w<2O zX5PHP{el{omkE%=w4BpN-8NUxUfdz*bxwMK97Hh9yES~`-y*8kNd%*3G)X~-6neE5 z@%rPng0FJB2FSs5nU+(t7IAxSTF`7?w-kg(p|4HWan&uWcs!x8#FU(OW%vX?r_0vC z{to(fN@Z+Wn#bQC{2e0w&Pc&Ghl|)(;_18!moD;uyn4LjC*rq=o2%!8YO8uaw$?v2 zX^ewFDbAtfR;gzP=H_i`rGoDtALx{$C_-vMsT(tLg7Ulb%(@SkqgPtB6x0 zT9CcGn?WE)Xx4!~yiygnmJRM3(l-TxQbJoDzifWn@04@9K_EwH)`4FZO3f<2!vAj9 z*c1dx32k*W8dJ}o^5PVOK#tI?!?pJGF?osMP3)JKNO4*yCA8H+h)@v75jrjh-(8Du zzEG|r$(2ZPTBPr^l+acOl{}J?BS&b~AveaDR*GsjlG+t1P79@kwmPTY>Y!e$rPqoSr-f2NTOBkqXc-xhBQ&Qg_f@S98hx~kJ|e|wp_I^8NA^28y3eZm zW`skI(4ZsV>WC5Kp-}t7ec+fjQbJoDajqgjm^-y1#p%YmEwDP`lBXb0N@%Miu3dvb zj?kQLTKHwfei&FRLy zAl4E0s1yWB32k-6z1AR*BQ)!XdvmNK9vM;)C?&Mj5sy9wfgGV(M?5OUI^q#71%XmR zTOIKzY7oc~IxYv_OO18JBXtUba&=ltXsaWh4J21aia8?1I^ww`))CJ*DF~Dj+UkgB zHG@Eo(5xe#6Js6m%$b5fDWR>7cy=}j2S&*Joe9Q<-vCg1@$6 zI>|KNK2)k;!9AueUL3nCfgD8q@SV9gk<5OhfYy)?y*=3={#@DJM+8dY&1t&-v2d(G zY`JfxbLR&Uc$-J5ho2j55E;i`c5XEmgGlmiN2NAgHN+q$ZT;Fo#OWF-yx~Z;drNmX1H@2=4;h_a;&Vo%lp=N2|1avQ`t+VdUvU)FwWY*#;!?I2LfwcSs4>@tYA1`i96 zQw$=>x4xCye!!kUs?cttgNV^aD()-sn0Q&uQqlFFP7N9?tyG>lxV9jB9&%@c`-)PR z)x1AC(t9Ey=9NQ^pm}vjezz+{{U+>=#FP)`Uty2%4qjW{1^5IntCf zgFsGk2&;oituM>ByM2fdnx*8viPb?htEq;4u3h95hp;-R^)$7jzpK(Kd5932rEsjI zHOo(@5~36#TCYM*aR{q}&b(^Q5ubZ)5cf5XjD`2H;(JB$O6ob;cCU6e?KB{#7(^V8 zY((BE(?2?FcM*7BX*J!iJo!KxIf#(cVs`Hi5p-6~#I0lwo;Hkk`(jH(1ac68=OGK_Iai24 zDgIr~U#236W6oa`J(s~AKx#Q+9fpGlITP~V5hx`mRZ?SN138~^by~q$GtlqpV&{}dAHA-Ej)@w6z_AEqPyoi=ngvHKj)m#-j%E+5p}Zbc@+68X>Y}Iv@DLA#kVsNfl~V`HTEcORMOr{ zrM3vDO+s!DY-ijbcH5j|cyTN*isKa_2N9G1)6%18W^wIuOf`NkhocDa-B;uw0&UvdeGNGN4&Qx61WLJe#c_8y z{tm}yKn^0XeCED$!0~q|_D+NdlzO#(bC2R;CHa26xlIZ(A==!p_33`C=KHm)eyQS7#5H4aIN~gr zaG^&L+G4w+8`3_<-{Bk5$Uy{-UFL>#z;T`UhBP8j>cGhQ9>v~?$3%|j#PLOV6h#gq zuuQZENs+}k!Wj2jv{yW-fL3X4-Ul3EjG~K0$Uy{-eCFnTz%c=M^g#qlxwXrY#W?N& z$4)>FBHU4wBa3ky2#PM|AW#Zh*2K%m`$U_u$$U&f#YgZI;(dU>lJP&gH&{~h_QB2E>^URUj9z|S?`yj{4 z;FuZQ=Ey+=&Ruj94a9Gg-39UC z_;)#5&HirTP6KiT&0b$jdv;CcFAzpb5UktrsRP{S!hJQ-mx^Fck#jO;C+5E+h-=ny zW%q>B#XTK2SC|$`$sST_u8@O!UYOFEbsz$F<<5*iDXc>mA>TNhU#wk1@EMJGWxEhF zjoafi2ls0+r9ydbd*mPjca+bJKq;z2J~I*T-4$Y{1@ZoQCYK|YGIXrlOKlJ2AR^XR z3<9;wem$p);Vi6|4h=}QS|GnfWg>~raz&eRr(CGW$a)pT4 zdWxxCObexAtBes#`)&w-*!fBHP}iYyf-Wg10V#Gua@%_kNyOeHM+jE@{zpjfe?;{D zM-a!RF&x$>h*)>h))l=@5z#9ZnqH`Ibj0ywk{n->5fpPGxvfQ#bkOS*5xr8O>4ge| z5Dp^ledU~x-Zx1O-E$f3-O=k5A-z(e>4gdhfl{o4qHQD(r41$oy-pF*D;1hvsPGeC z{w<_;SCUs<^qk2F#Vko~Iwxspp0(|H(XR%L(sRNi;hbvgWY^N{RN_8ot$~$7dh5cZ zM0=3`o~Zq2e+-+=AM7Is5wiAfcTA@H*XfT+&kFzViSohV78OH!gTs^%_aJf4llD?) z=0@~tNJKA&IJJvygY`xcFyH(pYMz)8O?;Xw1XTAGCxaWxHlmijG)5Noc-An!UczeC!=NX}QeM1L)y6GD?7cyMaek|4_OYLxevWP9T06aAqUkM@h~6>rk%I`-M^Wo;ZlLKclZf6iajYGs z++Is>m}q*-MAJJaP9H=B)*JP~5$hv*%S6*VCJq9naC|f`xJ2}piKcf<0^}fq+rMy3 zq&G}Lddo!9J0=bSrI;=pcWE9sdtN*v;*O&974~uEcRMB)hhR=Ih`7zcN+PixTgd4>F*7v>7lcs@WWo|$=u=J&ji zg9uj#drIsv@q1o~Kq+?@nfo8NJHI7_JJm80vGw7ew|TEf(|bh`zgL7DM7Y+@o)Wh^ zzvqPrl)}Ax+6ADwgXa)_2Malfz}{uv+R{yEo|;Q)M4*)0 z=Irb8=)f2zUYHh2;izQZ z^NM&bKMMKma4G}q*F8?kolPqtdA!?3>h#W*PEi<)6@X0Vf6;>1>Qv7?N zT``8DA!?3>h#X9pX%|7!5F?I;SQH^r{JYF>HdlNyj8BCj2h(NRwOu|L#;3xHB1DQ2 zg*sX^YvB7!Y|I))u!XaKB{cgbCy!s^y;Zem^248J(zo;Y`j7kX9vpA{IOHG#SJi3N z`QESnU#_#Kei4CEY3u5GH;(F&Jl5fU($VeEcmAENvyBeqAOhFnm0HoUPN0_AQ@@Bn zsRi4bd9D85DcNn|i?q%VX0-^~^arB1xl%6s&Iw#o3H zpT0)#n5?-wc;e1XgFp@<$U;t^JD{_*%W{Ib*#pU&BcBn{LMfr`y3g<>HCHi$k;uVy zQA|mxdA~lD_vL4|MsM|-6d(r?SR)kA@~7IXVr!Q>qh{4fEL_$T+46ZUi%v%$e>$)A z-nG&2k*9B&o<@k2tK$K>W%|SCr=qlQV1OLycbqQM+=J~qSl`~3n|IcOrSyig2c#fG z3hj;70IHNJNB7({oy>H;&!tBd{Z_Xl!=`!@^+)$U;Inp~GkDDk5!lm|8hNy4-n~0- z)!i@K>mX2yQ{r*nu6QkMTr;m_nHzPT{(l&Ra1en#jn4-u3AGIoBB?gk~x6oUM-U=p^~M zbpD(X$SDqCb+GgiLKQ^_nx({Nv^qFPoHIrsr#OVw!KLQX7exq~rNpbXI{Hz~at$*A zImIEY4sJbe#kvi0W+OsqmSVe-`H^lGlZCLQ)NOETHgbwXSRI@jSL?Du LB}9bKw&ed0Cio@> literal 0 HcmV?d00001 diff --git a/data/widowx/widowx.urdf b/data/widowx/widowx.urdf new file mode 100644 index 000000000..0463f9336 --- /dev/null +++ b/data/widowx/widowx.urdf @@ -0,0 +1,279 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/pybullet/examples/widows.py b/examples/pybullet/examples/widows.py new file mode 100644 index 000000000..aa1af8cb5 --- /dev/null +++ b/examples/pybullet/examples/widows.py @@ -0,0 +1,21 @@ +import pybullet as p +import time + +p.connect(p.GUI) +p.loadURDF("table/table.urdf", 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0) +p.setGravity(0,0,-10) +arm = p.loadURDF("widowx/widowx.urdf",useFixedBase=True) + +p.resetBasePositionAndOrientation(arm,[-0.098612,-0.000726,-0.194018],[0.000000,0.000000,0.000000,1.000000]) + + +while (1): + p.stepSimulation() + time.sleep(0.01) + #p.saveWorld("test.py") + viewMat = p.getDebugVisualizerCamera()[2] + #projMatrix = [0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0] + projMatrix = p.getDebugVisualizerCamera()[3] + width=640 + height=480 + img_arr = p.getCameraImage(width=width,height=height,viewMatrix=viewMat,projectionMatrix=projMatrix) From dcaaed923813fa1b354e380bb47773910e91760f Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 29 Jun 2017 22:06:27 -0700 Subject: [PATCH 10/22] also generate TinyRendererVisualShapeConverter for programmatically generated collision shapes use similar random colors for TinyRenderer (if rgba colors are not specified) --- .../Importers/ImportURDFDemo/UrdfParser.h | 3 +- .../PhysicsServerCommandProcessor.cpp | 79 +++++++++++++++---- .../TinyRendererVisualShapeConverter.cpp | 21 ++++- 3 files changed, 85 insertions(+), 18 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 817536916..33fa0d49b 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -156,7 +156,8 @@ struct UrdfLink UrdfLink() :m_parentLink(0), - m_parentJoint(0) + m_parentJoint(0), + m_linkIndex(-2) { } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 1c07479cc..c0aff3429 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -132,6 +132,7 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw struct InteralCollisionShapeData { btCollisionShape* m_collisionShape; + b3AlignedObjectArray m_urdfCollisionObjects; void clear() { m_collisionShape=0; @@ -1739,21 +1740,28 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface return -1; } - virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const + virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const { - - #if 0 - if (m_data->m_customVisualShapesConverter) + //if there is a visual, use it, otherwise convert collision shape back into UrdfCollision... + + UrdfModel model;// = m_data->m_urdfParser.getModel(); + UrdfLink link; + int colShapeUniqueId = m_createBodyArgs.m_linkCollisionShapeUniqueIds[urdfIndex]; + if (colShapeUniqueId>=0) + { + InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId); + if (handle) { - const UrdfModel& model = m_data->m_urdfParser.getModel(); - UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex); - if (linkPtr) + for (int i=0;im_urdfCollisionObjects.size();i++) { - m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, *linkPtr, &model, colObj, bodyUniqueId); + link.m_collisionArray.push_back(handle->m_urdfCollisionObjects[i]); } } } - #endif + //UrdfVisual vis; + //link.m_visualArray.push_back(vis); + //UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex); + m_data->m_visualConverter.convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, &link, &model, colObj, bodyUniqueId); } virtual void setBodyUniqueId(int bodyId) { @@ -3588,6 +3596,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); btCollisionShape* shape = 0; + b3AlignedObjectArray urdfCollisionObjects; btCompoundShape* compound = 0; @@ -3597,6 +3606,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } for (int i=0;iaddChildShape(childTransform,shape); } + urdfColObj.m_geometry.m_type = URDF_GEOM_SPHERE; + urdfColObj.m_geometry.m_sphereRadius = radius; break; } case GEOM_BOX: { //double halfExtents[3] = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_sphereRadius; - shape = worldImporter->createBoxShape(btVector3( + btVector3 halfExtents( clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[0], clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[1], - clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[2])); + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[2]); + shape = worldImporter->createBoxShape(halfExtents); if (compound) { compound->addChildShape(childTransform,shape); } + urdfColObj.m_geometry.m_type = URDF_GEOM_BOX; + urdfColObj.m_geometry.m_boxSize = 2.*halfExtents; break; } case GEOM_CAPSULE: @@ -3649,6 +3670,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { compound->addChildShape(childTransform,shape); } + urdfColObj.m_geometry.m_type = URDF_GEOM_CAPSULE; + urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleRadius; + urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleHeight; + break; } case GEOM_CYLINDER: @@ -3659,6 +3684,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { compound->addChildShape(childTransform,shape); } + urdfColObj.m_geometry.m_type = URDF_GEOM_CYLINDER; + urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleRadius; + urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleHeight; + break; } case GEOM_PLANE: @@ -3672,6 +3701,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { compound->addChildShape(childTransform,shape); } + urdfColObj.m_geometry.m_type = URDF_GEOM_PLANE; + urdfColObj.m_geometry.m_planeNormal.setValue( + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[0], + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[1], + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[2]); + break; } case GEOM_MESH: @@ -3685,7 +3720,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm const std::string& urdf_path=""; std::string fileName = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshFileName; - + urdfColObj.m_geometry.m_type = URDF_GEOM_MESH; + urdfColObj.m_geometry.m_meshFileName = fileName; + + urdfColObj.m_geometry.m_meshScale = meshScale; char relativeFileName[1024]; char pathPrefix[1024]; pathPrefix[0] = 0; @@ -3702,6 +3740,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type); if (foundFile) { + urdfColObj.m_geometry.m_meshFileType = out_type; + if (out_type==UrdfGeometry::FILE_OBJ) { //create a convex hull for each shape, and store it in a btCompoundShape @@ -3718,10 +3758,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } btAlignedObjectArray convertedVerts; convertedVerts.reserve(glmesh->m_numvertices); - btVector3 meshScale(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[0], - clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[1], - clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[2]); - + for (int i=0; im_numvertices; i++) { convertedVerts.push_back(btVector3( @@ -3809,7 +3846,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { } } + if (urdfColObj.m_geometry.m_type != URDF_GEOM_UNKNOWN) + { + urdfCollisionObjects.push_back(urdfColObj); + } } + + #if 0 shape = worldImporter->createCylinderShapeX(radius,height); shape = worldImporter->createCylinderShapeY(radius,height); @@ -3829,6 +3872,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm int collisionShapeUid = m_data->m_userCollisionShapeHandles.allocHandle(); InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeUid); handle->m_collisionShape = shape; + for (int i=0;im_urdfCollisionObjects.push_back(urdfCollisionObjects[i]); + } serverStatusOut.m_createCollisionShapeResultArgs.m_collisionShapeUniqueId = collisionShapeUid; m_data->m_worldImporters.push_back(worldImporter); serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_COMPLETED; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index f18becf58..e70e08e1f 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -506,6 +506,15 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi } +static btVector4 sColors[4] = +{ + btVector4(60./256.,186./256.,84./256.,1), + btVector4(244./256.,194./256.,13./256.,1), + btVector4(219./256.,50./256.,54./256.,1), + btVector4(72./256.,133./256.,237./256.,1), + + //btVector4(1,1,0,1), +}; void TinyRendererVisualShapeConverter::convertVisualShapes( @@ -546,7 +555,17 @@ void TinyRendererVisualShapeConverter::convertVisualShapes( } btTransform childTrans = vis->m_linkLocalFrame; - float rgbaColor[4] = {1,1,1,1}; + + + int colorIndex = colObj? colObj->getBroadphaseHandle()->getUid() & 3 : 0; + + btVector4 color; + color = sColors[colorIndex]; + float rgbaColor[4] = {color[0],color[1],color[2],color[3]}; + if (colObj->getCollisionShape()->getShapeType()==STATIC_PLANE_PROXYTYPE) + { + color.setValue(1,1,1,1); + } if (model && useVisual) { btHashString matName(linkPtr->m_visualArray[v1].m_materialName.c_str()); From dd3d55610b242482dbf3171b550285ff6c21cfea Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 30 Jun 2017 13:35:07 -0700 Subject: [PATCH 11/22] fixes in pybullet.loadTexture, changeVisualShape replacing texture. (also works for OpenGL3 renderer now) --- .../CommonGUIHelperInterface.h | 3 + .../CommonInterfaces/CommonRenderInterface.h | 3 + examples/ExampleBrowser/OpenGLGuiHelper.cpp | 12 ++ examples/ExampleBrowser/OpenGLGuiHelper.h | 3 + .../OpenGLWindow/GLInstancingRenderer.cpp | 20 +++ examples/OpenGLWindow/GLInstancingRenderer.h | 3 +- examples/SharedMemory/PhysicsClientC_API.cpp | 12 ++ examples/SharedMemory/PhysicsClientC_API.h | 3 + .../PhysicsServerCommandProcessor.cpp | 169 ++++++++++++------ .../SharedMemory/PhysicsServerExample.cpp | 43 +++++ examples/SharedMemory/SharedMemoryCommands.h | 6 + .../TinyRendererVisualShapeConverter.cpp | 43 +++-- examples/pybullet/pybullet.c | 7 +- src/LinearMath/btHashMap.h | 24 ++- 14 files changed, 275 insertions(+), 76 deletions(-) diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index 233f3002a..d552f1ebf 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -41,6 +41,9 @@ struct GUIHelperInterface virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]) {} virtual void changeSpecularColor(int instanceUid, const double specularColor[3]) {} + virtual int getShapeIndexFromInstance(int instanceUid){return -1;} + virtual void replaceTexture(int shapeIndex, int textureUid){} + virtual Common2dCanvasInterface* get2dCanvasInterface()=0; diff --git a/examples/CommonInterfaces/CommonRenderInterface.h b/examples/CommonInterfaces/CommonRenderInterface.h index 851db8d5f..236f666c9 100644 --- a/examples/CommonInterfaces/CommonRenderInterface.h +++ b/examples/CommonInterfaces/CommonRenderInterface.h @@ -70,6 +70,9 @@ struct CommonRenderInterface virtual int registerTexture(const unsigned char* texels, int width, int height, bool flipPixelsY=true)=0; virtual void updateTexture(int textureIndex, const unsigned char* texels, bool flipPixelsY=true)=0; virtual void activateTexture(int textureIndex)=0; + virtual void replaceTexture(int shapeIndex, int textureIndex){}; + + virtual int getShapeIndexFromInstance(int srcIndex) {return -1;} virtual bool readSingleInstanceTransformToCPU(float* position, float* orientation, int srcIndex)=0; diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index a02b5dbca..dfc259542 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -318,6 +318,18 @@ void OpenGLGuiHelper::removeGraphicsInstance(int graphicsUid) }; } +int OpenGLGuiHelper::getShapeIndexFromInstance(int instanceUid) +{ + return m_data->m_glApp->m_renderer->getShapeIndexFromInstance(instanceUid); +} + +void OpenGLGuiHelper::replaceTexture(int shapeIndex, int textureUid) +{ + if (shapeIndex>=0) + { + m_data->m_glApp->m_renderer->replaceTexture(shapeIndex, textureUid); + }; +} void OpenGLGuiHelper::changeRGBAColor(int instanceUid, const double rgbaColor[4]) { if (instanceUid>=0) diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h index ccc935dcb..ade554a8a 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.h +++ b/examples/ExampleBrowser/OpenGLGuiHelper.h @@ -29,6 +29,9 @@ struct OpenGLGuiHelper : public GUIHelperInterface virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]); virtual void changeSpecularColor(int instanceUid, const double specularColor[3]); + virtual int getShapeIndexFromInstance(int instanceUid); + virtual void replaceTexture(int shapeIndex, int textureUid); + virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape); virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld); diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index 3796bf371..e756dec66 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -392,6 +392,15 @@ GLInstancingRenderer::~GLInstancingRenderer() +int GLInstancingRenderer::getShapeIndexFromInstance(int srcIndex) +{ + b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex); + if (pg) + { + return pg->m_shapeIndex; + } + return -1; +} @@ -955,6 +964,17 @@ int GLInstancingRenderer::registerTexture(const unsigned char* texels, int width } +void GLInstancingRenderer::replaceTexture(int shapeIndex, int textureId) +{ + if (shapeIndex >=0 && shapeIndex < m_data->m_textureHandles.size()) + { + b3GraphicsInstance* gfxObj = m_graphicsInstances[shapeIndex]; + if (textureId>=0) + { + gfxObj->m_texturehandle = m_data->m_textureHandles[textureId].m_glTexture; + } + } +} void GLInstancingRenderer::updateTexture(int textureIndex, const unsigned char* texels, bool flipPixelsY) { diff --git a/examples/OpenGLWindow/GLInstancingRenderer.h b/examples/OpenGLWindow/GLInstancingRenderer.h index a066197b5..6d0ea2819 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.h +++ b/examples/OpenGLWindow/GLInstancingRenderer.h @@ -67,7 +67,8 @@ public: virtual int registerTexture(const unsigned char* texels, int width, int height, bool flipPixelsY=true); virtual void updateTexture(int textureIndex, const unsigned char* texels, bool flipPixelsY=true); virtual void activateTexture(int textureIndex); - + virtual void replaceTexture(int shapeIndex, int textureId); + virtual int getShapeIndexFromInstance(int srcIndex); ///position x,y,z, quaternion x,y,z,w, color r,g,b,a, scaling x,y,z virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index cb3e69de4..5a89751bd 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2938,6 +2938,18 @@ b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, return (b3SharedMemoryCommandHandle) command; } +int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle) +{ + int uid = -1; + const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + btAssert(status->m_type == CMD_LOAD_TEXTURE_COMPLETED); + if (status->m_type == CMD_LOAD_TEXTURE_COMPLETED) + { + uid = status->m_loadTextureResultArguments.m_textureUniqueId; + } + return uid; +} + b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index da7bdac42..9c7271275 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -217,6 +217,9 @@ b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientH void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo); b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename); +int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle); + + b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId); void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, double rgbaColor[4]); void b3UpdateVisualShapeSpecularColor(b3SharedMemoryCommandHandle commandHandle, double specularColor[3]); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index c0aff3429..55fac1dc9 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -20,7 +20,7 @@ #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" #include "Bullet3Common/b3HashMap.h" #include "../Utils/ChromeTraceUtil.h" - +#include "stb_image/stb_image.h" #include "BulletInverseDynamics/MultiBodyTree.hpp" #include "IKTrajectoryHelper.h" #include "btBulletDynamicsCommon.h" @@ -129,7 +129,7 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw } }; -struct InteralCollisionShapeData +struct InternalCollisionShapeData { btCollisionShape* m_collisionShape; b3AlignedObjectArray m_urdfCollisionObjects; @@ -139,7 +139,7 @@ struct InteralCollisionShapeData } }; -struct InteralBodyData +struct InternalBodyData { btMultiBody* m_multiBody; btRigidBody* m_rigidBody; @@ -152,7 +152,7 @@ struct InteralBodyData b3HashMap m_audioSources; #endif //B3_ENABLE_TINY_AUDIO - InteralBodyData() + InternalBodyData() { clear(); } @@ -183,8 +183,20 @@ struct InteralUserConstraintData } }; -typedef b3PoolBodyHandle InternalBodyHandle; -typedef b3PoolBodyHandle InternalCollisionShapeHandle; +struct InternalTextureData +{ + int m_tinyRendererTextureId; + int m_openglTextureId; + void clear() + { + m_tinyRendererTextureId = -1; + m_openglTextureId = -1; + } +}; + +typedef b3PoolBodyHandle InternalTextureHandle; +typedef b3PoolBodyHandle InternalBodyHandle; +typedef b3PoolBodyHandle InternalCollisionShapeHandle; class btCommandChunk { @@ -1145,6 +1157,7 @@ struct ContactPointsStateLogger : public InternalStateLogger struct PhysicsServerCommandProcessorInternalData { ///handle management + b3ResizablePool< InternalTextureHandle > m_textureHandles; b3ResizablePool< InternalBodyHandle > m_bodyHandles; b3ResizablePool m_userCollisionShapeHandles; @@ -1269,7 +1282,7 @@ struct PhysicsServerCommandProcessorInternalData int handle = allocHandle(); bla.push_back(handle); InternalBodyHandle* body = getHandle(handle); - InteralBodyData* body2 = body; + InternalBodyData* body2 = body; } for (int i=0;i0)) { int bodyUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[0]; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body) { if (body->m_multiBody) @@ -3256,7 +3269,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm for (int i=0;im_bodyHandles.getHandle(usedHandle); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(usedHandle); if (body && (body->m_multiBody || body->m_rigidBody)) { serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[actualNumBodies++] = usedHandle; @@ -3345,7 +3358,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { { int bodyUniqueId = sd.m_bodyUniqueIds[i]; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body) { if (body->m_multiBody) @@ -3959,7 +3972,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); } serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); } } @@ -4024,7 +4037,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); } serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); hasStatus = true; @@ -4096,7 +4109,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Printf("Processed CMD_CREATE_SENSOR"); } int bodyUniqueId = clientCmd.m_createSensorArguments.m_bodyUniqueId; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body && body->m_multiBody) { btMultiBody* mb = body->m_multiBody; @@ -4216,7 +4229,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body && body->m_multiBody) { @@ -4406,7 +4419,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_FAILED; hasStatus=true; int bodyUniqueId = clientCmd.m_requestCollisionInfoArgs.m_bodyUniqueId; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body && body->m_multiBody) @@ -4507,7 +4520,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Printf("Sending the actual state (Q,U)"); } int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body && body->m_multiBody) @@ -4824,7 +4837,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btAssert(bodyUniqueId >= 0); btAssert(linkIndex >= -1); - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body && body->m_multiBody) { @@ -5010,7 +5023,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { int bodyUniqueId = clientCmd.m_getDynamicsInfoArgs.m_bodyUniqueId; int linkIndex = clientCmd.m_getDynamicsInfoArgs.m_linkIndex; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body && body->m_multiBody) { SharedMemoryStatus& serverCmd = serverStatusOut; @@ -5151,7 +5164,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Printf("Server Init Pose not implemented yet"); } int bodyUniqueId = clientCmd.m_initPoseArgs.m_bodyUniqueId; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); btVector3 baseLinVel(0, 0, 0); btVector3 baseAngVel(0, 0, 0); @@ -5745,7 +5758,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (bodyUniqueIdA >= 0) { - InteralBodyData* bodyA = m_data->m_bodyHandles.getHandle(bodyUniqueIdA); + InternalBodyData* bodyA = m_data->m_bodyHandles.getHandle(bodyUniqueIdA); if (bodyA) { if (bodyA->m_multiBody) @@ -5779,7 +5792,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } if (bodyUniqueIdB>=0) { - InteralBodyData* bodyB = m_data->m_bodyHandles.getHandle(bodyUniqueIdB); + InternalBodyData* bodyB = m_data->m_bodyHandles.getHandle(bodyUniqueIdB); if (bodyB) { if (bodyB->m_multiBody) @@ -6048,7 +6061,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i) { - InteralBodyData* body = m_data->m_bodyHandles.getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]); bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME) != 0); if (body && body->m_multiBody) @@ -6254,12 +6267,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_CONSTRAINT) { btScalar defaultMaxForce = 500.0; - InteralBodyData* parentBody = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex); + InternalBodyData* parentBody = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex); if (parentBody && parentBody->m_multiBody) { if ((clientCmd.m_userConstraintArguments.m_parentJointIndex>=-1) && clientCmd.m_userConstraintArguments.m_parentJointIndex < parentBody->m_multiBody->getNumLinks()) { - InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; + InternalBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; //also create a constraint with just a single multibody/rigid body without child //if (childBody) { @@ -6413,7 +6426,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } else { - InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; + InternalBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; if (parentBody && childBody) { @@ -6762,12 +6775,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm BT_PROFILE("CMD_UPDATE_VISUAL_SHAPE"); SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_FAILED; - + InternalTextureHandle* texHandle = 0; + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) { + texHandle = m_data->m_textureHandles.getHandle(clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId); + if (clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId>=0) { - m_data->m_visualConverter.activateShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId, clientCmd.m_updateVisualShapeDataArguments.m_jointIndex, clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex, clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId); + if (texHandle) + { + m_data->m_visualConverter.activateShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId, clientCmd.m_updateVisualShapeDataArguments.m_jointIndex, clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex, texHandle->m_tinyRendererTextureId); + } } } @@ -6784,10 +6803,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { if (bodyHandle->m_multiBody->getBaseCollider()) { - m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); int graphicsIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex(); + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) + { + if (texHandle) + { + int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex); + m_data->m_guiHelper->replaceTexture(shapeIndex,texHandle->m_openglTextureId); + } + } if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) { + m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); } if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) @@ -6802,10 +6829,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { if (bodyHandle->m_multiBody->getLink(linkIndex).m_collider) { - m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); int graphicsIndex = bodyHandle->m_multiBody->getLink(linkIndex).m_collider->getUserIndex(); + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) + { + if (texHandle) + { + int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex); + m_data->m_guiHelper->replaceTexture(shapeIndex,texHandle->m_openglTextureId); + } + } if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) { + m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); } if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) @@ -6828,25 +6863,59 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; } - case CMD_LOAD_TEXTURE: - { + case CMD_LOAD_TEXTURE: + { BT_PROFILE("CMD_LOAD_TEXTURE"); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; - - int uid = m_data->m_visualConverter.loadTextureFile(clientCmd.m_loadTextureArguments.m_textureFileName); - - if (uid>=0) - { - serverCmd.m_type = CMD_LOAD_TEXTURE_COMPLETED; - } else - { - serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; - } - hasStatus = true; - - break; - } + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; + + char relativeFileName[1024]; + char pathPrefix[1024]; + + if(b3ResourcePath::findResourcePath(clientCmd.m_loadTextureArguments.m_textureFileName,relativeFileName,1024)) + { + b3FileUtils::extractPath(relativeFileName,pathPrefix,1024); + + int texHandle = m_data->m_textureHandles.allocHandle(); + InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle); + if(texH) + { + texH->m_tinyRendererTextureId = -1; + texH->m_openglTextureId = -1; + + int uid = m_data->m_visualConverter.loadTextureFile(relativeFileName); + if(uid>=0) + { + int m_tinyRendererTextureId; + texH->m_tinyRendererTextureId = uid; + } + + { + int width,height,n; + unsigned char* imageData= stbi_load(relativeFileName,&width,&height,&n,3); + + if(imageData) + { + texH->m_openglTextureId = m_data->m_guiHelper->registerTexture(imageData,width,height); + free(imageData); + } + else + { + b3Warning("Unsupported texture image format [%s]\n",relativeFileName); + } + } + serverCmd.m_loadTextureResultArguments.m_textureUniqueId = texHandle; + serverCmd.m_type = CMD_LOAD_TEXTURE_COMPLETED; + } + } + else + { + serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; + } + hasStatus = true; + + break; + } case CMD_LOAD_BULLET: { @@ -7041,7 +7110,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if ((clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR) || (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR)) { int bodyUniqueId = clientCmd.m_userDebugDrawArgs.m_objectUniqueId; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body) { btCollisionObject* destColObj = 0; diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 294381583..0a28b9c31 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -131,6 +131,8 @@ enum MultiThreadedGUIHelperCommunicationEnums eGUIHelperChangeGraphicsInstanceRGBAColor, eGUIHelperChangeGraphicsInstanceSpecularColor, eGUIHelperSetVisualizerFlag, + eGUIHelperChangeGraphicsInstanceTextureId, + eGUIHelperGetShapeIndexFromInstance, }; @@ -932,6 +934,29 @@ public: m_cs->setSharedParam(1,eGUIHelperRemoveGraphicsInstance); workerThreadWait(); } + + int m_getShapeIndex_instance; + int getShapeIndex_shapeIndex; + + virtual int getShapeIndexFromInstance(int instance) + { + m_getShapeIndex_instance = instance; + m_cs->lock(); + m_cs->setSharedParam(1,eGUIHelperGetShapeIndexFromInstance); + workerThreadWait(); + return getShapeIndex_shapeIndex; + } + + int m_graphicsInstanceChangeTextureId; + int m_graphicsInstanceChangeTextureShapeIndex; + virtual void replaceTexture(int shapeIndex, int textureUid) + { + m_graphicsInstanceChangeTextureShapeIndex = shapeIndex; + m_graphicsInstanceChangeTextureId = textureUid; + m_cs->lock(); + m_cs->setSharedParam(1,eGUIHelperChangeGraphicsInstanceTextureId); + workerThreadWait(); + } double m_rgbaColor[4]; int m_graphicsInstanceChangeColor; @@ -1913,6 +1938,24 @@ void PhysicsServerExample::updateGraphics() break; } + case eGUIHelperGetShapeIndexFromInstance: + { + m_multiThreadedHelper->getShapeIndex_shapeIndex = m_multiThreadedHelper->m_childGuiHelper->getShapeIndexFromInstance(m_multiThreadedHelper->m_getShapeIndex_instance); + m_multiThreadedHelper->mainThreadRelease(); + break; + } + + case eGUIHelperChangeGraphicsInstanceTextureId: + { + m_multiThreadedHelper->m_childGuiHelper->replaceTexture( + m_multiThreadedHelper->m_graphicsInstanceChangeTextureShapeIndex, + m_multiThreadedHelper->m_graphicsInstanceChangeTextureId); + m_multiThreadedHelper->mainThreadRelease(); + break; + } + + + case eGUIHelperChangeGraphicsInstanceRGBAColor: { m_multiThreadedHelper->m_childGuiHelper->changeRGBAColor(m_multiThreadedHelper->m_graphicsInstanceChangeColor,m_multiThreadedHelper->m_rgbaColor); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 103dd5f96..4b9fe12b9 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -283,6 +283,11 @@ struct LoadTextureArgs char m_textureFileName[MAX_FILENAME_LENGTH]; }; +struct b3LoadTextureResultArgs +{ + int m_textureUniqueId; +}; + struct SendVisualShapeDataArgs { int m_bodyUniqueId; @@ -1015,6 +1020,7 @@ struct SharedMemoryStatus struct b3CreateMultiBodyResultArgs m_createMultiBodyResultArgs; struct b3SendCollisionInfoArgs m_sendCollisionInfoArgs; struct SendMouseEvents m_sendMouseEvents; + struct b3LoadTextureResultArgs m_loadTextureResultArguments; }; }; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index e70e08e1f..f804dcb3f 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -1070,12 +1070,15 @@ void TinyRendererVisualShapeConverter::resetAll() void TinyRendererVisualShapeConverter::activateShapeTexture(int shapeUniqueId, int textureUniqueId) { btAssert(textureUniqueId < m_data->m_textures.size()); - TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(shapeUniqueId); - if (ptrptr && *ptrptr) - { - TinyRendererObjectArray* ptr = *ptrptr; - ptr->m_renderObjects[0]->m_model->setDiffuseTextureFromData(m_data->m_textures[textureUniqueId].textureData,m_data->m_textures[textureUniqueId].m_width,m_data->m_textures[textureUniqueId].m_height); - } + if (textureUniqueId>=0 && textureUniqueIdm_textures.size()) + { + TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(shapeUniqueId); + if (ptrptr && *ptrptr) + { + TinyRendererObjectArray* ptr = *ptrptr; + ptr->m_renderObjects[0]->m_model->setDiffuseTextureFromData(m_data->m_textures[textureUniqueId].textureData,m_data->m_textures[textureUniqueId].m_width,m_data->m_textures[textureUniqueId].m_height); + } + } } void TinyRendererVisualShapeConverter::activateShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId) @@ -1085,18 +1088,26 @@ void TinyRendererVisualShapeConverter::activateShapeTexture(int objectUniqueId, { if (m_data->m_visualShapes[i].m_objectUniqueId == objectUniqueId && m_data->m_visualShapes[i].m_linkIndex == jointIndex) { - start = i; - break; - } - } - - if (start >= 0) - { - if (start + shapeIndex < m_data->m_visualShapes.size()) - { - activateShapeTexture(start + shapeIndex, textureUniqueId); + if (shapeIndex<0) + { + activateShapeTexture(i, textureUniqueId); + } else + { + start = i; + break; + } } } + if (shapeIndex>=0) + { + if (start >= 0) + { + if (start + shapeIndex < m_data->m_visualShapes.size()) + { + activateShapeTexture(start + shapeIndex, textureUniqueId); + } + } + } } int TinyRendererVisualShapeConverter::registerTexture(unsigned char* texels, int width, int height) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 882de968c..08aee99e3 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4578,6 +4578,9 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args, PyObject* statusType = b3GetStatusType(statusHandle); if (statusType == CMD_LOAD_TEXTURE_COMPLETED) { + PyObject* item; + item = PyInt_FromLong(b3GetStatusTextureUniqueId(statusHandle)); + return item; } else { @@ -4586,8 +4589,8 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args, PyObject* } } - Py_INCREF(Py_None); - return Py_None; + PyErr_SetString(SpamError, "Error loading texture"); + return NULL; } static PyObject* MyConvertContactPoint(struct b3ContactInformation* contactPointPtr) diff --git a/src/LinearMath/btHashMap.h b/src/LinearMath/btHashMap.h index 3e8b75b7d..a0a062750 100644 --- a/src/LinearMath/btHashMap.h +++ b/src/LinearMath/btHashMap.h @@ -389,28 +389,38 @@ protected: const Value* getAtIndex(int index) const { btAssert(index < m_valueArray.size()); - - return &m_valueArray[index]; + btAssert(index>=0); + if (index>=0 && index < m_valueArray.size()) + { + return &m_valueArray[index]; + } + return 0; } Value* getAtIndex(int index) { btAssert(index < m_valueArray.size()); - - return &m_valueArray[index]; + btAssert(index>=0); + if (index>=0 && index < m_valueArray.size()) + { + return &m_valueArray[index]; + } + return 0; } Key getKeyAtIndex(int index) { btAssert(index < m_keyArray.size()); - return m_keyArray[index]; + btAssert(index>=0); + return m_keyArray[index]; } const Key getKeyAtIndex(int index) const { btAssert(index < m_keyArray.size()); - return m_keyArray[index]; - } + btAssert(index>=0); + return m_keyArray[index]; + } Value* operator[](const Key& key) { From 88897cc7442bd41ade167c65fb9d9b13c6a65080 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 30 Jun 2017 19:11:43 -0700 Subject: [PATCH 12/22] implement pybullet.changeTexture. For now, the width/height has to match the target texture unique id, otherwise crashes may happen (a check for width/height match will be added) See also examples\pybullet\examples\changeTexture.py --- data/tex256.png | Bin 0 -> 12089 bytes .../CommonGUIHelperInterface.h | 1 + examples/ExampleBrowser/OpenGLGuiHelper.cpp | 6 ++ examples/ExampleBrowser/OpenGLGuiHelper.h | 1 + .../OpenGLWindow/GLInstancingRenderer.cpp | 43 +++++++--- examples/SharedMemory/PhysicsClientC_API.cpp | 19 +++++ examples/SharedMemory/PhysicsClientC_API.h | 1 + .../PhysicsClientSharedMemory.cpp | 4 +- examples/SharedMemory/PhysicsDirect.cpp | 8 ++ .../PhysicsServerCommandProcessor.cpp | 18 +++++ .../SharedMemory/PhysicsServerExample.cpp | 28 +++++++ examples/SharedMemory/SharedMemoryCommands.h | 7 ++ examples/SharedMemory/SharedMemoryPublic.h | 2 + examples/pybullet/examples/changeTexture.py | 43 ++++++++++ examples/pybullet/pybullet.c | 74 +++++++++++++++++- 15 files changed, 241 insertions(+), 14 deletions(-) create mode 100644 data/tex256.png create mode 100644 examples/pybullet/examples/changeTexture.py diff --git a/data/tex256.png b/data/tex256.png new file mode 100644 index 0000000000000000000000000000000000000000..130d4aadec279ee9e0791ad9c9c4fb35db117148 GIT binary patch literal 12089 zcmW++c{o)4`#$H)jF~aajIFUYV_%Z3#bg;{%Ni1rEK?Fu8EY!b86;#YOL|{1MX5+C zl}Z^)DBesdq{SeSrN%z?-+X_6oa=MWbDirfpX<5q=YH<{$#8SA6Bm^i1pq+Y!QRFl z06^g>2w)JxC9K-pL%5)$9roFxhtME^!p$tqTL?o~rv0HP0ML{D-vR2})1(N4$Y9HT zmH_ZD7q@lCpbM~;k9UYvUvgvrJq3PfFS9g!*Y~}>NEHpDS%i7qqi`gq@scdczzW$XD7!6!||EKi4 z`S~`VHkS+s%-j6d=l1R6QzH|7F$;53OQa9oKUV7-OA@kHzfLxMG&HgJv+$6A=tq*- zZ`teN`X*^(8TdUf7tf_VFzY@YD@aay&!??LZKMKoqC2uzQx}*2Y$~qx2Mi%cRsx}h zf3Oxw_HYiR6mQ=Bd130U zYQ(0339mKq^1A!${JU6Pnf$J!p$`|pb6{2tY!r6Y3JBIH=R5Q)Q6Gb{7;-8gdYYT%bm z^CDCLGGTKe-prps^^dH0+P$igAMNCO4fY8~eea<`Whn!~C1$z|n-pW-5`f zz<<{>8={dl5+|;IPNIL%*$Q5d2}C!PGA*3VWi9jE#o*Q*R^Hqg3?~D#~Y%6*eyd0?t?aWaDbU#MWw?IF)D`o)u zfbO%&WO1}w;#6GtkH)}PqD0WBbS`n~o4bM)|J4z9N6csOV@tDC7lU%y3P<$RparMH ze7n=lAxC70832w~?Np?XFrS><6LhUnlA_Q5Mk>*I8@CVErUX`)O}o;x5wZu0+gLsM zxmLgfMLq|s_fl)83S55sK{Fk#!>tXDU0y=^-2;0der^5pvA23>Dt;gJy+FE~pfRs})vbufi z>(H7hfE#bW$9q-E1s2)jy+F==nS~a_uA)cPQ`@VnHV(91F5F>+7yuuY2FrIF=am87 zSNhhmx-dNpJoD$EO}aucLhAk|Rr}ecrPuTMSG*;^pqs%~Zv)G7r?y*qi^eGG(${hp z`~eJdJ^(0Pa>C(&UxE`5G#NGQ##leuCz_t?`8Bgu2l!4q}^n`x8q!yAlGwUD@3jExY z4i&MifaQ2^+~TsH4ERo9BhQuq=a#7ptwshf9b_3cz)s{em3H!*yn-p?azsJCjMmA6 zU{sO_Le}eMsEqd%vPbIAGeraKFZ4^Q=ucJj-kIN?67(rRPE$Pu14}JbJjrNJ2Zm)? zGHz;&eeY=p9ekl9N4`Y34Tx(fCy75(`doq1~EH#?7Id?Obvrm=* z{$Q=b-p`fctn(}JD=KYx7>#Kg6isUQ1r)!ooJlZQLm59@NCmF#DfN`ok*h)&Ma9b! z=~NmK$H_mp8_B^2udn&_hCyn6xY(JHGbyv2v-dfKVsSKgH{D17`ZFTeUinU1<7-)B_)T-m|G;_z zZ?2|%haZHi+j^td!K57DY7W-Zxn$rp7%5eIny}!QHqE$%_(VMH-@0x2DmkRv-lSZF`zAf%bdUla)yj$#a zwh~ie1x%(QG?v@Z{<>zQDE!}LLFe`XI4mjwJ>Z83|oVcc$S z&qr-*p;0C-{wmM|1NRTqoyic_@LvOW9$v)3`qF-Cb^Bg# zCCMr!QT2Ff{zG5$r26F?kH87T_7@9>QGYLSDzD%ClM0BV1%J%W&AySev8LQST%y=o zZ&|v3_YV^5&ziLa_1Jo!HYjS>I+TA~}*b*yOvbhjApYR}YbFPE9Y7;D( zXmR*4kIV&0ULXe3^|PPV7Xo&LjQTxBslL>D^hrgnME;*X&b3Z_8_}y{)B?wt`^sqpO z#&bp{z92D`CdvOc|M%t95i~^>LsZ$n1ni$m?w&^ZlT|p`0i~A5LX$YzNOPf4#pqnEeP!O1k^Ok=AM`f3BE;}&>mv+zq z7jPZoM}FCS7Qq^ClmrQU>LL80<(b_xI_RvYEvmg`v}0(bYYyPOHaE2`9)27PQ&;Bi z5dj;&i%kT&0H7Soz@?Yv&4726ffdn9nzapA>R}k#y7EE=&KhYQGM0z5w0O67JXB$a z=gfe^-i=LN;#Q2d|H36ZjULY_s+dk%`T`|Gg8gByMAJtd-ZPlrdL7;Vx=N~Ii-7zb zi#_&NDjm`G-zaJ`KEVUxA8dA=%zLHzEb1abt5I@~S*Q&oTb-$FS&-H7MbF@hTDtoK zDD-^!K}D4(cZcD~%|0ZX-QhkA9oPr&!xDAT{T!DI1OpZNer>_=wM!R(t(x{N&s%$| z@!rgdsUpHQkt^94x1A8S^I*1iR$1=M<;vE^Z4%yU)u7PObnYX$? zX>zl%@PWPnI9J98->omQj!1yfP!%3{ufdVUUX=jdmFvV#$3zQUOej0D zRKaAm7!yi{*kgm8CUCpuCTLlDA5$yZ;`T5D$N5-xuQ4f=khf>72-w=vBB}J|@8$h& zp(z(*fD$XlDQ-G_INy(;3);viN-$KOgCjjJ^oM8?^L@u;LG+UJu}=LMs-r z_wEzUvnt=9>(A)}eCjuJeJ0|OSn!xaJZyF3A3w(PU(*&R%w1p7F|zBr0rZ?+nO^^Z zFXE13@EKMJxlhp`pUn89sCpFrE!3AD!~HR6=s}+WH&keF4^-6tdJ1vuT)VLAl$i{x2*}p1)_d6Nu#(cxo91W_j39;?!;yK$#R!!xWTs&_vtZjiTGcvc zr(l|=<=W7K_U%$Q>Bnd+-L-gc&#*L^au&2MW22+`SN{$p1~j+ITba4$5XQt81@Mk^ zG1s481#s-}sgE92h=I9_JM7v8@MS&rxWOBd#uP%Fn8wD{XpStIzqwE^Dd)3?>Zoia zdZqh{)7t5iwU5!XE^uT8EH=q~2;{c;1){&TsnF%}(`EKbq&pMqE)04mH(J79pJN0! zv`P?aTp~{ZoD@x$s(;klE`DRsE7>n@4f5DsGqEz8Dn-XrY6N399%U~kl>zguC=&0G zv{&M48IiRumM$wXENX?^h=_-m-juqZ_`$(s+wh)=-`KwnD}${@JI{kxVloMFuDd)j zI+KI#*wPHnnTO2>OlJbs_lc-k8#MrhnB5mB326Vdv_id*%QpBAGQF@PASpohprzDHHx@Tc6-!5vmrs_#gs>s z2r4dHS_3O9zIHrzB5)o;k@HNII5}T38g5Q(`PKS%cyjbAah>0$d^El(lU$3;i+J%a z&mlm9p8UtvS*a!9K8CXNw=d({;4V-w89egjDKd|pn7kYwXCXx|DE)C&)r;7lsRT$e zN`}@XTLyQ>Je7M;-V$SmshRL(XH9lm{?dA%^ksq{C)a&o9}t^av>Mz-O7#xEsQih} zLpyW@IuXYl47a)bGs^ciilt|49+=Qvt3|SB&fkRHj2hgAp~y|)uRM1UoNu}FMJ*#$ zLocsKEug#w%_Zgk98n+BGb(VlPsO+uP;R{+mb#_3LLWtP)$SSDMVRS+X%#61WF?zg z9p05az;Z6Lb&(yrXms}=rQD=g`62f=w)_1Si3ls; zA49jHAW$-sz*+uPaw`!&n#z3BC}FGXL)l`=u};Xc*%}nRBk){9@Wma9mNUTIyAW1_ zCB+o*noEek5N&BMFJ0WGstXQ72Ex{^uBlzzKk%gCwR(Dgg+I_s@}|4evX{!5S>QG- z*R%cus*j+;K^zs?lVtum0^Gl5AG6W#u1P#k@16s{=5 zxNnF4F*z^2m4qjXajHImnb}RF1WTrB4x>ErAMsd78j#;;ZcdMb_54)cb;b`DjPh~OzgZ?li z{W#vjQT2h6$}y#r)wsv|lgH6F0n@~;+X7%ja@$CrRW4i22|h2^sM#VBgTgAJO|^ci zn!K*VK&ROv9v_JeO*SI&;Z^i6;nNk;$ zw%Mhua#_XoH@Iz7oL(C@T5-r78eG6Ud1+~Rar2mQNCfET%h1DLKI`Vn^&n{R06T1O zZT|d~(;_)R&Zbg37$tfJ2Nq!TH3R`E!ZwHdJYRFHCq!@B@p^ya;;|A`4k~0F<}8Em zcHI3p0snv|baV_tD}Y&=)Bz@OwKG?RG=cZll8GX7hoSmvouKy#ItH^n1=kkqeJvO! zBd`dGPE_tvnfv<_pUZ^GGm=d7xWkAv*5&?4j)Wd#G*i-onejxCzh*9R=iAV$sQF_u z)MgZ5%)IgDfJDd);8!O&7{i$qBQtMQpH9K?&Ox#}8YD@Wl#I@^^FPbNiM*KWh;#9D zGiCZEoYn%e*GYvhYrbr;4H{cOY*e*JM$$}cwx^WNq0X)^Vg7k>8@XxX;x>tPTVdo; zo6PQzxrKcfi|Ic{YwoH7@uB7qN1~+8HbV8ayAoM~QIvO(-!-cq&Rw%ZMriYcwb^$oJFm znZqa7+|{)$r~&+mk?@t5aW zRVqwRw8tX-tpYKe^Ph&e^fREL<#p0nMGmJ+;ayn!i6rp8f}1+N;d21shnIGFu?7VUAv}!Zx+iHGPEP7rox3+8Sd%hpTa&zzcM^dcd+rd`VzS0pa3Sv5 zzMdxRdN3HD`{`_o35|p1>eG|J8LVhfT&cXoo;GyIyS!dP+-IcL0zy6>c=;wMmc4do z!}6tQ`iYJ=o|g_Qe#NA3i(_SI(-`-Mp#?$tJNa!R(Mq4TIh(Mv8r2FD?_xM3H(Jmk zC-eU5s~2s9#@9jB3jV$Q$^YJ>yE`)Fu6v}3(>Q041|wfGVY#cFj>xY12AUfD22BKu zdphF~dE*mkY8(3JHwpMkm_j^oHc@M6i%KXi?K6h#7MeLQQ$1pfl-w)LtGakTo8X4$ zu1cXYUNpapw?JK3;d(Exvqua(Qgb*A0 zahK&ea~e=eMd@KX`6scQEG`dmy1<}ammD{YBC{Vhak6R$%M8V&P4NsXHI-Y3P_=c7 z&~c)fH^*c@mAY3I!dx3d=Ptj=#vlvVMagaw04TZ4PNWZq&a$=wZ<9TfyJj6+BOWav zuK_?xe<~$+k8&P;-18kvuC1X$F)6>$ z8~fWp0~+u22$V62VjK(LgSTr8z9p3#ffdQdC^4!m0R6-!>HoDLRqHV(L`Mb(1Jv+% z$90&eqePU^9@e!HiB{TXo>?kIV~qNg#~GlRZv8w&D4xfkm!L6*=7c2I7H%0qC~*|| z_h!HlPSZM{^4wJ

crBQE_`TK7HDaV)JP?95@srp&$W2v${xdgXkUwF&KR*agfn` z#2MgV%&!{K0BoKR5-zq|B8~dUR3QyHbvrp#nWKxjY^!6VMdMtxay%ZNO2Ab3SPAL5 z7prTD9Lb4lUOjTITiCrVucSbtD@)H>R>0#ovL5bDZ#{l}P*<2CvGmJB2y6R-%_bdo zEXSq9u6J!2VG5+9eHxYFnhno5v$|ONNAl-bs^?s;jAPef#R``5IVqCX{N;NGes>TD z?x#P%<_Y`BpfwHBsT9=mijc|AE?u`2^-!dIMwIv)p^0u@_;BG3roKN~UePZSxDnI4 zycS5@SumXtwPP8(0KYQ61}?izL2`c6j=od>c+-{W3TwWBgwu&_$1Lc zhE8)AWolVhH1JFh3wjSy)Y)LdwG9Q6;Yfty8Tsf9tpN5uh5mg&vFQkS{N#?>PwObE z1#j^V4(tC{pKixH$YLiOaTXA#UIQ<{o$U)CJY9YTBq$lPA3 ztUw8V-E=l|DStBelk~bYxOV~JBO%y3pr};i+%>tN;PTzJ(WWbDCUOsOJpc+rAMP55qU4w~X!Co!mDwqQYNO zmynHY)(Yp}MpEpv`ik9R*1H!-agP%@QyXFHNLqp~tAnvKs zzYavBc{;q-+L!&o_WwNSiI=6MR5jVz@Cd&Fi_~`|k?_^0CKZsDRmF$l+$c`?7jQ8A z(WA&Tt)^kG%6%F&7uvhMSQfXC?QeU6)3`4}tF~uJxU0VnUy?Vygirbh(F`MMMv%|P zDvCM24#P(1wNuu2CN=qbAd;wDsAd+tujlyqm)-<7cAL1tZ`)}1U2d~T zBAGrNT2K8?o5q=!sV_&c|L8N*2wW2&fQR_U1o~e~V9eVY!1Gy$uN^qvaCI~nPVsg9 z`Tc0_{d_w@X-XZPu$LU&iJ@(r4$smtexNl!NZqI6~L zI54t>_D-qhs)TXbN%b zpOTAxApUyv<>pV303KbfM=m;b=rf*up%LTYNdw9O-!lKS;O2af2v8~D?+)aRGmyP6 zB^3=JGsRTQ=)({J#PC}Ucgcp`KWxg*(myOtDE8fjzkq<1nvYGQLsD4kQDJ`-Z`Cp@ z5jREN2I$I``QJ%`=jHdi!bIh)lHT`*X#X#_^I+e7l(pWmy#9O3)RYWQ~4b$kaz@HT;XIKSLxdo&yJ4J{~6 zZK)(l=NJ3u-)t2HugKCbzU`63qkng~ZBNR$j>pWv2o- z@MN6gf$KW8wcGQ~pZhHO$^Pq+ZF(N1a_<6?h9{6dYwJ*I)zZNh9B0vusg_#02 zqmR8oe{d6YAr)5du-}T=}*KGU)~O)R}6pU!UnYOJJ1u%cv8fBwu)PO ziYXaJJlzx+HXAqiSums(T2U6FosZ4=ge8*fXM$iijdOp%pM=A@C9wpP>!_Mcj+WYP z_vG*q#GR8a^O9RCsjN-!B$fo2=woN^zph9o%Bj8#trB-l5TShyU{r`TI>DU_2p638 zWl${@<10R?1n9#5Nm-FGgUU|!C^p9<~7nf?AvvB;Am=G|1 zcK~|^>-Ey|k2Z%zSX1vVv^{n+%>0K0{o>3~nX3%#2LZ_{!wS_HMtrh2jjc`}dsJ=9 zwsBEQ#DM{ zQJxppUxR*gelt27FsD%tOwQ_sb?Hm~Ybh!NRTuBM8Xd<{?52E~+pR|#VYOHn0i%hT z5~+5sK!(|Nw2Hy!B=S*m5%6GkNgeo-4(A|pADlfrD*?JKKS6SQC3ryfZQNx$o$lN% z5kfn0ENlDSl(crCkLjMs$EX{5iz#QI%#C`DLq;I|v*YsClg%Ul>R8Yz&i9aoh`JZl zRtM@%Zz)RfloE{0xrZnKsLp&Kb7SD%Bdvql&!$njLg>!ge`C}Q$)8i!&8!}X0@f2m zb1sfmsRB83wxI}BqpcIO;Ok;g_ZTh}y{La9=0e05{!JBjCMWB6Qz|W=2WwfvC=U7X zm0p=ddexCvNtMUm(*Z2WtxWj?h)Xz7d?X0%9IJSY+ITMxPJPW-==KmoG(ze#!7UyJ zl#5bNq>7M%L1N90`h-J=TSvJs>F*G@B63uPPy6VX8Y8okTuX6 z;_sC$p!^ZRIzEcrl8#+039U*gNA11-0j=gxM4#!zV5)6ZUSz~6ZY6_ zCQI?o_(+v`6K31?gYBw zd#v$OMeaN)?{&*j@|xGa4;Ee)JW2a^^CbSqGyMg9udLuzm?$*xZYOdW!-0_Vb&S{w zj^1;PFi#3O)M&nOZXIsffL1j+WU9I9kXQti-L>gHd1^B@#CaOlU&YxZ?S-q>Q5$)9 zp+3D~kxLN#j<~VfP_z0YEK24Hdc(wpe}4s3#PK$ukRR8JdNtzVc0RRFHdj{3@4*|9 zO_m6*y*lZvT*d3o%f~DnBVryU>-B@vtzUy8D%NPtt|B<6{V+tbZklZaNL z*hi~nuRx@L|F|EW2K?L>wu*E6LD?Df=7{g+TUFVxix|{@b&Z^XYt7Uj!UhQ2Ge%eu zI=#~ECx(Y9b;$Jpk1mvL%YY^WG4*FM-T4^UE2M5l3V4S_}3yI^VR8}(q)a=e)vc2zdQJA#)rMm zWXG_OXJrVJ=;95;UgSHfFH7o`KkB64_fZ(Q4MR4zTW-<+-F;Oaiarrlio-A$>m(_> zmcDx}MZy^ctEPq8(8UK25n4vK3oB22J9)8lz4MhGg&SLncikts3(1a{sP!We8IZ0~ zjaKs0MnGfK+BKw_p?^d9@FDpWO9!cb+no&+_?Fip2Y(dAmE4hgt_-{CKiAaEV6o%wl5P0fkx9!W@Ln;8c$toReIMhd>o%mMm01LUX9JYA)Ns4NE5I#{ ze8>Ng91mX+$NeDad}A-3w?IC9Ll1GQ5l|z#`vQj_M{&0u`FosE{HXr{_4T8?10buR z$i#|J%t>Kd?Mnh}(Nh!Q`4NB2Ya+m(%r=0*d?Zbzm{LYYQ$GDX)hDU^&2b$V8(Kxa z4v36OsEq*KSFb%`Pt$VoLyUJ1rnW(g(8KfK_Lq&P_JQE>`D~vsF4Da(fO67@ws&OY zITLvLKF+)luv#j`66aKD}SpX*mb zp3JwAIhGNMNd|O^ETksEx<9Q?&ni8He56b_P|0&Z@|TE|g#tnC4ih?e|MQfNJUn|W zIbyce+*lNJ%N~9lgy#lvtBL|*E^{`Z|7|37LCuk2SOpw;QvuUWd{K80t-`HMtV@ql zh`x4>Kc}7aT7~0mOHfHld~XhR;Jvdj(pP2kfYdkT7jjSv3q}`h}g;t(X;->)kBzX-l z4z>~}{yuOX@@>l%s%qNW?YpgBD*WswjLzNq;EqjI)155)2ZC~pMBLz$cU9KJU&=XB z*6PK)P{?T062QEIG4x5={oDk#pUzBrDfe+k2xmR}8WVr(yA4e7{NcvU(_32Bx4YEF zvHtx|A$=(Qu&e&vuaIBOAHy|WVoPtdfwSx1DQ{YJT{!1bp|iliugO0=wh6?Vz{G_f zu&xJA1Wf9$B4SMe6YSJej#1Qi;GJ~+nPn({yN2M?QsW(xn<6MRgt!x(7l)}&q*Exs zMvo%cL1GR`3YllsIm_)XyqCLdlgz218=9st6R;D&lM9Hv&q7_dEbP_#-GXcHcg0R^ zNh)syD#c=`EP=l-BRWfC<8D1&b1odcx8k3ZDN<~c6bt>T)vEvc0tUzRiHD&$?du;# zkUuhTYag~KJm?g_VG&?)fbT(L?5KyqEhVD zN%UIZiny-k*yE9#N&HuXljEmG~5(Izzc{|d$PgcP9l|Lc14Eslv*O&7bN&H8fOHm04Aepg> zWtOqZH=tABf#3T4nD6lGM4(SjPXOE}^C}VnFInjlAQHCmT6jCBpsY;;qSAjm<5^X) zoL5JdS1;P~gazz%4VfZz=NL&J1iSRKPqj2DN=R9@vkp6kRquueCJHV!hd~*!0fUrM5jc2XnlSLLpMCq zgtl=4iUlf2dmA8KI!)-{6zr^bi#VO7?j-=SQr`;xQ8$RRpa6;O%4qxKH4rRKCQviQ z_Bq2ZV4?aU2(4*ByLlU=s5#4k{IdtD?%NcT)PPSSYzUH}QFZ_-7cTgOpHa#4MoJc? zZDb-u%{cR0E6?f?SJWXx(7zefjtCKJt~gZ4uH+EY)no{@!jTpFUWkfGPb&fxu6}fX zJut@dz9T|rw9N<+HRN!3ct1mV0m<#Yy6dhooO2*(`hE|XHg^s5FG3vaHx>ZS_XC)3 z1_!09fmk^@Obyn} zSeBCxFUL>lyG@9i9(;%9v?9suvnd&SV5sF4Ce9zVXje9a?e<_K-8ZyS?xa?y`1rj_ z2ePGT6yba4mb*=oUXQ~iMrSH5dO`m3rD24lFOcPR5|bO$c|e$6T?$#*lJnV;6>7`K zpujj8p68d+2=$`fCcXu1v{+VJ z`zyVaR)NG6#$SY`B8Ss{6Plcg;&k2Z#d{yxD{g=9FiJk2{!EnHS&B#n3|{3RG#-m8 zrnQGbp9Da0UirkHlWWHUG5g!>Z$_>NP*>gQybe4^aJ~txTLfg~6SH!u}O-{h?vdN-Sl z1U5=cm{1$biB2ULq;dCO5)Z?^8w=ibL3Pp|bkuQRe`(DswFvx49!4OQO70XD6GXT4 z-iN1zpUoNa2UOOdC<(`8N=)METjG5$2sJD3X@AD;?gxAjx!O#^oz5VlQz%%wt?X-& zXM`mL}Hs*`6aR1-)ul4;uJ2&ZT)pG;g~P%yW<#p1_O#cp2K`N_uy>G z$1`#2jhuh=9yVSzws%Nbp?p1;>Ha#d>cYfuc@e<)>(!Ylxp1Z>HcBpG*tex}ypX`uTQeJ+$DqQ+t9~EdzxyK$?h+rjEH`0gTp7GkcUf#gX z9p=lOXHy#eyp}q-{j!t~BZ%9EXqyv{J5R-WiRQm1k?M7zZeuL}_Te>C^_`ACDCaNi zFQ1D nH8Ij8=0LFbEXw*JZUcS63fzMJ`%^gZ4ge0eE;cn*jO70VQiDBj literal 0 HcmV?d00001 diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index d552f1ebf..cbd52aa6f 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -40,6 +40,7 @@ struct GUIHelperInterface virtual void removeGraphicsInstance(int graphicsUid) {} virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]) {} virtual void changeSpecularColor(int instanceUid, const double specularColor[3]) {} + virtual void changeTexture(int textureUniqueId, const unsigned char* rgbTexels, int width, int height){} virtual int getShapeIndexFromInstance(int instanceUid){return -1;} virtual void replaceTexture(int shapeIndex, int textureUid){} diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index dfc259542..49b717b5a 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -292,6 +292,12 @@ int OpenGLGuiHelper::registerTexture(const unsigned char* texels, int width, int return textureId; } +void OpenGLGuiHelper::changeTexture(int textureUniqueId, const unsigned char* texels, int width, int height) +{ + bool flipPixelsY = true; + m_data->m_glApp->m_renderer->updateTexture(textureUniqueId, texels,flipPixelsY); +} + int OpenGLGuiHelper::registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId) { diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h index ade554a8a..52a522e0e 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.h +++ b/examples/ExampleBrowser/OpenGLGuiHelper.h @@ -28,6 +28,7 @@ struct OpenGLGuiHelper : public GUIHelperInterface virtual void removeGraphicsInstance(int graphicsUid); virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]); virtual void changeSpecularColor(int instanceUid, const double specularColor[3]); + virtual void changeTexture(int textureUniqueId, const unsigned char* rgbTexels, int width, int height); virtual int getShapeIndexFromInstance(int instanceUid); virtual void replaceTexture(int shapeIndex, int textureUid); diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index e756dec66..92a11f417 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -117,13 +117,14 @@ GLint lineWidthRange[2]={1,1}; enum { - eGfxTransparency=1 + eGfxTransparency=1, }; + struct b3GraphicsInstance { GLuint m_cube_vao; GLuint m_index_vbo; - GLuint m_texturehandle; + GLuint m_textureIndex; int m_numIndices; int m_numVertices; @@ -141,7 +142,7 @@ struct b3GraphicsInstance b3GraphicsInstance() :m_cube_vao(-1), m_index_vbo(-1), - m_texturehandle(0), + m_textureIndex(-1), m_numIndices(-1), m_numVertices(-1), m_numGraphicsInstances(0), @@ -188,6 +189,7 @@ struct InternalTextureHandle GLuint m_glTexture; int m_width; int m_height; + int m_enableFiltering; }; struct b3PublicGraphicsInstanceData @@ -954,7 +956,7 @@ int GLInstancingRenderer::registerTexture(const unsigned char* texels, int width h.m_glTexture = textureHandle; h.m_width = width; h.m_height = height; - + h.m_enableFiltering = true; m_data->m_textureHandles.push_back(h); if (texels) { @@ -971,7 +973,8 @@ void GLInstancingRenderer::replaceTexture(int shapeIndex, int textureId) b3GraphicsInstance* gfxObj = m_graphicsInstances[shapeIndex]; if (textureId>=0) { - gfxObj->m_texturehandle = m_data->m_textureHandles[textureId].m_glTexture; + gfxObj->m_textureIndex = textureId; + } } } @@ -980,9 +983,6 @@ void GLInstancingRenderer::updateTexture(int textureIndex, const unsigned cha { if (textureIndex>=0) { - - - glActiveTexture(GL_TEXTURE0); b3Assert(glGetError() ==GL_NO_ERROR); InternalTextureHandle& h = m_data->m_textureHandles[textureIndex]; @@ -1011,7 +1011,10 @@ void GLInstancingRenderer::updateTexture(int textureIndex, const unsigned cha glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, h.m_width,h.m_height,0,GL_RGB,GL_UNSIGNED_BYTE,&texels[0]); } b3Assert(glGetError() ==GL_NO_ERROR); - glGenerateMipmap(GL_TEXTURE_2D); + if (h.m_enableFiltering) + { + glGenerateMipmap(GL_TEXTURE_2D); + } b3Assert(glGetError() ==GL_NO_ERROR); } } @@ -1048,7 +1051,7 @@ int GLInstancingRenderer::registerShape(const float* vertices, int numvertices, if (textureId>=0) { - gfxObj->m_texturehandle = m_data->m_textureHandles[textureId].m_glTexture; + gfxObj->m_textureIndex = textureId; } gfxObj->m_primitiveType = primitiveType; @@ -2201,10 +2204,26 @@ b3Assert(glGetError() ==GL_NO_ERROR); { glActiveTexture(GL_TEXTURE0); GLuint curBindTexture = 0; - if (gfxObj->m_texturehandle) - curBindTexture = gfxObj->m_texturehandle; + if (gfxObj->m_textureIndex>=0) + { + curBindTexture = m_data->m_textureHandles[gfxObj->m_textureIndex].m_glTexture; + + glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_BASE_LEVEL,0); + + if (m_data->m_textureHandles[gfxObj->m_textureIndex].m_enableFiltering) + { + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + } else + { + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + } + } else + { curBindTexture = m_data->m_defaultTexturehandle; + } //disable lazy evaluation, it just leads to bugs //if (lastBindTexture != curBindTexture) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 5a89751bd..22ce287a0 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2918,6 +2918,25 @@ void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3Visu } } +b3SharedMemoryCommandHandle b3CreateChangeTextureCommandInit(b3PhysicsClientHandle physClient, int textureUniqueId, int width, int height, const char* rgbPixels) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_CHANGE_TEXTURE; + + command->m_changeTextureArgs.m_textureUniqueId = textureUniqueId; + command->m_changeTextureArgs.m_width = width; + command->m_changeTextureArgs.m_height = height; + int numPixels = width*height; + cl->uploadBulletFileToSharedMemory(rgbPixels,numPixels*3); + command->m_updateFlags = 0; + return (b3SharedMemoryCommandHandle) command; +} + + b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 9c7271275..a0ce40f7b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -219,6 +219,7 @@ void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3Visu b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename); int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle); +b3SharedMemoryCommandHandle b3CreateChangeTextureCommandInit(b3PhysicsClientHandle physClient, int textureUniqueId, int width, int height, const char* rgbPixels); b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId); void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, double rgbaColor[4]); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 0877b1d0a..12585e13b 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -1368,7 +1368,9 @@ void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); } else { for (int i = 0; i < len; i++) { - m_data->m_testBlock1->m_bulletStreamDataClientToServer[i] = data[i]; + //m_data->m_testBlock1->m_bulletStreamDataClientToServer[i] = data[i]; + m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[i] = data[i]; + } } } diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index afbf92f0a..d26ff70da 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -1095,6 +1095,14 @@ void PhysicsDirect::setSharedMemoryKey(int key) void PhysicsDirect::uploadBulletFileToSharedMemory(const char* data, int len) { + if (len>SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) + { + len = SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE; + } + for (int i=0;im_bulletStreamDataServerToClient[i] = data[i]; + } //m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len); } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 55fac1dc9..2719f2788 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6863,6 +6863,24 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; } + + case CMD_CHANGE_TEXTURE: + { + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_CHANGE_TEXTURE_COMMAND_FAILED; + + InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(clientCmd.m_changeTextureArgs.m_textureUniqueId); + if(texH) + { + int gltex = texH->m_openglTextureId; + m_data->m_guiHelper->changeTexture(gltex, + (const unsigned char*)bufferServerToClient, clientCmd.m_changeTextureArgs.m_width,clientCmd.m_changeTextureArgs.m_height); + + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + } + hasStatus = true; + break; + } case CMD_LOAD_TEXTURE: { BT_PROFILE("CMD_LOAD_TEXTURE"); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 0a28b9c31..80968c880 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -133,6 +133,7 @@ enum MultiThreadedGUIHelperCommunicationEnums eGUIHelperSetVisualizerFlag, eGUIHelperChangeGraphicsInstanceTextureId, eGUIHelperGetShapeIndexFromInstance, + eGUIHelperChangeTexture, }; @@ -958,6 +959,23 @@ public: workerThreadWait(); } + + int m_changeTextureUniqueId; + const unsigned char* m_changeTextureRgbTexels; + int m_changeTextureWidth; + int m_changeTextureHeight; + + virtual void changeTexture(int textureUniqueId, const unsigned char* rgbTexels, int width, int height) + { + m_changeTextureUniqueId = textureUniqueId; + m_changeTextureRgbTexels = rgbTexels; + m_changeTextureWidth = width; + m_changeTextureHeight = height; + m_cs->lock(); + m_cs->setSharedParam(1,eGUIHelperChangeTexture); + workerThreadWait(); + } + double m_rgbaColor[4]; int m_graphicsInstanceChangeColor; virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]) @@ -1955,6 +1973,16 @@ void PhysicsServerExample::updateGraphics() } + case eGUIHelperChangeTexture: + { + m_multiThreadedHelper->m_childGuiHelper->changeTexture( + m_multiThreadedHelper->m_changeTextureUniqueId, + m_multiThreadedHelper->m_changeTextureRgbTexels, + m_multiThreadedHelper->m_changeTextureWidth, + m_multiThreadedHelper->m_changeTextureHeight); + m_multiThreadedHelper->mainThreadRelease(); + break; + } case eGUIHelperChangeGraphicsInstanceRGBAColor: { diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 4b9fe12b9..a5db4cd71 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -898,6 +898,12 @@ struct b3CreateMultiBodyResultArgs int m_bodyUniqueId; }; +struct b3ChangeTextureArgs +{ + int m_textureUniqueId; + int m_width; + int m_height; +}; struct SharedMemoryCommand { @@ -950,6 +956,7 @@ struct SharedMemoryCommand struct b3CreateVisualShapeArgs m_createVisualShapeArgs; struct b3CreateMultiBodyArgs m_createMultiBodyArgs; struct b3RequestCollisionInfoArgs m_requestCollisionInfoArgs; + struct b3ChangeTextureArgs m_changeTextureArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index eaf3de675..05875d9b5 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -67,6 +67,7 @@ enum EnumSharedMemoryClientCommand CMD_CREATE_MULTI_BODY, CMD_REQUEST_COLLISION_INFO, CMD_REQUEST_MOUSE_EVENTS_DATA, + CMD_CHANGE_TEXTURE, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -161,6 +162,7 @@ enum EnumSharedMemoryServerStatus CMD_REQUEST_COLLISION_INFO_COMPLETED, CMD_REQUEST_COLLISION_INFO_FAILED, CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED, + CMD_CHANGE_TEXTURE_COMMAND_FAILED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; diff --git a/examples/pybullet/examples/changeTexture.py b/examples/pybullet/examples/changeTexture.py new file mode 100644 index 000000000..c9e592f88 --- /dev/null +++ b/examples/pybullet/examples/changeTexture.py @@ -0,0 +1,43 @@ +import pybullet as p +import time +p.connect(p.GUI) +planeUidA = p.loadURDF("plane_transparent.urdf",[0,0,0]) +planeUid = p.loadURDF("plane_transparent.urdf",[0,0,-1]) + +texUid = p.loadTexture("tex256.png") + +p.changeVisualShape(planeUidA,-1,rgbaColor=[1,1,1,0.5]) +p.changeVisualShape(planeUid,-1,rgbaColor=[1,1,1,0.5]) +p.changeVisualShape(planeUid,-1, textureUniqueId = texUid) + +width = 256 +height = 256 +pixels = [255]*width*height*3 +colorR = 0 +colorG = 0 +colorB = 0 + + +#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) +#p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) + +blue=0 +logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "renderbench.json") +for i in range (100000): + p.stepSimulation() + for i in range (width): + for j in range(height): + pixels[(i+j*width)*3+0]=i + pixels[(i+j*width)*3+1]=(j+blue)%256 + pixels[(i+j*width)*3+2]=blue + blue=blue+1 + p.changeTexture(texUid, pixels,width,height) + start = time.time() + p.getCameraImage(300,300,renderer=p.ER_BULLET_HARDWARE_OPENGL) + end = time.time() + print("rendering duraction") + print(end-start) +p.stopStateLogging(logId) +#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) +#p.configureDebugVisualizer(p.COV_ENABLE_GUI,1) + diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 08aee99e3..4cf3eb6ac 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1433,7 +1433,7 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar int i; for (i = 0; i < numControlledDofs; i++) { - int jointIndex = pybullet_internalGetFloatFromSequence(jointIndicesSeq, i); + int jointIndex = pybullet_internalGetIntFromSequence(jointIndicesSeq, i); if ((jointIndex >= numJoints) || (jointIndex < 0)) { Py_DECREF(jointIndicesSeq); @@ -4551,6 +4551,75 @@ static PyObject* pybullet_changeVisualShape(PyObject* self, PyObject* args, PyOb return Py_None; } + +static PyObject* pybullet_changeTexture(PyObject* self, PyObject* args, PyObject* keywds) +{ + b3SharedMemoryCommandHandle commandHandle = 0; + b3SharedMemoryStatusHandle statusHandle=0; + int statusType = -1; + int textureUniqueId = -1; + int physicsClientId = 0; + int width=-1; + int height=-1; + + PyObject* pixelsObj = 0; + + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"textureUniqueId", "pixels", "width", "height", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOii|i", kwlist, &textureUniqueId, &pixelsObj, &width, &height, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (textureUniqueId>=0 && width>=0 && height>=0 && pixelsObj) + { + PyObject* seqPixels = PySequence_Fast(pixelsObj, "expected a sequence"); + PyObject* item; + int i; + int numPixels = width*height; + unsigned char* pixelBuffer = (char*) malloc (numPixels*3); + if (PyList_Check(seqPixels)) + { + for (i=0;i Date: Sat, 1 Jul 2017 09:15:00 -0700 Subject: [PATCH 13/22] texture check fix --- examples/OpenGLWindow/GLInstancingRenderer.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index 92a11f417..7b0c617ed 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -118,14 +118,14 @@ GLint lineWidthRange[2]={1,1}; enum { eGfxTransparency=1, + eGfxHasTexture = 2, }; struct b3GraphicsInstance { - GLuint m_cube_vao; - GLuint m_index_vbo; - GLuint m_textureIndex; - + GLuint m_cube_vao; + GLuint m_index_vbo; + GLuint m_textureIndex; int m_numIndices; int m_numVertices; @@ -974,6 +974,7 @@ void GLInstancingRenderer::replaceTexture(int shapeIndex, int textureId) if (textureId>=0) { gfxObj->m_textureIndex = textureId; + gfxObj->m_flags |= eGfxHasTexture; } } @@ -1052,6 +1053,7 @@ int GLInstancingRenderer::registerShape(const float* vertices, int numvertices, if (textureId>=0) { gfxObj->m_textureIndex = textureId; + gfxObj->m_flags |= eGfxHasTexture; } gfxObj->m_primitiveType = primitiveType; @@ -1438,8 +1440,8 @@ void GLInstancingRenderer::updateCamera(int upAxis) -//#define STB_IMAGE_WRITE_IMPLEMENTATION -#include "stb_image/stb_image_write.h" +////#define STB_IMAGE_WRITE_IMPLEMENTATION +#include "third_party/stblib/stb_image_write.h" void writeTextureToPng(int textureWidth, int textureHeight, const char* fileName, int numComponents) { @@ -2204,7 +2206,7 @@ b3Assert(glGetError() ==GL_NO_ERROR); { glActiveTexture(GL_TEXTURE0); GLuint curBindTexture = 0; - if (gfxObj->m_textureIndex>=0) + if (gfxObj->m_flags & eGfxHasTexture) { curBindTexture = m_data->m_textureHandles[gfxObj->m_textureIndex].m_glTexture; From 9cf747b35b6140c1000a4a55c9f79fc9bce93e42 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 1 Jul 2017 10:01:55 -0700 Subject: [PATCH 14/22] a few compile/warning fixes --- data/multibody.bullet | Bin 14844 -> 14844 bytes examples/ExampleBrowser/OpenGLGuiHelper.cpp | 4 ++-- .../OpenGLWindow/GLInstancingRenderer.cpp | 4 ++-- examples/pybullet/pybullet.c | 5 ----- 4 files changed, 4 insertions(+), 9 deletions(-) diff --git a/data/multibody.bullet b/data/multibody.bullet index 0dee8f14afc067fbc54030a22bd9941df44f6edd..b071e4eb732e8af9e2708912d4ed2875df2e1bb5 100644 GIT binary patch delta 487 zcmexU{HJ)rmdPTVHWNS8OfFz$$z=e80}c=xL~6m94<@KG+Dxx!U~p)^&%_86<$}pT zX=XUT!JdHugdt!_!QOo!7T5K~wg|exo^kR-CUK3|LI;X}vNLShBm$-%_%T3CXJCN1 zBJW$hz0b=E>&Y@qYJ3a~kF6W*r!I-N7i90TnmmEUWby_^mWd@olMPs;SV0b8o;;C7 ze)0__<@yH<>JaxvR6$L26EvS*@66x^G|^?jV(axH?R!A(`S)$b?$5vL?PujFg88wl z+iWDxD%rgFTo3lcKg$ODD+l83b#|9qfz%VH-wd|_b;8}df9z(o{aPXdw(w|xk)5Y= zw2k-b&3h+rU{*2Mu!jL0(jePF0GZDN1_tiQ6Im=LeyTD7vNr=U7~I$#YybcMe>*65OG;YWJ}8?3CJt1y ze{uqohy_S7OcF#dDcB2F4B{{3JO|?2zX&?IS8?}ryR@`4`&lz*+D*J5CdI%|;eH4x z+hEVY2!u#rvJ8`&`X;!*hJOncLGlK_7b=1T7kr2CD`uTxV4VDrQJD7uR7JvGpv+`N z7Ht7$pgIs{0b-!5|4-h)A~yLJlcFd?*-r*rpdf?N^Rj&l@~7^ykau;MY``kw020jR zj*A7+AaEdd0oYmy50#$+6a*Oy0x+jfW?|7_1qt|04&+dtyoFi82dvc+Y5SC}Ii`H8Y2Z!T1mwD#q|Yec65{<9+)4`$X44}qTD JyplcG2mkm_glApp->m_renderer->updateTexture(textureUniqueId, texels,flipPixelsY); + m_data->m_glApp->m_renderer->updateTexture(textureUniqueId, rgbTexels,flipPixelsY); } diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index 7b0c617ed..c00f41be4 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -1440,8 +1440,8 @@ void GLInstancingRenderer::updateCamera(int upAxis) -////#define STB_IMAGE_WRITE_IMPLEMENTATION -#include "third_party/stblib/stb_image_write.h" +//#define STB_IMAGE_WRITE_IMPLEMENTATION +#include "stb_image/stb_image_write.h" void writeTextureToPng(int textureWidth, int textureHeight, const char* fileName, int numComponents) { diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 4cf3eb6ac..637899e6c 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4651,11 +4651,6 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args, PyObject* item = PyInt_FromLong(b3GetStatusTextureUniqueId(statusHandle)); return item; } - else - { - PyErr_SetString(SpamError, "Error loading texture"); - return NULL; - } } PyErr_SetString(SpamError, "Error loading texture"); From 5178ad4abcc420f4fda0f7e097a89e3f80d31a45 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 1 Jul 2017 10:28:28 -0700 Subject: [PATCH 15/22] add proper pointer casts --- examples/pybullet/pybullet.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 637899e6c..aba9c69c2 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4583,7 +4583,7 @@ static PyObject* pybullet_changeTexture(PyObject* self, PyObject* args, PyObject PyObject* item; int i; int numPixels = width*height; - unsigned char* pixelBuffer = (char*) malloc (numPixels*3); + unsigned char* pixelBuffer = (unsigned char*) malloc (numPixels*3); if (PyList_Check(seqPixels)) { for (i=0;i Date: Sat, 1 Jul 2017 10:43:02 -0700 Subject: [PATCH 16/22] bump up pybullet version --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index d146cecc6..1a04f4de5 100644 --- a/setup.py +++ b/setup.py @@ -419,7 +419,7 @@ else: setup( name = 'pybullet', - version='1.1.8', + version='1.2.0', description='Official Python Interface for the Bullet Physics SDK Robotics Simulator', long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine 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', From e6f7eb486a1da9a5f589f56b3aec5126ea6b8a90 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 1 Jul 2017 10:55:13 -0700 Subject: [PATCH 17/22] bump up pybullet setup.py version --- setup.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 1a04f4de5..d487f86e9 100644 --- a/setup.py +++ b/setup.py @@ -386,6 +386,7 @@ if _platform == "linux" or _platform == "linux2": sources = sources + ["examples/ThirdPartyLibs/enet/unix.c"]\ +["examples/OpenGLWindow/X11OpenGLWindow.cpp"]\ +["examples/ThirdPartyLibs/Glew/glew.c"] + include_dirs += ["examples/ThirdPartyLibs/optionalX11"] elif _platform == "win32": print("win32!") libraries = ['Ws2_32','Winmm','User32','Opengl32','kernel32','glu32','Gdi32','Comdlg32'] @@ -419,7 +420,7 @@ else: setup( name = 'pybullet', - version='1.2.0', + version='1.2.1', description='Official Python Interface for the Bullet Physics SDK Robotics Simulator', long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine 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', From b63023c6920019fe409bc2e4638ce1da0fb9c440 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 1 Jul 2017 11:47:57 -0700 Subject: [PATCH 18/22] add transparent plane --- data/plane_transparent.mtl | 14 ++++++++++++++ data/plane_transparent.obj | 18 ++++++++++++++++++ data/plane_transparent.urdf | 29 +++++++++++++++++++++++++++++ 3 files changed, 61 insertions(+) create mode 100644 data/plane_transparent.mtl create mode 100644 data/plane_transparent.obj create mode 100644 data/plane_transparent.urdf diff --git a/data/plane_transparent.mtl b/data/plane_transparent.mtl new file mode 100644 index 000000000..6b700a066 --- /dev/null +++ b/data/plane_transparent.mtl @@ -0,0 +1,14 @@ +newmtl Material + Ns 10.0000 + Ni 1.5000 + d 1.0000 + Tr 0.0000 + Tf 1.0000 1.0000 1.0000 + illum 2 + Ka 0.0000 0.0000 0.0000 + Kd 0.5880 0.5880 0.5880 + Ks 0.0000 0.0000 0.0000 + Ke 0.0000 0.0000 0.0000 + map_Kd tex4x4.png + + diff --git a/data/plane_transparent.obj b/data/plane_transparent.obj new file mode 100644 index 000000000..b1317066e --- /dev/null +++ b/data/plane_transparent.obj @@ -0,0 +1,18 @@ +# Blender v2.66 (sub 1) OBJ File: '' +# www.blender.org +mtllib plane_transparent.mtl +o Plane +v 15.000000 -15.000000 0.000000 +v 15.000000 15.000000 0.000000 +v -15.000000 15.000000 0.000000 +v -15.000000 -15.000000 0.000000 + +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 0.000000 0.000000 + +usemtl Material +s off +f 1/1 2/2 3/3 +f 1/1 3/3 4/4 diff --git a/data/plane_transparent.urdf b/data/plane_transparent.urdf new file mode 100644 index 000000000..4054d2948 --- /dev/null +++ b/data/plane_transparent.urdf @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 0df8887990b1dea569a063f15509a1fd71e6a22e Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 14 Jul 2017 23:12:16 +0100 Subject: [PATCH 19/22] improve pybullet performance of loadURDF/SDF/MJCF and 'createCollisionShape'/'createMultiBody' for GUI/VR/SHARED_MEMORY, use p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) before loading and p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) afterwards --- .../InProcessExampleBrowser.cpp | 12 ++++++---- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 11 ++++++++- .../SharedMemoryInProcessPhysicsC_API.cpp | 12 ++++++---- examples/SharedMemory/SharedMemoryPublic.h | 3 ++- .../StandaloneMain/hellovr_opengl_main.cpp | 22 ++++++++++++------ examples/Utils/ChromeTraceUtil.cpp | 23 ++++++++++++------- setup.py | 2 +- 7 files changed, 58 insertions(+), 27 deletions(-) diff --git a/examples/ExampleBrowser/InProcessExampleBrowser.cpp b/examples/ExampleBrowser/InProcessExampleBrowser.cpp index 776973724..9c0572626 100644 --- a/examples/ExampleBrowser/InProcessExampleBrowser.cpp +++ b/examples/ExampleBrowser/InProcessExampleBrowser.cpp @@ -230,6 +230,7 @@ static double gMinUpdateTimeMicroSecs = 4000.; void ExampleBrowserThreadFunc(void* userPtr,void* lsMemory) { + printf("ExampleBrowserThreadFunc started\n"); ExampleBrowserThreadLocalStorage* localStorage = (ExampleBrowserThreadLocalStorage*) lsMemory; @@ -257,7 +258,9 @@ void ExampleBrowserThreadFunc(void* userPtr,void* lsMemory) do { - B3_PROFILE("ExampleBrowserThreadFunc"); + clock.usleep(0); + + //B3_PROFILE("ExampleBrowserThreadFunc"); float deltaTimeInSeconds = clock.getTimeMicroseconds()/1000000.f; { if (deltaTimeInSeconds > 0.1) @@ -266,13 +269,13 @@ void ExampleBrowserThreadFunc(void* userPtr,void* lsMemory) } if (deltaTimeInSeconds < (gMinUpdateTimeMicroSecs/1e6)) { - B3_PROFILE("clock.usleep"); - clock.usleep(gMinUpdateTimeMicroSecs/10.); + //B3_PROFILE("clock.usleep"); exampleBrowser->updateGraphics(); } else { - B3_PROFILE("exampleBrowser->update"); + //B3_PROFILE("exampleBrowser->update"); clock.reset(); + exampleBrowser->updateGraphics(); exampleBrowser->update(deltaTimeInSeconds); } } @@ -429,6 +432,7 @@ void btUpdateInProcessExampleBrowserMainThread(btInProcessExampleBrowserMainThre { float deltaTimeInSeconds = data->m_clock.getTimeMicroseconds()/1000000.f; data->m_clock.reset(); + data->m_exampleBrowser->updateGraphics(); data->m_exampleBrowser->update(deltaTimeInSeconds); } void btShutDownExampleBrowserMainThread(btInProcessExampleBrowserMainThreadInternalData* data) diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 7459729af..3de9af446 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -127,6 +127,8 @@ extern bool useShadowMap; bool visualWireframe=false; static bool renderVisualGeometry=true; static bool renderGrid = true; +static bool gEnableRenderLoop = true; + bool renderGui = true; static bool enable_experimental_opencl = false; @@ -370,6 +372,10 @@ void OpenGLExampleBrowser::registerFileImporter(const char* extension, CommonExa void OpenGLExampleBrowserVisualizerFlagCallback(int flag, bool enable) { + if (flag == COV_ENABLE_RENDERING) + { + gEnableRenderLoop = (enable!=0); + } if (flag == COV_ENABLE_SHADOWS) { useShadowMap = enable; @@ -1185,7 +1191,7 @@ void OpenGLExampleBrowser::updateGraphics() { if (!pauseSimulation || singleStepSimulation) { - B3_PROFILE("sCurrentDemo->updateGraphics"); + //B3_PROFILE("sCurrentDemo->updateGraphics"); sCurrentDemo->updateGraphics(); } } @@ -1193,6 +1199,9 @@ void OpenGLExampleBrowser::updateGraphics() void OpenGLExampleBrowser::update(float deltaTime) { + if (!gEnableRenderLoop) + return; + b3ChromeUtilsEnableProfiling(); B3_PROFILE("OpenGLExampleBrowser::update"); diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp index 8b5200f6b..5a545795b 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp @@ -4,6 +4,7 @@ #include "PhysicsClientSharedMemory.h" #include"../ExampleBrowser/InProcessExampleBrowser.h" +#include #include "PhysicsServerExampleBullet2.h" @@ -52,12 +53,12 @@ public: } } { - unsigned long int ms = m_clock.getTimeMilliseconds(); - if (ms>20) + //unsigned long int ms = m_clock.getTimeMilliseconds(); + //if (ms>2) { - B3_PROFILE("m_clock.reset()"); + // B3_PROFILE("m_clock.reset()"); - m_clock.reset(); + // m_clock.reset(); btUpdateInProcessExampleBrowserMainThread(m_data); } } @@ -165,7 +166,8 @@ public: // return non-null if there is a status, nullptr otherwise virtual const struct SharedMemoryStatus* processServerStatus() { - //m_physicsServerExample->updateGraphics(); + printf("updating graphics!\n"); + m_physicsServerExample->updateGraphics(); unsigned long long int curTime = m_clock.getTimeMicroseconds(); unsigned long long int dtMicro = curTime - m_prevTime; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 05875d9b5..55de32c57 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -4,7 +4,8 @@ #define SHARED_MEMORY_KEY 12347 ///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures ///my convention is year/month/day/rev -#define SHARED_MEMORY_MAGIC_NUMBER 201706015 +#define SHARED_MEMORY_MAGIC_NUMBER 201707140 +//#define SHARED_MEMORY_MAGIC_NUMBER 201706015 //#define SHARED_MEMORY_MAGIC_NUMBER 201706001 //#define SHARED_MEMORY_MAGIC_NUMBER 201703024 diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index a430ea615..5936e7cf0 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -373,8 +373,8 @@ void MyKeyboardCallback(int key, int state) #include "../SharedMemory/SharedMemoryPublic.h" extern bool useShadowMap; -bool gEnableVRRenderControllers=true; - +static bool gEnableVRRenderControllers=true; +static bool gEnableVRRendering = true; void VRPhysicsServerVisualizerFlagCallback(int flag, bool enable) @@ -391,7 +391,10 @@ void VRPhysicsServerVisualizerFlagCallback(int flag, bool enable) { gEnableVRRenderControllers = enable; } - + if (flag == COV_ENABLE_RENDERING) + { + gEnableVRRendering = enable; + } if (flag == COV_ENABLE_WIREFRAME) @@ -887,13 +890,18 @@ void CMainApplication::RunMainLoop() while ( !bQuit && !m_app->m_window->requestedExit()) { b3ChromeUtilsEnableProfiling(); + if (gEnableVRRendering) { - B3_PROFILE("main"); + B3_PROFILE("main"); - bQuit = HandleInput(); + bQuit = HandleInput(); - RenderFrame(); - } + RenderFrame(); + } else + { + b3Clock::usleep(0); + sExample->updateGraphics(); + } } } diff --git a/examples/Utils/ChromeTraceUtil.cpp b/examples/Utils/ChromeTraceUtil.cpp index 02a892ba5..fefa9171c 100644 --- a/examples/Utils/ChromeTraceUtil.cpp +++ b/examples/Utils/ChromeTraceUtil.cpp @@ -250,18 +250,25 @@ void b3ChromeUtilsStopTimingsAndWriteJsonFile(const char* fileNamePrefix) static int fileCounter = 0; sprintf(fileName,"%s_%d.json",fileNamePrefix, fileCounter++); gTimingFile = fopen(fileName,"w"); - fprintf(gTimingFile,"{\"traceEvents\":[\n"); - //dump the content to file - for (int i=0;i Date: Fri, 14 Jul 2017 23:32:53 +0100 Subject: [PATCH 20/22] add example of faster URDF/SDF loading in VR/GUI mode, by temporary disabling rendering --- examples/pybullet/examples/loadingBench.py | 28 ++++++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 examples/pybullet/examples/loadingBench.py diff --git a/examples/pybullet/examples/loadingBench.py b/examples/pybullet/examples/loadingBench.py new file mode 100644 index 000000000..e576e2afe --- /dev/null +++ b/examples/pybullet/examples/loadingBench.py @@ -0,0 +1,28 @@ +import pybullet as p +import time +p.connect(p.GUI) + +p.resetSimulation() +timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "loadingBenchVR.json") +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) +print("load plane.urdf") +p.loadURDF("plane.urdf") +print("load r2d2.urdf") + +p.loadURDF("r2d2.urdf",0,0,1) +print("load kitchen/1.sdf") +p.loadSDF("kitchens/1.sdf") +print("load 100 times plate.urdf") +for i in range (100): + p.loadURDF("dinnerware/plate.urdf",0,i,1) + +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) + +p.stopStateLogging(timinglog) +print("stopped state logging") +p.getCameraImage(320,200) + +while (1): + p.stepSimulation() + + From cfc7917586c0b785182f4e72184b8cc071a3e12b Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Tue, 18 Jul 2017 13:52:29 -0700 Subject: [PATCH 21/22] Add init function and module for pybullet with EGL. --- examples/pybullet/pybullet.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index aba9c69c2..a4c9549de 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -7224,14 +7224,22 @@ PyMODINIT_FUNC #if PY_MAJOR_VERSION >= 3 PyInit_pybullet(void) #else +#ifdef BT_USE_EGL +initpybullet_egl(void) +#else initpybullet(void) +#endif //BT_USE_EGL #endif { PyObject* m; #if PY_MAJOR_VERSION >= 3 m = PyModule_Create(&moduledef); +#else +#ifdef BT_USE_EGL + m = Py_InitModule3("pybullet_egl", SpamMethods, "Python bindings for Bullet"); #else m = Py_InitModule3("pybullet", SpamMethods, "Python bindings for Bullet"); +#endif //BT_USE_EGL #endif #if PY_MAJOR_VERSION >= 3 From d28dd2f80a0153f6b6e18822be1c5484ce6e811b Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 29 Jul 2017 13:18:49 +0200 Subject: [PATCH 22/22] add 'createObstacleCourse.py' example, helps reproducing Parkour paper: https://arxiv.org/abs/1707.02286 --- data/stone.mtl | 10 ++ data/stone.obj | 32 +++++++ .../pybullet/examples/createObstacleCourse.py | 93 +++++++++++++++++++ 3 files changed, 135 insertions(+) create mode 100644 data/stone.mtl create mode 100644 data/stone.obj create mode 100644 examples/pybullet/examples/createObstacleCourse.py diff --git a/data/stone.mtl b/data/stone.mtl new file mode 100644 index 000000000..70d3ba1da --- /dev/null +++ b/data/stone.mtl @@ -0,0 +1,10 @@ +# Blender MTL File: 'None' +# Material Count: 1 + +newmtl None +Ns 0 +Ka 0.000000 0.000000 0.000000 +Kd 0.8 0.8 0.8 +Ks 0.8 0.8 0.8 +d 1 +illum 2 diff --git a/data/stone.obj b/data/stone.obj new file mode 100644 index 000000000..0fbe8c5f3 --- /dev/null +++ b/data/stone.obj @@ -0,0 +1,32 @@ +# Blender v2.78 (sub 0) OBJ File: '' +# www.blender.org +mtllib stone.mtl +o Cube +v -0.246350 -0.246483 -0.000624 +v -0.151407 -0.176325 0.172867 +v -0.246350 0.249205 -0.000624 +v -0.151407 0.129477 0.172867 +v 0.249338 -0.246483 -0.000624 +v 0.154395 -0.176325 0.172867 +v 0.249338 0.249205 -0.000624 +v 0.154395 0.129477 0.172867 +vn -0.8772 0.0000 0.4801 +vn 0.0000 0.8230 0.5680 +vn 0.8772 0.0000 0.4801 +vn 0.0000 -0.9271 0.3749 +vn 0.0000 0.0000 -1.0000 +vn 0.0000 0.0000 1.0000 +usemtl None +s off +f 1//1 4//1 3//1 +f 4//2 7//2 3//2 +f 8//3 5//3 7//3 +f 6//4 1//4 5//4 +f 7//5 1//5 3//5 +f 4//6 6//6 8//6 +f 1//1 2//1 4//1 +f 4//2 8//2 7//2 +f 8//3 6//3 5//3 +f 6//4 2//4 1//4 +f 7//5 5//5 1//5 +f 4//6 2//6 6//6 diff --git a/examples/pybullet/examples/createObstacleCourse.py b/examples/pybullet/examples/createObstacleCourse.py new file mode 100644 index 000000000..6baa636ff --- /dev/null +++ b/examples/pybullet/examples/createObstacleCourse.py @@ -0,0 +1,93 @@ +import pybullet as p +import time +import math + +p.connect(p.GUI) +#don't create a ground plane, to allow for gaps etc +p.resetSimulation() +#p.createCollisionShape(p.GEOM_PLANE) +#p.createMultiBody(0,0) +#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]); +p.resetDebugVisualizerCamera(15,-346,-16,[-15,0,1]); + +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) + +sphereRadius = 0.05 +colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius) +stoneId = p.createCollisionShape(p.GEOM_MESH,fileName="stone.obj") + +boxHalfLength = 0.5 +boxHalfWidth = 2.5 +boxHalfHeight = 0.1 +segmentLength = 5 + +colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[boxHalfLength,boxHalfWidth,boxHalfHeight]) + +mass = 1 +visualShapeId = -1 + +segmentStart = 0 + +for i in range (segmentLength): + p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1]) + segmentStart=segmentStart-1 + +for i in range (segmentLength): + height = 0 + if (i%2): + height=1 + p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1+height]) + segmentStart=segmentStart-1 + +baseOrientation = p.getQuaternionFromEuler([math.pi/2.,0,math.pi/2.]) + +for i in range (segmentLength): + p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1]) + segmentStart=segmentStart-1 + if (i%2): + p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,i%3,-0.1+height+boxHalfWidth],baseOrientation=baseOrientation) + +for i in range (segmentLength): + p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1]) + width=4 + for j in range (width): + p.createMultiBody(baseMass=0,baseCollisionShapeIndex = stoneId,basePosition = [segmentStart,0.5*(i%2)+j-width/2.,0]) + segmentStart=segmentStart-1 + + +link_Masses=[1] +linkCollisionShapeIndices=[colBoxId] +linkVisualShapeIndices=[-1] +linkPositions=[[0,0,0]] +linkOrientations=[[0,0,0,1]] +linkInertialFramePositions=[[0,0,0]] +linkInertialFrameOrientations=[[0,0,0,1]] +indices=[0] +jointTypes=[p.JOINT_REVOLUTE] +axis=[[1,0,0]] + +baseOrientation = [0,0,0,1] +for i in range (segmentLength): + boxId = p.createMultiBody(0,colSphereId,-1,[segmentStart,0,-0.1],baseOrientation,linkMasses=link_Masses,linkCollisionShapeIndices=linkCollisionShapeIndices,linkVisualShapeIndices=linkVisualShapeIndices,linkPositions=linkPositions,linkOrientations=linkOrientations,linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations,linkParentIndices=indices,linkJointTypes=jointTypes,linkJointAxis=axis) + p.changeDynamics(boxId,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0) + print(p.getNumJoints(boxId)) + for joint in range (p.getNumJoints(boxId)): + targetVelocity = 1 + if (i%2): + targetVelocity =-1 + p.setJointMotorControl2(boxId,joint,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=10) + segmentStart=segmentStart-1.1 + + + +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) +while (1): + camData = p.getDebugVisualizerCamera() + viewMat = camData[2] + projMat = camData[3] + p.getCameraImage(256,256,viewMatrix=viewMat, projectionMatrix=projMat, renderer=p.ER_BULLET_HARDWARE_OPENGL) + keys = p.getKeyboardEvents() + p.stepSimulation() + #print(keys) + time.sleep(0.01) +