diff --git a/MANIFEST.in b/MANIFEST.in index 35db1b38e..0d31466ce 100644 --- a/MANIFEST.in +++ b/MANIFEST.in @@ -5,7 +5,6 @@ recursive-include Extras *.h recursive-include Extras *.hpp recursive-include src *.h recursive-include src *.hpp -recursive-include data *.* recursive-include examples/pybullet/gym *.* include examples/ThirdPartyLibs/enet/unix.c include examples/OpenGLWindow/X11OpenGLWindow.cpp diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index 07cb2696d..484391388 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -187,6 +187,7 @@ struct BulletMJCFImporterInternalData //those collision shapes are deleted by caller (todo: make sure this happens!) btAlignedObjectArray m_allocatedCollisionShapes; + mutable btAlignedObjectArray m_allocatedMeshInterfaces; BulletMJCFImporterInternalData() :m_activeModel(-1), @@ -1855,6 +1856,8 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes( int linkI { btTriangleMesh* meshInterface = new btTriangleMesh(); + m_data->m_allocatedMeshInterfaces.push_back(meshInterface); + for (int i=0;im_numIndices/3;i++) { float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw; @@ -1926,6 +1929,15 @@ class btCollisionShape* BulletMJCFImporter::getAllocatedCollisionShape(int index return m_data->m_allocatedCollisionShapes[index]; } +int BulletMJCFImporter::getNumAllocatedMeshInterfaces() const +{ + return m_data->m_allocatedMeshInterfaces.size(); +} + +btStridingMeshInterface* BulletMJCFImporter::getAllocatedMeshInterface(int index) +{ + return m_data->m_allocatedMeshInterfaces[index]; +} int BulletMJCFImporter::getNumModels() const { diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h index 306d311ca..05e5dc2fe 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h @@ -78,6 +78,9 @@ public: virtual int getNumModels() const; virtual void activateModel(int modelIndex); + virtual int getNumAllocatedMeshInterfaces() const; + virtual btStridingMeshInterface* getAllocatedMeshInterface(int index); + }; diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 6f6d1f752..a80317119 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -54,6 +54,7 @@ ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData int m_bodyId; btHashMap m_linkColors; btAlignedObjectArray m_allocatedCollisionShapes; + mutable btAlignedObjectArray m_allocatedMeshInterfaces; LinkVisualShapesConverter* m_customVisualShapesConverter; @@ -572,7 +573,7 @@ bool findExistingMeshFile( } } -btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix) +btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix) const { BT_PROFILE("convertURDFToCollisionShape"); @@ -775,6 +776,7 @@ upAxisMat.setIdentity(); { BT_PROFILE("convert trimesh"); btTriangleMesh* meshInterface = new btTriangleMesh(); + m_data->m_allocatedMeshInterfaces.push_back(meshInterface); { BT_PROFILE("convert vertices"); @@ -1229,6 +1231,19 @@ btCollisionShape* BulletURDFImporter::getAllocatedCollisionShape(int index) return m_data->m_allocatedCollisionShapes[index]; } +int BulletURDFImporter::getNumAllocatedMeshInterfaces() const +{ + return m_data->m_allocatedMeshInterfaces.size(); +} + + +btStridingMeshInterface* BulletURDFImporter::getAllocatedMeshInterface(int index) +{ + return m_data->m_allocatedMeshInterfaces[index]; +} + + + class btCompoundShape* BulletURDFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const { diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index c8fbf6204..5e0892fdd 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -62,6 +62,8 @@ public: virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const; + class btCollisionShape* convertURDFToCollisionShape(const struct UrdfCollision* collision, const char* urdfPathPrefix) const; + ///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const; @@ -69,7 +71,9 @@ public: virtual int getNumAllocatedCollisionShapes() const; virtual class btCollisionShape* getAllocatedCollisionShape(int index); - + virtual int getNumAllocatedMeshInterfaces() const; + virtual class btStridingMeshInterface* getAllocatedMeshInterface(int index); + }; diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index d6853e3b6..e1a98d0c2 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -80,6 +80,8 @@ public: virtual class btCollisionShape* getAllocatedCollisionShape(int /*index*/ ) {return 0;} virtual int getNumModels() const {return 0;} virtual void activateModel(int /*modelIndex*/) { } + virtual int getNumAllocatedMeshInterfaces() const { return 0;} + virtual class btStridingMeshInterface* getAllocatedMeshInterface(int index) {return 0;} }; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 9975312da..06a599339 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1191,6 +1191,7 @@ struct PhysicsServerCommandProcessorInternalData btAlignedObjectArray m_strings; btAlignedObjectArray m_collisionShapes; + btAlignedObjectArray m_meshInterfaces; MyOverlapFilterCallback* m_broadphaseCollisionFilterCallback; btHashedOverlappingPairCache* m_pairCache; @@ -2027,6 +2028,11 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() btCollisionShape* shape = m_data->m_collisionShapes[j]; delete shape; } + for (int j=0;jm_meshInterfaces.size();j++) + { + delete m_data->m_meshInterfaces[j]; + } + m_data->m_meshInterfaces.clear(); m_data->m_collisionShapes.clear(); delete m_data->m_dynamicsWorld; @@ -2232,7 +2238,12 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, } } - + + for (int i=0;im_meshInterfaces.push_back(u2b.getAllocatedMeshInterface(i)); + } + for (int i=0;im_data->m_meshInterfaces.push_back(meshInterface); { BT_PROFILE("convert vertices"); @@ -3801,6 +3812,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { BT_PROFILE("create btBvhTriangleMeshShape"); btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true); + m_data->m_collisionShapes.push_back(trimesh); //trimesh->setLocalScaling(collision->m_geometry.m_meshScale); shape = trimesh; if (compound) @@ -3808,6 +3820,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm compound->addChildShape(childTransform,shape); } } + delete glmesh; } else { diff --git a/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py b/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py index 3c09f85e3..c1ef9349d 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py @@ -22,7 +22,7 @@ class BulletClient(object): """Inject the client id into Bullet functions.""" attribute = getattr(pybullet, name) if inspect.isbuiltin(attribute): - if name not in ["invertTransform", "multiplyTransforms", + if name not in ["invertTransform", "computeViewMatrix","multiplyTransforms", "getMatrixFromQuaternion"]: # A temporary hack for now. attribute = functools.partial(attribute, physicsClientId=self._client) return attribute diff --git a/examples/pybullet/gym/pybullet_envs/bullet/minitaur.py b/examples/pybullet/gym/pybullet_envs/bullet/minitaur.py index 252f3f906..7e05c0ec1 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/minitaur.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/minitaur.py @@ -4,7 +4,7 @@ import copy import math import numpy as np -import motor +from . import motor import os INIT_POSITION = [0, 0, .2] diff --git a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py index dcdcc5aa5..566c098ed 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py @@ -8,8 +8,8 @@ from gym import spaces from gym.utils import seeding import numpy as np import pybullet -import bullet_client -import minitaur +from . import bullet_client +from . import minitaur import os NUM_SUBSTEPS = 5 diff --git a/examples/pybullet/gym/pybullet_envs/bullet/racecar.py b/examples/pybullet/gym/pybullet_envs/bullet/racecar.py index 12736d94f..55b9393ff 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/racecar.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/racecar.py @@ -5,54 +5,79 @@ import math class Racecar: - def __init__(self, bullet_client, urdfRootPath='', timeStep=0.01): - self.urdfRootPath = urdfRootPath - self.timeStep = timeStep - self._p = bullet_client - self.reset() + def __init__(self, bullet_client, urdfRootPath='', timeStep=0.01): + self.urdfRootPath = urdfRootPath + self.timeStep = timeStep + self._p = bullet_client + self.reset() - def reset(self): - self.racecarUniqueId = self._p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","racecar/racecar.urdf"), [0,0,.2]) - self.maxForce = 20 - self.nMotors = 2 - self.motorizedwheels=[2] - self.inactiveWheels = [3,5,7] - for wheel in self.inactiveWheels: - self._p.setJointMotorControl2(self.racecarUniqueId,wheel,self._p.VELOCITY_CONTROL,targetVelocity=0,force=0) + def reset(self): + car = self._p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","racecar/racecar_differential.urdf"), [0,0,.2],useFixedBase=False) + self.racecarUniqueId = car + #for i in range (self._p.getNumJoints(car)): + # print (self._p.getJointInfo(car,i)) + for wheel in range(self._p.getNumJoints(car)): + self._p.setJointMotorControl2(car,wheel,self._p.VELOCITY_CONTROL,targetVelocity=0,force=0) + self._p.getJointInfo(car,wheel) - self.motorizedWheels = [2] - self.steeringLinks=[4,6] - self.speedMultiplier = 4. - + #self._p.setJointMotorControl2(car,10,self._p.VELOCITY_CONTROL,targetVelocity=1,force=10) + c = self._p.createConstraint(car,9,car,11,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) + self._p.changeConstraint(c,gearRatio=1, maxForce=10000) - def getActionDimension(self): - return self.nMotors + c = self._p.createConstraint(car,10,car,13,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) + self._p.changeConstraint(c,gearRatio=-1, maxForce=10000) - def getObservationDimension(self): - return len(self.getObservation()) + c = self._p.createConstraint(car,9,car,13,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) + self._p.changeConstraint(c,gearRatio=-1, maxForce=10000) - def getObservation(self): - observation = [] - pos,orn=self._p.getBasePositionAndOrientation(self.racecarUniqueId) - - observation.extend(list(pos)) - observation.extend(list(orn)) - - return observation + c = self._p.createConstraint(car,16,car,18,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) + self._p.changeConstraint(c,gearRatio=1, maxForce=10000) + + c = self._p.createConstraint(car,16,car,19,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) + self._p.changeConstraint(c,gearRatio=-1, maxForce=10000) + + c = self._p.createConstraint(car,17,car,19,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) + self._p.changeConstraint(c,gearRatio=-1, maxForce=10000) + + c = self._p.createConstraint(car,1,car,18,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) + self._p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15, maxForce=10000) + c = self._p.createConstraint(car,3,car,19,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) + self._p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15,maxForce=10000) + + self.steeringLinks = [0,2] + self.maxForce = 20 + self.nMotors = 2 + self.motorizedwheels=[8,15] + self.speedMultiplier = 20. + self.steeringMultiplier = 0.5 + + def getActionDimension(self): + return self.nMotors + + def getObservationDimension(self): + return len(self.getObservation()) + + def getObservation(self): + observation = [] + pos,orn=self._p.getBasePositionAndOrientation(self.racecarUniqueId) + + observation.extend(list(pos)) + observation.extend(list(orn)) + + return observation + + def applyAction(self, motorCommands): + targetVelocity=motorCommands[0]*self.speedMultiplier + #print("targetVelocity") + #print(targetVelocity) + steeringAngle = motorCommands[1]*self.steeringMultiplier + #print("steeringAngle") + #print(steeringAngle) + #print("maxForce") + #print(self.maxForce) + + for motor in self.motorizedwheels: + self._p.setJointMotorControl2(self.racecarUniqueId,motor,self._p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=self.maxForce) + for steer in self.steeringLinks: + self._p.setJointMotorControl2(self.racecarUniqueId,steer,self._p.POSITION_CONTROL,targetPosition=steeringAngle) - def applyAction(self, motorCommands): - targetVelocity=motorCommands[0]*self.speedMultiplier - #print("targetVelocity") - #print(targetVelocity) - steeringAngle = motorCommands[1] - #print("steeringAngle") - #print(steeringAngle) - #print("maxForce") - #print(self.maxForce) - - - for motor in self.motorizedwheels: - self._p.setJointMotorControl2(self.racecarUniqueId,motor,self._p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=self.maxForce) - for steer in self.steeringLinks: - self._p.setJointMotorControl2(self.racecarUniqueId,steer,self._p.POSITION_CONTROL,targetPosition=steeringAngle) - diff --git a/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py index a1fe54cbb..458d265bd 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py @@ -8,7 +8,7 @@ import time import pybullet from . import racecar import random -import bullet_client +from . import bullet_client class RacecarGymEnv(gym.Env): metadata = { @@ -20,6 +20,7 @@ class RacecarGymEnv(gym.Env): urdfRoot="", actionRepeat=50, isEnableSelfCollision=True, + isDiscrete=False, renders=False): print("init") self._timeStep = 0.01 @@ -30,6 +31,7 @@ class RacecarGymEnv(gym.Env): self._ballUniqueId = -1 self._envStepCounter = 0 self._renders = renders + self._isDiscrete = isDiscrete if self._renders: self._p = bullet_client.BulletClient( connection_mode=pybullet.GUI) @@ -42,7 +44,13 @@ class RacecarGymEnv(gym.Env): #print("observationDim") #print(observationDim) observation_high = np.array([np.finfo(np.float32).max] * observationDim) - self.action_space = spaces.Discrete(9) + if (isDiscrete): + self.action_space = spaces.Discrete(9) + else: + action_dim = 2 + self._action_bound = 1 + action_high = np.array([self._action_bound] * action_dim) + self.action_space = spaces.Box(-action_high, action_high) self.observation_space = spaces.Box(-observation_high, observation_high) self.viewer = None @@ -53,10 +61,10 @@ class RacecarGymEnv(gym.Env): #self._p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","plane.urdf")) stadiumobjects = self._p.loadSDF(os.path.join(os.path.dirname(__file__),"../data","stadium.sdf")) #move the stadium objects slightly above 0 - for i in stadiumobjects: - pos,orn = self._p.getBasePositionAndOrientation(i) - newpos = [pos[0],pos[1],pos[2]+0.1] - self._p.resetBasePositionAndOrientation(i,newpos,orn) + #for i in stadiumobjects: + # pos,orn = self._p.getBasePositionAndOrientation(i) + # newpos = [pos[0],pos[1],pos[2]-0.1] + # self._p.resetBasePositionAndOrientation(i,newpos,orn) dist = 5 +2.*random.random() ang = 2.*3.1415925438*random.random() @@ -96,11 +104,15 @@ class RacecarGymEnv(gym.Env): basePos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) #self._p.resetDebugVisualizerCamera(1, 30, -40, basePos) - fwd = [-5,-5,-5,0,0,0,5,5,5] - steerings = [-0.3,0,0.3,-0.3,0,0.3,-0.3,0,0.3] - forward = fwd[action] - steer = steerings[action] - realaction = [forward,steer] + if (self._isDiscrete): + fwd = [-1,-1,-1,0,0,0,1,1,1] + steerings = [-0.6,0,0.6,-0.6,0,0.6,-0.6,0,0.6] + forward = fwd[action] + steer = steerings[action] + realaction = [forward,steer] + else: + realaction = action + self._racecar.applyAction(realaction) for i in range(self._actionRepeat): self._p.stepSimulation() diff --git a/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py index 317a2089e..72b5f1630 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py @@ -5,7 +5,8 @@ from gym import spaces from gym.utils import seeding import numpy as np import time -import pybullet as p +import pybullet +from . import bullet_client from . import racecar import random @@ -17,8 +18,9 @@ class RacecarZEDGymEnv(gym.Env): def __init__(self, urdfRoot="", - actionRepeat=100, + actionRepeat=10, isEnableSelfCollision=True, + isDiscrete=True, renders=True): print("init") self._timeStep = 0.01 @@ -30,11 +32,14 @@ class RacecarZEDGymEnv(gym.Env): self._renders = renders self._width = 100 self._height = 10 - self._p = p + + self._isDiscrete = isDiscrete if self._renders: - p.connect(p.GUI) + self._p = bullet_client.BulletClient( + connection_mode=pybullet.GUI) else: - p.connect(p.DIRECT) + self._p = bullet_client.BulletClient() + self._seed() self.reset() observationDim = len(self.getExtendedObservation()) @@ -42,22 +47,22 @@ class RacecarZEDGymEnv(gym.Env): #print(observationDim) observation_high = np.array([np.finfo(np.float32).max] * observationDim) - self.action_space = spaces.Discrete(6) + self.action_space = spaces.Discrete(9) self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 4)) self.viewer = None def _reset(self): - p.resetSimulation() + self._p.resetSimulation() #p.setPhysicsEngineParameter(numSolverIterations=300) - p.setTimeStep(self._timeStep) - #p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","plane.urdf")) - stadiumobjects = p.loadSDF(os.path.join(os.path.dirname(__file__),"../data","stadium.sdf")) + self._p.setTimeStep(self._timeStep) + #self._p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","plane.urdf")) + stadiumobjects = self._p.loadSDF(os.path.join(os.path.dirname(__file__),"../data","stadium.sdf")) #move the stadium objects slightly above 0 for i in stadiumobjects: - pos,orn = p.getBasePositionAndOrientation(i) + pos,orn = self._p.getBasePositionAndOrientation(i) newpos = [pos[0],pos[1],pos[2]+0.1] - p.resetBasePositionAndOrientation(i,newpos,orn) + self._p.resetBasePositionAndOrientation(i,newpos,orn) dist = 5 +2.*random.random() ang = 2.*3.1415925438*random.random() @@ -66,39 +71,39 @@ class RacecarZEDGymEnv(gym.Env): bally = dist * math.cos(ang) ballz = 1 - self._ballUniqueId = p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","sphere2red.urdf"),[ballx,bally,ballz]) - p.setGravity(0,0,-10) - self._racecar = racecar.Racecar(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) + self._ballUniqueId = self._p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","sphere2red.urdf"),[ballx,bally,ballz]) + self._p.setGravity(0,0,-10) + self._racecar = racecar.Racecar(self._p,urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 for i in range(100): - p.stepSimulation() + self._p.stepSimulation() self._observation = self.getExtendedObservation() return np.array(self._observation) def __del__(self): - p.disconnect() + self._p = 0 def _seed(self, seed=None): self.np_random, seed = seeding.np_random(seed) return [seed] def getExtendedObservation(self): - carpos,carorn = p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) - carmat = p.getMatrixFromQuaternion(carorn) - ballpos,ballorn = p.getBasePositionAndOrientation(self._ballUniqueId) - invCarPos,invCarOrn = p.invertTransform(carpos,carorn) - ballPosInCar,ballOrnInCar = p.multiplyTransforms(invCarPos,invCarOrn,ballpos,ballorn) + carpos,carorn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) + carmat = self._p.getMatrixFromQuaternion(carorn) + ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId) + invCarPos,invCarOrn = self._p.invertTransform(carpos,carorn) + ballPosInCar,ballOrnInCar = self._p.multiplyTransforms(invCarPos,invCarOrn,ballpos,ballorn) dist0 = 0.3 dist1 = 1. eyePos = [carpos[0]+dist0*carmat[0],carpos[1]+dist0*carmat[3],carpos[2]+dist0*carmat[6]+0.3] targetPos = [carpos[0]+dist1*carmat[0],carpos[1]+dist1*carmat[3],carpos[2]+dist1*carmat[6]+0.3] up = [carmat[2],carmat[5],carmat[8]] - viewMat = p.computeViewMatrix(eyePos,targetPos,up) - #viewMat = p.computeViewMatrixFromYawPitchRoll(carpos,1,0,0,0,2) + viewMat = self._p.computeViewMatrix(eyePos,targetPos,up) + #viewMat = self._p.computeViewMatrixFromYawPitchRoll(carpos,1,0,0,0,2) #print("projectionMatrix:") - #print(p.getDebugVisualizerCamera()[3]) + #print(self._p.getDebugVisualizerCamera()[3]) 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] - img_arr = p.getCameraImage(width=self._width,height=self._height,viewMatrix=viewMat,projectionMatrix=projMatrix) + img_arr = self._p.getCameraImage(width=self._width,height=self._height,viewMatrix=viewMat,projectionMatrix=projMatrix) rgb=img_arr[2] np_img_arr = np.reshape(rgb, (self._height, self._width, 4)) self._observation = np_img_arr @@ -106,17 +111,18 @@ class RacecarZEDGymEnv(gym.Env): def _step(self, action): if (self._renders): - basePos,orn = p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) - #p.resetDebugVisualizerCamera(1, 30, -40, basePos) + basePos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) + #self._p.resetDebugVisualizerCamera(1, 30, -40, basePos) - fwd = [5,0,5,10,10,10] - steerings = [-0.5,0,0.5,-0.3,0,0.3] + fwd = [-1,-1,-1,0,0,0,1,1,1] + steerings = [-0.6,0,0.6,-0.6,0,0.6,-0.6,0,0.6] forward = fwd[action] steer = steerings[action] realaction = [forward,steer] + self._racecar.applyAction(realaction) for i in range(self._actionRepeat): - p.stepSimulation() + self._p.stepSimulation() if self._renders: time.sleep(self._timeStep) self._observation = self.getExtendedObservation() @@ -137,7 +143,7 @@ class RacecarZEDGymEnv(gym.Env): return self._envStepCounter>1000 def _reward(self): - closestPoints = p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) + closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) numPt = len(closestPoints) reward=-1000 diff --git a/examples/pybullet/gym/pybullet_envs/data/differential/diff_arm.stl b/examples/pybullet/gym/pybullet_envs/data/differential/diff_arm.stl new file mode 100644 index 000000000..8d33d772f Binary files /dev/null and b/examples/pybullet/gym/pybullet_envs/data/differential/diff_arm.stl differ diff --git a/examples/pybullet/gym/pybullet_envs/data/differential/diff_carrier.stl b/examples/pybullet/gym/pybullet_envs/data/differential/diff_carrier.stl new file mode 100644 index 000000000..2b9cd9cf6 Binary files /dev/null and b/examples/pybullet/gym/pybullet_envs/data/differential/diff_carrier.stl differ diff --git a/examples/pybullet/gym/pybullet_envs/data/differential/diff_carrier_cover.stl b/examples/pybullet/gym/pybullet_envs/data/differential/diff_carrier_cover.stl new file mode 100644 index 000000000..8afd68086 Binary files /dev/null and b/examples/pybullet/gym/pybullet_envs/data/differential/diff_carrier_cover.stl differ diff --git a/examples/pybullet/gym/pybullet_envs/data/differential/diff_leftshaft.stl b/examples/pybullet/gym/pybullet_envs/data/differential/diff_leftshaft.stl new file mode 100644 index 000000000..f44b120ef Binary files /dev/null and b/examples/pybullet/gym/pybullet_envs/data/differential/diff_leftshaft.stl differ diff --git a/examples/pybullet/gym/pybullet_envs/data/differential/diff_motor_cover.stl b/examples/pybullet/gym/pybullet_envs/data/differential/diff_motor_cover.stl new file mode 100644 index 000000000..a664860b3 Binary files /dev/null and b/examples/pybullet/gym/pybullet_envs/data/differential/diff_motor_cover.stl differ diff --git a/examples/pybullet/gym/pybullet_envs/data/differential/diff_pinion.stl b/examples/pybullet/gym/pybullet_envs/data/differential/diff_pinion.stl new file mode 100644 index 000000000..7e9638779 Binary files /dev/null and b/examples/pybullet/gym/pybullet_envs/data/differential/diff_pinion.stl differ diff --git a/examples/pybullet/gym/pybullet_envs/data/differential/diff_rightshaft.stl b/examples/pybullet/gym/pybullet_envs/data/differential/diff_rightshaft.stl new file mode 100644 index 000000000..db4024912 Binary files /dev/null and b/examples/pybullet/gym/pybullet_envs/data/differential/diff_rightshaft.stl differ diff --git a/examples/pybullet/gym/pybullet_envs/data/differential/diff_ring.stl b/examples/pybullet/gym/pybullet_envs/data/differential/diff_ring.stl new file mode 100644 index 000000000..7ca2a6c62 Binary files /dev/null and b/examples/pybullet/gym/pybullet_envs/data/differential/diff_ring.stl differ diff --git a/examples/pybullet/gym/pybullet_envs/data/differential/diff_ring.urdf b/examples/pybullet/gym/pybullet_envs/data/differential/diff_ring.urdf new file mode 100644 index 000000000..3e20aebd7 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/data/differential/diff_ring.urdf @@ -0,0 +1,198 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_envs/data/differential/diff_side.stl b/examples/pybullet/gym/pybullet_envs/data/differential/diff_side.stl new file mode 100644 index 000000000..f8ed10e8a Binary files /dev/null and b/examples/pybullet/gym/pybullet_envs/data/differential/diff_side.stl differ diff --git a/examples/pybullet/gym/pybullet_envs/data/differential/diff_spider.stl b/examples/pybullet/gym/pybullet_envs/data/differential/diff_spider.stl new file mode 100644 index 000000000..1c152401f Binary files /dev/null and b/examples/pybullet/gym/pybullet_envs/data/differential/diff_spider.stl differ diff --git a/examples/pybullet/gym/pybullet_envs/data/differential/diff_spider_shaft.stl b/examples/pybullet/gym/pybullet_envs/data/differential/diff_spider_shaft.stl new file mode 100644 index 000000000..12b2213f1 Binary files /dev/null and b/examples/pybullet/gym/pybullet_envs/data/differential/diff_spider_shaft.stl differ diff --git a/examples/pybullet/gym/pybullet_envs/data/differential/diff_stand.stl b/examples/pybullet/gym/pybullet_envs/data/differential/diff_stand.stl new file mode 100644 index 000000000..168bdb1dc Binary files /dev/null and b/examples/pybullet/gym/pybullet_envs/data/differential/diff_stand.stl differ diff --git a/examples/pybullet/gym/pybullet_envs/data/differential/modelorigin.txt b/examples/pybullet/gym/pybullet_envs/data/differential/modelorigin.txt new file mode 100644 index 000000000..af250e7d6 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/data/differential/modelorigin.txt @@ -0,0 +1,2 @@ +stl files were copied from http://www.otvinta.com/download09.html +URDF file was manually created, along with mimicJointConstraint.py \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_envs/data/racecar/racecar_differential.urdf b/examples/pybullet/gym/pybullet_envs/data/racecar/racecar_differential.urdf index c4f6ee42e..6ef547917 100644 --- a/examples/pybullet/gym/pybullet_envs/data/racecar/racecar_differential.urdf +++ b/examples/pybullet/gym/pybullet_envs/data/racecar/racecar_differential.urdf @@ -35,7 +35,7 @@ - + @@ -68,7 +68,7 @@ - + @@ -165,7 +165,7 @@ - + @@ -209,7 +209,7 @@ - + @@ -337,7 +337,7 @@ - + @@ -377,7 +377,7 @@ - + @@ -416,7 +416,7 @@ - + @@ -456,7 +456,7 @@ - + @@ -492,7 +492,7 @@ - + @@ -528,7 +528,7 @@ - + @@ -568,7 +568,7 @@ - + @@ -607,7 +607,7 @@ - + @@ -645,7 +645,7 @@ - + @@ -681,7 +681,7 @@ - + @@ -736,8 +736,5 @@ - - - diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index 573f7b284..e034c7252 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -3,7 +3,7 @@ import numpy as np import pybullet as p -class MujocoXmlBaseBulletEnv(gym.Env): +class MJCFBaseBulletEnv(gym.Env): """ Base class for MuJoCo .xml environments in a Scene. These environments create single-player scenes and behave like normal Gym environments, if diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_pybullet_racecar.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_pybullet_racecar.py index 76bfa9d03..717d82343 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_pybullet_racecar.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_pybullet_racecar.py @@ -12,7 +12,7 @@ from baselines import deepq def main(): - env = RacecarGymEnv(renders=True) + env = RacecarGymEnv(renders=False,isDiscrete=True) act = deepq.load("racecar_model.pkl") print(act) while True: diff --git a/examples/pybullet/gym/pybullet_envs/examples/racecarGymEnvTest.py b/examples/pybullet/gym/pybullet_envs/examples/racecarGymEnvTest.py index f7445625e..a2ef5a855 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/racecarGymEnvTest.py +++ b/examples/pybullet/gym/pybullet_envs/examples/racecarGymEnvTest.py @@ -5,30 +5,34 @@ parentdir = os.path.dirname(os.path.dirname(currentdir)) os.sys.path.insert(0,parentdir) from pybullet_envs.bullet.racecarGymEnv import RacecarGymEnv -environment = RacecarGymEnv(renders=True) +isDiscrete = True + +environment = RacecarGymEnv(renders=True, isDiscrete=isDiscrete) environment.reset() targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0) -steeringSlider = environment._p.addUserDebugParameter("steering",-0.5,0.5,0) +steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0) while (True): targetVelocity = environment._p.readUserDebugParameter(targetVelocitySlider) steeringAngle = environment._p.readUserDebugParameter(steeringSlider) - discreteAction = 0 - if (targetVelocity<-0.33): - discreteAction=0 + if (isDiscrete): + discreteAction = 0 + if (targetVelocity<-0.33): + discreteAction=0 + else: + if (targetVelocity>0.33): + discreteAction=6 + else: + discreteAction=3 + if (steeringAngle>-0.17): + if (steeringAngle>0.17): + discreteAction=discreteAction+2 + else: + discreteAction=discreteAction+1 + action=discreteAction else: - if (targetVelocity>0.33): - discreteAction=6 - else: - discreteAction=3 - if (steeringAngle>-0.17): - if (steeringAngle>0.17): - discreteAction=discreteAction+2 - else: - discreteAction=discreteAction+1 - - action=discreteAction + action=[targetVelocity,steeringAngle] state, reward, done, info = environment.step(action) obs = environment.getExtendedObservation() print("obs") diff --git a/examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py b/examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py index 2a33d456a..7e64c7bb3 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py +++ b/examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py @@ -9,7 +9,7 @@ print ("hello") environment = RacecarZEDGymEnv(renders=True) targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0) -steeringSlider = environment._p.addUserDebugParameter("steering",-0.5,0.5,0) +steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0) while (True): targetVelocity = environment._p.readUserDebugParameter(targetVelocitySlider) @@ -27,8 +27,8 @@ while (True): discreteAction=discreteAction+2 else: discreteAction=discreteAction+1 - action=discreteAction + state, reward, done, info = environment.step(action) obs = environment.getExtendedObservation() print("obs") diff --git a/examples/pybullet/gym/pybullet_envs/examples/train_pybullet_racecar.py b/examples/pybullet/gym/pybullet_envs/examples/train_pybullet_racecar.py index 10d0a8328..c17bc11c4 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/train_pybullet_racecar.py +++ b/examples/pybullet/gym/pybullet_envs/examples/train_pybullet_racecar.py @@ -23,7 +23,7 @@ def callback(lcl, glb): def main(): - env = RacecarGymEnv(renders=False) + env = RacecarGymEnv(renders=False,isDiscrete=True) model = deepq.models.mlp([64]) act = deepq.learn( env, diff --git a/examples/pybullet/gym/pybullet_envs/examples/train_pybullet_zed_racecar.py b/examples/pybullet/gym/pybullet_envs/examples/train_pybullet_zed_racecar.py index 499d763f4..68c5b0046 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/train_pybullet_zed_racecar.py +++ b/examples/pybullet/gym/pybullet_envs/examples/train_pybullet_zed_racecar.py @@ -22,7 +22,7 @@ def callback(lcl, glb): def main(): - env = RacecarZEDGymEnv(renders=False) + env = RacecarZEDGymEnv(renders=False, isDiscrete=True) model = deepq.models.cnn_to_mlp( convs=[(32, 8, 4), (64, 4, 2), (64, 3, 1)], hiddens=[256], diff --git a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py index 32d040e02..662564a06 100644 --- a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py +++ b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py @@ -1,12 +1,12 @@ from .scene_stadium import SinglePlayerStadiumScene -from .env_bases import MujocoXmlBaseBulletEnv +from .env_bases import MJCFBaseBulletEnv import numpy as np from robot_locomotors import Hopper, Walker2D, HalfCheetah, Ant, Humanoid -class WalkerBaseBulletEnv(MujocoXmlBaseBulletEnv): +class WalkerBaseBulletEnv(MJCFBaseBulletEnv): def __init__(self, robot): - MujocoXmlBaseBulletEnv.__init__(self, robot) + MJCFBaseBulletEnv.__init__(self, robot) self.camera_x = 0 self.walk_target_x = 1e3 # kilometer away self.walk_target_y = 0 @@ -16,7 +16,7 @@ class WalkerBaseBulletEnv(MujocoXmlBaseBulletEnv): return self.stadium_scene def _reset(self): - r = MujocoXmlBaseBulletEnv._reset(self) + r = MJCFBaseBulletEnv._reset(self) self.parts, self.jdict, self.ordered_joints, self.robot_body = self.robot.addToScene( self.stadium_scene.ground_plane_mjcf) self.ground_ids = set([(self.parts[f].bodies[self.parts[f].bodyIndex], self.parts[f].bodyPartIndex) for f in diff --git a/examples/pybullet/gym/pybullet_envs/gym_pendulum_envs.py b/examples/pybullet/gym/pybullet_envs/gym_pendulum_envs.py index ce83f3e74..a0a78d20e 100644 --- a/examples/pybullet/gym/pybullet_envs/gym_pendulum_envs.py +++ b/examples/pybullet/gym/pybullet_envs/gym_pendulum_envs.py @@ -1,14 +1,14 @@ from .scene_abstract import SingleRobotEmptyScene -from .env_bases import MujocoXmlBaseBulletEnv +from .env_bases import MJCFBaseBulletEnv from robot_pendula import InvertedPendulum, InvertedPendulumSwingup, InvertedDoublePendulum import gym, gym.spaces, gym.utils, gym.utils.seeding import numpy as np import os, sys -class InvertedPendulumBulletEnv(MujocoXmlBaseBulletEnv): +class InvertedPendulumBulletEnv(MJCFBaseBulletEnv): def __init__(self): self.robot = InvertedPendulum() - MujocoXmlBaseBulletEnv.__init__(self, self.robot) + MJCFBaseBulletEnv.__init__(self, self.robot) def create_single_player_scene(self): return SingleRobotEmptyScene(gravity=9.8, timestep=0.0165, frame_skip=1) @@ -34,12 +34,12 @@ class InvertedPendulumBulletEnv(MujocoXmlBaseBulletEnv): class InvertedPendulumSwingupBulletEnv(InvertedPendulumBulletEnv): def __init__(self): self.robot = InvertedPendulumSwingup() - MujocoXmlBaseBulletEnv.__init__(self, self.robot) + MJCFBaseBulletEnv.__init__(self, self.robot) -class InvertedDoublePendulumBulletEnv(MujocoXmlBaseBulletEnv): +class InvertedDoublePendulumBulletEnv(MJCFBaseBulletEnv): def __init__(self): self.robot = InvertedDoublePendulum() - MujocoXmlBaseBulletEnv.__init__(self, self.robot) + MJCFBaseBulletEnv.__init__(self, self.robot) def create_single_player_scene(self): return SingleRobotEmptyScene(gravity=9.8, timestep=0.0165, frame_skip=1) diff --git a/examples/pybullet/gym/pybullet_envs/robot_bases.py b/examples/pybullet/gym/pybullet_envs/robot_bases.py index dd9dfbf3c..4ff262723 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_bases.py +++ b/examples/pybullet/gym/pybullet_envs/robot_bases.py @@ -4,7 +4,7 @@ import numpy as np import os -class MujocoXmlBasedRobot: +class MJCFBasedRobot: """ Base class for mujoco .xml based agents. """ diff --git a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py index 92cda05ac..51149080b 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py +++ b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py @@ -1,10 +1,10 @@ -from robot_bases import MujocoXmlBasedRobot +from robot_bases import MJCFBasedRobot import numpy as np -class WalkerBase(MujocoXmlBasedRobot): +class WalkerBase(MJCFBasedRobot): def __init__(self, fn, robot_name, action_dim, obs_dim, power): - MujocoXmlBasedRobot.__init__(self, fn, robot_name, action_dim, obs_dim) + MJCFBasedRobot.__init__(self, fn, robot_name, action_dim, obs_dim) self.power = power self.camera_x = 0 self.walk_target_x = 1e3 # kilometer away diff --git a/examples/pybullet/gym/pybullet_envs/robot_pendula.py b/examples/pybullet/gym/pybullet_envs/robot_pendula.py index 21d10f52d..1c6b14b05 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_pendula.py +++ b/examples/pybullet/gym/pybullet_envs/robot_pendula.py @@ -1,11 +1,11 @@ -from robot_bases import MujocoXmlBasedRobot +from robot_bases import MJCFBasedRobot import numpy as np -class InvertedPendulum(MujocoXmlBasedRobot): +class InvertedPendulum(MJCFBasedRobot): swingup = False force_gain = 12 # TODO: Try to find out why we need to scale the force def __init__(self): - MujocoXmlBasedRobot.__init__(self, 'inverted_pendulum.xml', 'cart', action_dim=1, obs_dim=5) + MJCFBasedRobot.__init__(self, 'inverted_pendulum.xml', 'cart', action_dim=1, obs_dim=5) def robot_specific_reset(self): self.pole = self.parts["pole"] @@ -53,9 +53,9 @@ class InvertedPendulumSwingup(InvertedPendulum): force_gain = 2.2 # TODO: Try to find out why we need to scale the force -class InvertedDoublePendulum(MujocoXmlBasedRobot): +class InvertedDoublePendulum(MJCFBasedRobot): def __init__(self): - MujocoXmlBasedRobot.__init__(self, 'inverted_double_pendulum.xml', 'cart', action_dim=1, obs_dim=9) + MJCFBasedRobot.__init__(self, 'inverted_double_pendulum.xml', 'cart', action_dim=1, obs_dim=9) def robot_specific_reset(self): self.pole2 = self.parts["pole2"]