From 3d702879c557826be094d4cd59a8f2595b7f4937 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 7 Sep 2017 11:06:42 -0700 Subject: [PATCH 1/9] pybullet: improvements in Gym Ant environment (work-in-progress) --- examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py | 2 +- examples/pybullet/gym/pybullet_envs/robot_bases.py | 4 ++-- examples/pybullet/gym/pybullet_envs/robot_locomotors.py | 2 +- examples/pybullet/gym/pybullet_envs/scene_abstract.py | 3 ++- examples/pybullet/gym/pybullet_envs/scene_stadium.py | 3 +-- 5 files changed, 7 insertions(+), 7 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py index d13290449..bc75f7bfd 100644 --- a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py +++ b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py @@ -15,7 +15,7 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv): self.walk_target_y = 0 def create_single_player_scene(self): - self.stadium_scene = SinglePlayerStadiumScene(gravity=9.8, timestep=0.0165/4, frame_skip=4) + self.stadium_scene = SinglePlayerStadiumScene(gravity=9.8, timestep=0.0165, frame_skip=4) return self.stadium_scene def _reset(self): diff --git a/examples/pybullet/gym/pybullet_envs/robot_bases.py b/examples/pybullet/gym/pybullet_envs/robot_bases.py index 573bfbd89..165ba8a09 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_bases.py +++ b/examples/pybullet/gym/pybullet_envs/robot_bases.py @@ -13,7 +13,7 @@ class MJCFBasedRobot: Base class for mujoco .xml based agents. """ - self_collision = False + self_collision = True def __init__(self, model_xml, robot_name, action_dim, obs_dim): self.parts = None @@ -87,7 +87,7 @@ class MJCFBasedRobot: if self.self_collision: self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( - p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml), flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)) + p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml), flags=p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)) else: self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml))) diff --git a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py index 51149080b..772dee5c6 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py +++ b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py @@ -114,7 +114,7 @@ class Ant(WalkerBase): foot_list = ['front_left_foot', 'front_right_foot', 'left_back_foot', 'right_back_foot'] def __init__(self): - WalkerBase.__init__(self, "ant.xml", "torso", action_dim=8, obs_dim=28, power=10.5) + WalkerBase.__init__(self, "ant.xml", "torso", action_dim=8, obs_dim=28, power=2.5) def alive_bonus(self, z, pitch): return +1 if z > 0.26 else -1 # 0.25 is central sphere rad, die if it scrapes the ground diff --git a/examples/pybullet/gym/pybullet_envs/scene_abstract.py b/examples/pybullet/gym/pybullet_envs/scene_abstract.py index 663401458..152427a25 100644 --- a/examples/pybullet/gym/pybullet_envs/scene_abstract.py +++ b/examples/pybullet/gym/pybullet_envs/scene_abstract.py @@ -68,7 +68,8 @@ class World: def clean_everything(self): p.resetSimulation() p.setGravity(0, 0, -self.gravity) - p.setPhysicsEngineParameter(fixedTimeStep=self.timestep, numSolverIterations=5, numSubSteps=2) + p.setDefaultContactERP(0.9) + p.setPhysicsEngineParameter(fixedTimeStep=self.timestep, numSolverIterations=5, numSubSteps=4) def step(self, frame_skip): p.stepSimulation() diff --git a/examples/pybullet/gym/pybullet_envs/scene_stadium.py b/examples/pybullet/gym/pybullet_envs/scene_stadium.py index 64f64e30d..023e6b13f 100644 --- a/examples/pybullet/gym/pybullet_envs/scene_stadium.py +++ b/examples/pybullet/gym/pybullet_envs/scene_stadium.py @@ -19,8 +19,7 @@ class StadiumScene(Scene): # stadium_pose = cpp_household.Pose() # if self.zero_at_running_strip_start_line: # stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants - filename = os.path.join(pybullet_data.getDataPath(),"stadium.sdf") - print(filename) + filename = os.path.join(pybullet_data.getDataPath(),"stadium_no_collision.sdf") self.stadium = p.loadSDF(filename) planeName = os.path.join(pybullet_data.getDataPath(),"mjcf/ground_plane.xml") From b0e50d0d5ad40d7eb1096874516662a107ab7694 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 7 Sep 2017 11:17:38 -0700 Subject: [PATCH 2/9] pybullet Gym envs: add more sleep in the enjoy functions to see what's happening. --- .../pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py | 2 +- .../examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py | 2 +- .../examples/enjoy_TF_HopperBulletEnv_v0_2017may.py | 2 +- .../examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py | 2 +- .../examples/enjoy_TF_Walker2DBulletEnv_v0_2017may.py | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py index cac33f88f..482b78be3 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py @@ -48,7 +48,7 @@ def main(): obs = env.reset() while 1: - time.sleep(0.001) + time.sleep(0.01) a = pi.act(obs) obs, r, done, _ = env.step(a) score += r diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py index 8bad97f97..27b16c1c4 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py @@ -51,7 +51,7 @@ def main(): obs = env.reset() while 1: - time.sleep(0.001) + time.sleep(0.01) a = pi.act(obs) obs, r, done, _ = env.step(a) score += r diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py index e260568c9..dc26d57b3 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py @@ -51,7 +51,7 @@ def main(): obs = env.reset() while 1: - time.sleep(0.001) + time.sleep(0.01) a = pi.act(obs) obs, r, done, _ = env.step(a) score += r diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py index 23d7a174c..c564cf675 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py @@ -46,7 +46,7 @@ def main(): obs = env.reset() while 1: - time.sleep(0.001) + time.sleep(0.01) a = pi.act(obs) obs, r, done, _ = env.step(a) score += r diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_Walker2DBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_Walker2DBulletEnv_v0_2017may.py index d01e08f1c..dfb6439fc 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_Walker2DBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_Walker2DBulletEnv_v0_2017may.py @@ -50,7 +50,7 @@ def main(): obs = env.reset() while 1: - time.sleep(0.001) + time.sleep(0.01) a = pi.act(obs) obs, r, done, _ = env.step(a) score += r From c30e9aea923cf15d847469202777ea90f741c5f8 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 7 Sep 2017 11:23:41 -0700 Subject: [PATCH 3/9] revert minitaur.urdf to previous (backward compatibility) and add _v1 for better version. --- data/quadruped/minitaur.urdf | 77 +- data/quadruped/minitaur_v1.urdf | 958 ++++++++++++++++++ examples/pybullet/examples/quadruped.py | 2 +- .../pybullet_data/quadruped/minitaur_v1.urdf | 958 ++++++++++++++++++ 4 files changed, 1941 insertions(+), 54 deletions(-) create mode 100644 data/quadruped/minitaur_v1.urdf create mode 100644 examples/pybullet/gym/pybullet_data/quadruped/minitaur_v1.urdf diff --git a/data/quadruped/minitaur.urdf b/data/quadruped/minitaur.urdf index 7bbeed76b..fa4bc2d37 100644 --- a/data/quadruped/minitaur.urdf +++ b/data/quadruped/minitaur.urdf @@ -267,11 +267,6 @@ - - - - - @@ -432,10 +427,8 @@ - - - - + + @@ -455,12 +448,11 @@ - - + @@ -503,10 +495,8 @@ - - - - + + @@ -526,12 +516,11 @@ - - + @@ -572,10 +561,8 @@ - - - - + + @@ -595,12 +582,11 @@ - - + @@ -642,10 +628,8 @@ - - - - + + @@ -665,12 +649,11 @@ - - + @@ -709,10 +692,8 @@ - - - - + + @@ -732,12 +713,11 @@ - - + @@ -780,10 +760,8 @@ - - - - + + @@ -803,12 +781,11 @@ - - + @@ -849,10 +826,8 @@ - - - - + + @@ -872,12 +847,11 @@ - - + @@ -917,10 +891,8 @@ - - - - + + @@ -940,12 +912,11 @@ - - + diff --git a/data/quadruped/minitaur_v1.urdf b/data/quadruped/minitaur_v1.urdf new file mode 100644 index 000000000..7bbeed76b --- /dev/null +++ b/data/quadruped/minitaur_v1.urdf @@ -0,0 +1,958 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/pybullet/examples/quadruped.py b/examples/pybullet/examples/quadruped.py index c6d93d05f..19527fa47 100644 --- a/examples/pybullet/examples/quadruped.py +++ b/examples/pybullet/examples/quadruped.py @@ -41,7 +41,7 @@ p.setTimeStep(fixedTimeStep) orn = p.getQuaternionFromEuler([0,0,0.4]) p.setRealTimeSimulation(0) -quadruped = p.loadURDF("quadruped/minitaur.urdf",[1,-1,.3],orn,useFixedBase=False, useMaximalCoordinates=useMaximalCoordinates) +quadruped = p.loadURDF("quadruped/minitaur_v1.urdf",[1,-1,.3],orn,useFixedBase=False, useMaximalCoordinates=useMaximalCoordinates) nJoints = p.getNumJoints(quadruped) diff --git a/examples/pybullet/gym/pybullet_data/quadruped/minitaur_v1.urdf b/examples/pybullet/gym/pybullet_data/quadruped/minitaur_v1.urdf new file mode 100644 index 000000000..7bbeed76b --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/quadruped/minitaur_v1.urdf @@ -0,0 +1,958 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 9ecd345783a2cdc076a44d514c8bea598899cd90 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 7 Sep 2017 11:40:11 -0700 Subject: [PATCH 4/9] backwards compatibility... --- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 23 +++++++++++++++---- .../Importers/ImportURDFDemo/URDFJointTypes.h | 1 + .../PhysicsServerCommandProcessor.cpp | 5 +++- 3 files changed, 24 insertions(+), 5 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 9ef61ddc7..9ee66c85f 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -390,8 +390,16 @@ void ConvertURDF2BulletInternal( { //b3Printf("Fixed joint\n"); - btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB); - + btGeneric6DofSpring2Constraint* dof6 = 0; + + //backward compatibility + if (flags & URDF_ORDER_TYPED_CONSTRAINT ) + { + dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB); + } else + { + dof6 = creation.createFixedJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA); + } if (enableConstraints) world1->addConstraint(dof6,true); } @@ -417,8 +425,15 @@ void ConvertURDF2BulletInternal( } else { - btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit); - + btGeneric6DofSpring2Constraint* dof6 = 0; + //backwards compatibility + if (flags & URDF_ORDER_TYPED_CONSTRAINT ) + { + dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit); + } else + { + dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit); + } if (enableConstraints) world1->addConstraint(dof6,true); //b3Printf("Revolute/Continuous joint\n"); diff --git a/examples/Importers/ImportURDFDemo/URDFJointTypes.h b/examples/Importers/ImportURDFDemo/URDFJointTypes.h index 2edad8617..5f470449d 100644 --- a/examples/Importers/ImportURDFDemo/URDFJointTypes.h +++ b/examples/Importers/ImportURDFDemo/URDFJointTypes.h @@ -61,6 +61,7 @@ enum UrdfCollisionFlags URDF_FORCE_CONCAVE_TRIMESH=1, URDF_HAS_COLLISION_GROUP=2, URDF_HAS_COLLISION_MASK=4, + URDF_ORDER_TYPED_CONSTRAINT=8, }; struct UrdfMaterialColor diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 747500bc1..a51202ee8 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2416,7 +2416,8 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, MyMultiBodyCreator creation(m_data->m_guiHelper); u2b.getRootTransformInWorld(rootTrans); - ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags); + flags |= URDF_ORDER_TYPED_CONSTRAINT; + ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags); @@ -2695,6 +2696,8 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto // printf("urdf root link index = %d\n",rootLinkIndex); MyMultiBodyCreator creation(m_data->m_guiHelper); + flags |= URDF_ORDER_TYPED_CONSTRAINT; + ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags); for (int i=0;i Date: Thu, 7 Sep 2017 12:22:35 -0700 Subject: [PATCH 5/9] don't build pybullet unless asked for it, see this: https://travis-ci.org/bulletphysics/bullet3/jobs/273018723 --- CMakeLists.txt | 3 --- 1 file changed, 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0937ea1aa..e24f3dfe5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -315,9 +315,6 @@ FIND_PACKAGE(PythonInterp ${PYTHON_VERSION_PYBULLET} ${EXACT_PYTHON_VERSION_FLAG # python library should exactly match that of the interpreter FIND_PACKAGE(PythonLibs ${PYTHON_VERSION_STRING} EXACT) SET(DEFAULT_BUILD_PYBULLET OFF) -IF(PYTHONLIBS_FOUND) - SET(DEFAULT_BUILD_PYBULLET ON) -ENDIF(PYTHONLIBS_FOUND) OPTION(BUILD_PYBULLET "Set when you want to build pybullet (Python bindings for Bullet)" ${DEFAULT_BUILD_PYBULLET}) OPTION(BUILD_ENET "Set when you want to build apps with enet UDP networking support" ON) From cbe0d0aff2867034b1a7f3a79c7543acfa610bec Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 7 Sep 2017 14:27:00 -0700 Subject: [PATCH 6/9] pybullet fix: accidently enable self-collision --- examples/Importers/ImportURDFDemo/URDF2Bullet.cpp | 4 ++-- examples/Importers/ImportURDFDemo/URDF2Bullet.h | 1 + examples/Importers/ImportURDFDemo/URDFJointTypes.h | 1 - examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 3 ++- examples/SharedMemory/SharedMemoryPublic.h | 2 ++ 5 files changed, 7 insertions(+), 4 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 9ee66c85f..7210b3219 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -393,7 +393,7 @@ void ConvertURDF2BulletInternal( btGeneric6DofSpring2Constraint* dof6 = 0; //backward compatibility - if (flags & URDF_ORDER_TYPED_CONSTRAINT ) + if (flags & CUF_RESERVED ) { dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB); } else @@ -427,7 +427,7 @@ void ConvertURDF2BulletInternal( btGeneric6DofSpring2Constraint* dof6 = 0; //backwards compatibility - if (flags & URDF_ORDER_TYPED_CONSTRAINT ) + if (flags & CUF_RESERVED ) { dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit); } else diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.h b/examples/Importers/ImportURDFDemo/URDF2Bullet.h index ccc63c4d9..68acf497d 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.h +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.h @@ -22,6 +22,7 @@ enum ConvertURDFFlags { CUF_USE_SELF_COLLISION=8, CUF_USE_SELF_COLLISION_EXCLUDE_PARENT=16, CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS=32, + CUF_RESERVED=64, }; void ConvertURDF2Bullet(const URDFImporterInterface& u2b, diff --git a/examples/Importers/ImportURDFDemo/URDFJointTypes.h b/examples/Importers/ImportURDFDemo/URDFJointTypes.h index 5f470449d..2edad8617 100644 --- a/examples/Importers/ImportURDFDemo/URDFJointTypes.h +++ b/examples/Importers/ImportURDFDemo/URDFJointTypes.h @@ -61,7 +61,6 @@ enum UrdfCollisionFlags URDF_FORCE_CONCAVE_TRIMESH=1, URDF_HAS_COLLISION_GROUP=2, URDF_HAS_COLLISION_MASK=4, - URDF_ORDER_TYPED_CONSTRAINT=8, }; struct UrdfMaterialColor diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index a51202ee8..47d3b36b8 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2416,7 +2416,8 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, MyMultiBodyCreator creation(m_data->m_guiHelper); u2b.getRootTransformInWorld(rootTrans); - flags |= URDF_ORDER_TYPED_CONSTRAINT; + //CUF_RESERVED is a temporary flag, for backward compatibility purposes + flags |= CUF_RESERVED; ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags); diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 13b8f18a5..cfa605bc4 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -589,6 +589,8 @@ enum eURDF_Flags URDF_USE_SELF_COLLISION=8,//see CUF_USE_SELF_COLLISION URDF_USE_SELF_COLLISION_EXCLUDE_PARENT=16, URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS=32, + URDF_RESERVED=64, + }; enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes From 2d3b1e8eaad6ea050341afe24f24787a33e260f1 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 7 Sep 2017 14:27:23 -0700 Subject: [PATCH 7/9] bump up pybullet version --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index afd39b132..2f30a8336 100644 --- a/setup.py +++ b/setup.py @@ -440,7 +440,7 @@ print("-----") setup( name = 'pybullet', - version='1.3.1', + version='1.3.2', 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 667115665210f4e346526a8a53cdce5d9f8e6042 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 7 Sep 2017 14:34:37 -0700 Subject: [PATCH 8/9] tweak pybullet title and bump up version --- setup.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/setup.py b/setup.py index 2f30a8336..b67974942 100644 --- a/setup.py +++ b/setup.py @@ -440,9 +440,9 @@ print("-----") setup( name = 'pybullet', - version='1.3.2', - 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.', + version='1.3.3', + description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', + long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', author='Erwin Coumans, Yunfei Bai, Jasmine Hsu', author_email='erwincoumans@google.com', From 03cde2ee9bed76c186a1507be6319328c3bf728c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 7 Sep 2017 14:40:46 -0700 Subject: [PATCH 9/9] fix build? (pybullet shouldn't be build by default) --- CMakeLists.txt | 33 +++++++++++++++++---------------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e24f3dfe5..01bb95519 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -300,22 +300,23 @@ ENDIF() OPTION(BUILD_BULLET3 "Set when you want to build Bullet 3" ON) -# Optional Python configuration -# builds pybullet automatically if all the requirements are met -SET(PYTHON_VERSION_PYBULLET "2.7" CACHE STRING "Python version pybullet will use.") -SET(Python_ADDITIONAL_VERSIONS 2.7 2.7.3 3 3.0 3.1 3.2 3.3 3.4 3.5 3.6) -SET_PROPERTY(CACHE PYTHON_VERSION_PYBULLET PROPERTY STRINGS ${Python_ADDITIONAL_VERSIONS}) -SET(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/build3/cmake ${CMAKE_MODULE_PATH}) -OPTION(EXACT_PYTHON_VERSION "Require Python and match PYTHON_VERSION_PYBULLET exactly, e.g. 2.7.3" OFF) -IF(EXACT_PYTHON_VERSION) - set(EXACT_PYTHON_VERSION_FLAG EXACT REQUIRED) -ENDIF(EXACT_PYTHON_VERSION) -# first find the python interpreter -FIND_PACKAGE(PythonInterp ${PYTHON_VERSION_PYBULLET} ${EXACT_PYTHON_VERSION_FLAG}) -# python library should exactly match that of the interpreter -FIND_PACKAGE(PythonLibs ${PYTHON_VERSION_STRING} EXACT) -SET(DEFAULT_BUILD_PYBULLET OFF) -OPTION(BUILD_PYBULLET "Set when you want to build pybullet (Python bindings for Bullet)" ${DEFAULT_BUILD_PYBULLET}) +OPTION(BUILD_PYBULLET "Set when you want to build pybullet (Python bindings for Bullet)" OFF) +IF(BUILD_PYBULLET) + # Optional Python configuration + # builds pybullet automatically if all the requirements are met + SET(PYTHON_VERSION_PYBULLET "2.7" CACHE STRING "Python version pybullet will use.") + SET(Python_ADDITIONAL_VERSIONS 2.7 2.7.3 3 3.0 3.1 3.2 3.3 3.4 3.5 3.6) + SET_PROPERTY(CACHE PYTHON_VERSION_PYBULLET PROPERTY STRINGS ${Python_ADDITIONAL_VERSIONS}) + SET(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/build3/cmake ${CMAKE_MODULE_PATH}) + OPTION(EXACT_PYTHON_VERSION "Require Python and match PYTHON_VERSION_PYBULLET exactly, e.g. 2.7.3" OFF) + IF(EXACT_PYTHON_VERSION) + set(EXACT_PYTHON_VERSION_FLAG EXACT REQUIRED) + ENDIF(EXACT_PYTHON_VERSION) + # first find the python interpreter + FIND_PACKAGE(PythonInterp ${PYTHON_VERSION_PYBULLET} ${EXACT_PYTHON_VERSION_FLAG}) + # python library should exactly match that of the interpreter + FIND_PACKAGE(PythonLibs ${PYTHON_VERSION_STRING} EXACT) +ENDIF() OPTION(BUILD_ENET "Set when you want to build apps with enet UDP networking support" ON) OPTION(BUILD_CLSOCKET "Set when you want to build apps with enet TCP networking support" ON)