diff --git a/examples/pybullet/gym/pybullet_data/xarm/xarm6.urdf b/examples/pybullet/gym/pybullet_data/xarm/xarm6.urdf
deleted file mode 100644
index f7ad51d9f..000000000
--- a/examples/pybullet/gym/pybullet_data/xarm/xarm6.urdf
+++ /dev/null
@@ -1,314 +0,0 @@
-
-
-
-
-
-
-
-
-
- /xarm
-
- gazebo_ros_control/DefaultRobotHWSim
- true
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- transmission_interface/SimpleTransmission
-
- hardware_interface/PositionJointInterface
-
-
- hardware_interface/PositionJointInterface
- 100
-
-
-
- transmission_interface/SimpleTransmission
-
- hardware_interface/PositionJointInterface
-
-
- hardware_interface/PositionJointInterface
- reduction
-
-
-
- transmission_interface/SimpleTransmission
-
- hardware_interface/PositionJointInterface
-
-
- hardware_interface/PositionJointInterface
- reduction
-
-
-
- transmission_interface/SimpleTransmission
-
- hardware_interface/PositionJointInterface
-
-
- hardware_interface/PositionJointInterface
- reduction
-
-
-
- transmission_interface/SimpleTransmission
-
- hardware_interface/PositionJointInterface
-
-
- hardware_interface/PositionJointInterface
- reduction
-
-
-
- transmission_interface/SimpleTransmission
-
- hardware_interface/PositionJointInterface
-
-
- hardware_interface/PositionJointInterface
- reduction
-
-
-
- true
-
-
- true
-
-
- true
-
-
- true
-
-
- true
-
-
- true
-
-
- true
-
-
-
diff --git a/examples/pybullet/gym/pybullet_data/xarm/xarm6_robot.urdf b/examples/pybullet/gym/pybullet_data/xarm/xarm6_robot.urdf
index c09cb1a72..cff254689 100644
--- a/examples/pybullet/gym/pybullet_data/xarm/xarm6_robot.urdf
+++ b/examples/pybullet/gym/pybullet_data/xarm/xarm6_robot.urdf
@@ -27,7 +27,16 @@
-
+
+
+
+
+
+
+
+
+
+
@@ -41,11 +50,11 @@
-
+
-
+
@@ -61,11 +70,11 @@
-
+
-
+
@@ -89,11 +98,11 @@
-
+
-
+
@@ -117,11 +126,11 @@
-
+
-
+
@@ -145,11 +154,11 @@
-
+
-
+
@@ -173,11 +182,11 @@
-
+
-
+
@@ -205,7 +214,7 @@
-
+
diff --git a/examples/pybullet/gym/pybullet_data/xarm/xarm_description/meshes/xarm6/collision/link6.mtl b/examples/pybullet/gym/pybullet_data/xarm/xarm_description/meshes/xarm6/collision/link6.mtl
new file mode 100644
index 000000000..f231bdf4c
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/xarm/xarm_description/meshes/xarm6/collision/link6.mtl
@@ -0,0 +1,10 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 500
+Ka 0.8 0.8 0.8
+Kd 0.8 0.8 0.8
+Ks 0.8 0.8 0.8
+d 1
+illum 2
diff --git a/examples/pybullet/gym/pybullet_data/xarm/xarm_description/meshes/xarm6/collision/link6_vhacd.obj b/examples/pybullet/gym/pybullet_data/xarm/xarm_description/meshes/xarm6/collision/link6_vhacd.obj
new file mode 100644
index 000000000..6db67b8f1
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/xarm/xarm_description/meshes/xarm6/collision/link6_vhacd.obj
@@ -0,0 +1,189 @@
+o convex_0
+v 0.033187 0.018503 -0.027247
+v -0.037691 -0.004112 -0.027247
+v -0.036680 -0.006625 -0.028254
+v 0.013576 -0.034758 0.000399
+v -0.012050 0.035092 0.000399
+v 0.001516 -0.051863 -0.020710
+v -0.035174 -0.014657 -0.000608
+v 0.037202 0.000916 0.000399
+v -0.009541 0.036598 -0.027749
+v 0.032685 -0.018178 -0.028254
+v 0.016093 0.034593 -0.000608
+v -0.032658 0.019510 -0.000608
+v -0.005016 -0.051863 -0.008652
+v -0.024620 -0.028233 -0.028254
+v 0.028153 -0.025719 -0.000608
+v -0.028642 0.025037 -0.027247
+v 0.013074 0.035092 -0.028254
+v 0.006542 -0.051863 -0.011667
+v -0.029646 -0.022706 0.000399
+v 0.028662 0.025046 -0.000608
+v 0.013576 -0.034758 -0.028254
+v 0.037711 -0.000091 -0.027749
+v -0.037691 0.004927 -0.000608
+v -0.034673 0.013984 -0.028254
+v 0.005029 0.037614 -0.000608
+v 0.036700 -0.009629 -0.000608
+v -0.031654 -0.021191 -0.027247
+v -0.007024 -0.051855 -0.014175
+v -0.022103 0.031072 -0.000608
+v -0.013054 -0.034767 0.000399
+v 0.022123 0.031072 -0.027247
+v 0.036700 0.009963 -0.000608
+v 0.028153 -0.025719 -0.027247
+v 0.001014 -0.051863 -0.007143
+v -0.014058 -0.034767 -0.028254
+v 0.005029 0.037614 -0.027247
+v -0.034171 0.014982 0.000399
+v 0.021119 0.030572 0.000399
+v -0.016073 0.034593 -0.027247
+v -0.028133 -0.025719 -0.000608
+v -0.008028 0.037098 -0.000608
+v 0.034692 -0.015664 -0.027247
+v 0.035194 0.012477 -0.028254
+v -0.005016 -0.051863 -0.019201
+v -0.036680 0.009955 -0.027247
+v 0.006542 -0.051863 -0.017193
+v -0.037691 -0.004112 -0.000608
+v 0.031674 -0.021191 -0.000608
+v -0.035174 -0.014657 -0.027247
+v -0.023616 0.029066 -0.028254
+v 0.033187 0.018503 -0.000608
+v 0.023636 -0.028732 0.000399
+v 0.028662 0.025046 -0.027247
+v 0.036700 0.009963 -0.027247
+v -0.005016 0.037614 -0.027247
+v 0.037711 0.004936 -0.000608
+v 0.037711 -0.004112 -0.027247
+v -0.032658 0.019510 -0.027247
+v -0.035676 0.012976 -0.000608
+v 0.022123 0.031072 -0.000608
+v -0.016073 0.034593 -0.000608
+v 0.020617 -0.031246 -0.028254
+v -0.028642 0.025037 -0.000608
+v 0.016093 0.034593 -0.027247
+f 36 11 64
+f 5 4 8
+f 3 10 14
+f 10 3 17
+f 13 6 18
+f 4 5 19
+f 14 10 21
+f 17 3 24
+f 3 14 27
+f 27 14 28
+f 4 19 30
+f 15 18 33
+f 13 18 34
+f 4 30 34
+f 30 13 34
+f 21 6 35
+f 14 21 35
+f 17 9 36
+f 25 11 36
+f 19 5 37
+f 5 29 37
+f 5 8 38
+f 25 5 38
+f 11 25 38
+f 19 7 40
+f 7 27 40
+f 27 28 40
+f 28 13 40
+f 13 30 40
+f 30 19 40
+f 5 25 41
+f 9 39 41
+f 33 10 42
+f 10 17 43
+f 22 10 43
+f 6 13 44
+f 13 28 44
+f 28 14 44
+f 35 6 44
+f 14 35 44
+f 3 2 45
+f 2 23 45
+f 24 3 45
+f 18 6 46
+f 33 18 46
+f 2 7 47
+f 7 19 47
+f 23 2 47
+f 19 37 47
+f 37 23 47
+f 26 8 48
+f 15 33 48
+f 42 26 48
+f 33 42 48
+f 2 3 49
+f 7 2 49
+f 3 27 49
+f 27 7 49
+f 9 17 50
+f 24 16 50
+f 17 24 50
+f 16 29 50
+f 39 9 50
+f 29 39 50
+f 1 20 51
+f 32 1 51
+f 8 32 51
+f 38 8 51
+f 20 38 51
+f 8 4 52
+f 18 15 52
+f 4 34 52
+f 34 18 52
+f 48 8 52
+f 15 48 52
+f 20 1 53
+f 17 31 53
+f 31 20 53
+f 43 17 53
+f 1 43 53
+f 1 32 54
+f 22 43 54
+f 43 1 54
+f 36 9 55
+f 25 36 55
+f 41 25 55
+f 9 41 55
+f 8 26 56
+f 32 8 56
+f 54 32 56
+f 22 54 56
+f 10 22 57
+f 42 10 57
+f 26 42 57
+f 56 26 57
+f 22 56 57
+f 12 16 58
+f 16 24 58
+f 24 45 58
+f 58 45 59
+f 37 12 59
+f 23 37 59
+f 45 23 59
+f 12 58 59
+f 31 11 60
+f 20 31 60
+f 38 20 60
+f 11 38 60
+f 29 5 61
+f 39 29 61
+f 5 41 61
+f 41 39 61
+f 6 21 62
+f 21 10 62
+f 10 33 62
+f 46 6 62
+f 33 46 62
+f 16 12 63
+f 29 16 63
+f 12 37 63
+f 37 29 63
+f 31 17 64
+f 11 31 64
+f 17 36 64
diff --git a/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py b/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py
deleted file mode 100644
index bd248f1d8..000000000
--- a/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py
+++ /dev/null
@@ -1,29 +0,0 @@
-import functools
-import inspect
-import pybullet
-
-
-class BulletClient(object):
- """A wrapper for pybullet to manage different clients."""
-
- def __init__(self, connection_mode=pybullet.DIRECT, options=""):
- """Create a simulation and connect to it."""
- self._client = pybullet.connect(pybullet.SHARED_MEMORY)
- if (self._client < 0):
- print("options=", options)
- self._client = pybullet.connect(connection_mode, options=options)
- self._shapes = {}
-
- def __del__(self):
- """Clean up connection if not already done."""
- try:
- pybullet.disconnect(physicsClientId=self._client)
- except pybullet.error:
- pass
-
- def __getattr__(self, name):
- """Inject the client id into Bullet functions."""
- attribute = getattr(pybullet, name)
- if inspect.isbuiltin(attribute):
- attribute = functools.partial(attribute, physicsClientId=self._client)
- return attribute
diff --git a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py
index 88d605f11..eefb74693 100644
--- a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py
+++ b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py
@@ -17,7 +17,7 @@ from gym import spaces
from gym.utils import seeding
import numpy as np
import pybullet
-from . import bullet_client
+import pybullet_utils.bullet_client as bc
from . import minitaur
import pybullet_data
from . import minitaur_env_randomizer
@@ -150,9 +150,9 @@ class MinitaurBulletDuckEnv(gym.Env):
self._action_repeat *= NUM_SUBSTEPS
if self._is_render:
- self._pybullet_client = bullet_client.BulletClient(connection_mode=pybullet.GUI)
+ self._pybullet_client = bc.BulletClient(connection_mode=pybullet.GUI)
else:
- self._pybullet_client = bullet_client.BulletClient()
+ self._pybullet_client = bc.BulletClient()
self.seed()
self.reset()
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 7a595b47d..ee63044c1 100644
--- a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py
+++ b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py
@@ -14,7 +14,7 @@ from gym import spaces
from gym.utils import seeding
import numpy as np
import pybullet
-from . import bullet_client
+import pybullet_utils.bullet_client as bc
from . import minitaur
import os
import pybullet_data
@@ -145,9 +145,9 @@ class MinitaurBulletEnv(gym.Env):
self._action_repeat *= NUM_SUBSTEPS
if self._is_render:
- self._pybullet_client = bullet_client.BulletClient(connection_mode=pybullet.GUI)
+ self._pybullet_client = bc.BulletClient(connection_mode=pybullet.GUI)
else:
- self._pybullet_client = bullet_client.BulletClient()
+ self._pybullet_client = bc.BulletClient()
self.seed()
self.reset()
diff --git a/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py
index a0118d668..af9d5d545 100644
--- a/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py
+++ b/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py
@@ -12,7 +12,7 @@ import numpy as np
import pybullet
from . import racecar
import random
-from . import bullet_client
+import pybullet_utils.bullet_client as bc
import pybullet_data
from pkg_resources import parse_version
@@ -40,9 +40,9 @@ class RacecarGymEnv(gym.Env):
self._renders = renders
self._isDiscrete = isDiscrete
if self._renders:
- self._p = bullet_client.BulletClient(connection_mode=pybullet.GUI)
+ self._p = bc.BulletClient(connection_mode=pybullet.GUI)
else:
- self._p = bullet_client.BulletClient()
+ self._p = bc.BulletClient()
self.seed()
#self.reset()
diff --git a/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py
index 8137708ba..5e8641cef 100644
--- a/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py
+++ b/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py
@@ -10,7 +10,7 @@ from gym.utils import seeding
import numpy as np
import time
import pybullet
-from . import bullet_client
+import pybullet_utils.bullet_client as bc
from . import racecar
import random
import pybullet_data
@@ -42,9 +42,9 @@ class RacecarZEDGymEnv(gym.Env):
self._isDiscrete = isDiscrete
if self._renders:
- self._p = bullet_client.BulletClient(connection_mode=pybullet.GUI)
+ self._p = bc.BulletClient(connection_mode=pybullet.GUI)
else:
- self._p = bullet_client.BulletClient()
+ self._p = bc.BulletClient()
self.seed()
self.reset()
diff --git a/examples/pybullet/gym/pybullet_envs/examples/xarm.py b/examples/pybullet/gym/pybullet_envs/examples/xarm.py
index ff6aef14b..781d5afa4 100644
--- a/examples/pybullet/gym/pybullet_envs/examples/xarm.py
+++ b/examples/pybullet/gym/pybullet_envs/examples/xarm.py
@@ -7,7 +7,12 @@ p.setAdditionalSearchPath(pd.getDataPath())
useFixedBase = True
flags = p.URDF_INITIALIZE_SAT_FEATURES#0#p.URDF_USE_SELF_COLLISION
-xarm = p.loadURDF("xarm/xarm6_with_gripper.urdf", flags = flags, useFixedBase=useFixedBase)
+
+#plane_pos = [0,0,0]
+#plane = p.loadURDF("plane.urdf", plane_pos, flags = flags, useFixedBase=useFixedBase)
+table_pos = [0,0,-0.625]
+table = p.loadURDF("table/table.urdf", table_pos, flags = flags, useFixedBase=useFixedBase)
+xarm = p.loadURDF("xarm/xarm6_robot.urdf", flags = flags, useFixedBase=useFixedBase)
while (1):
p.stepSimulation()
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/bullet_client.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/bullet_client.py
deleted file mode 100644
index 54e0bd09e..000000000
--- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/bullet_client.py
+++ /dev/null
@@ -1,54 +0,0 @@
-"""A wrapper for pybullet to manage different clients."""
-from __future__ import absolute_import
-from __future__ import division
-import functools
-import inspect
-import pybullet
-
-
-class BulletClient(object):
- """A wrapper for pybullet to manage different clients."""
-
- def __init__(self, connection_mode=None):
- """Creates a Bullet client and connects to a simulation.
-
- Args:
- connection_mode:
- `None` connects to an existing simulation or, if fails, creates a
- new headless simulation,
- `pybullet.GUI` creates a new simulation with a GUI,
- `pybullet.DIRECT` creates a headless simulation,
- `pybullet.SHARED_MEMORY` connects to an existing simulation.
- """
- self._shapes = {}
-
- if connection_mode is None:
- self._client = pybullet.connect(pybullet.SHARED_MEMORY)
- if self._client >= 0:
- return
- else:
- connection_mode = pybullet.DIRECT
- self._client = pybullet.connect(connection_mode)
-
- def __del__(self):
- """Clean up connection if not already done."""
- try:
- pybullet.disconnect(physicsClientId=self._client)
- except pybullet.error:
- pass
-
- def __getattr__(self, name):
- """Inject the client id into Bullet functions."""
- attribute = getattr(pybullet, name)
- if inspect.isbuiltin(attribute):
- if name not in [
- "invertTransform",
- "multiplyTransforms",
- "getMatrixFromQuaternion",
- "getEulerFromQuaternion",
- "computeViewMatrixFromYawPitchRoll",
- "computeProjectionMatrixFOV",
- "getQuaternionFromEuler",
- ]: # A temporary hack for now.
- attribute = functools.partial(attribute, physicsClientId=self._client)
- return attribute
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py
index 44297d11b..a48d46a03 100644
--- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py
@@ -14,7 +14,7 @@ from gym import spaces
from gym.utils import seeding
import numpy as np
import pybullet
-from pybullet_envs.minitaur.envs import bullet_client
+import pybullet_utils.bullet_client as bc
import pybullet_data
from pybullet_envs.minitaur.envs import minitaur
from pybullet_envs.minitaur.envs import minitaur_derpy
@@ -223,9 +223,9 @@ class MinitaurGymEnv(gym.Env):
self._env_randomizers = convert_to_list(env_randomizer) if env_randomizer else []
self._episode_proto = minitaur_logging_pb2.MinitaurEpisode()
if self._is_render:
- self._pybullet_client = bullet_client.BulletClient(connection_mode=pybullet.GUI)
+ self._pybullet_client = bc.BulletClient(connection_mode=pybullet.GUI)
else:
- self._pybullet_client = bullet_client.BulletClient()
+ self._pybullet_client = bc.BulletClient()
if self._urdf_version is None:
self._urdf_version = DEFAULT_URDF_VERSION
self._pybullet_client.setPhysicsEngineParameter(enableConeFriction=0)
diff --git a/examples/pybullet/gym/pybullet_envs/prediction/pybullet_sim_gym_env.py b/examples/pybullet/gym/pybullet_envs/prediction/pybullet_sim_gym_env.py
index 1c308a7fb..e7ba23040 100644
--- a/examples/pybullet/gym/pybullet_envs/prediction/pybullet_sim_gym_env.py
+++ b/examples/pybullet/gym/pybullet_envs/prediction/pybullet_sim_gym_env.py
@@ -14,7 +14,7 @@ from gym import spaces
from gym.utils import seeding
import numpy as np
import pybullet
-from pybullet_envs.bullet import bullet_client
+import pybullet_utils.bullet_client as bc
from pybullet_envs.prediction import boxstack_pybullet_sim
@@ -71,10 +71,10 @@ class PyBulletSimGymEnv(gym.Env):
print("urdf_root=" + self._urdf_root)
if self._is_render:
- self._pybullet_client = bullet_client.BulletClient(connection_mode=pybullet.GUI,
+ self._pybullet_client = bc.BulletClient(connection_mode=pybullet.GUI,
options=optionstring)
else:
- self._pybullet_client = bullet_client.BulletClient()
+ self._pybullet_client = bc.BulletClient()
if (debug_visualization == False):
self._pybullet_client.configureDebugVisualizer(flag=self._pybullet_client.COV_ENABLE_GUI,
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index 53b119478..ffffb4289 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -2065,6 +2065,10 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
{
b3LoadSoftBodyAddNeoHookeanForce(command, NeoHookeanMu, NeoHookeanLambda, NeoHookeanDamping);
}
+ if (useSelfCollision)
+ {
+ b3LoadSoftBodySetSelfCollision(command, useSelfCollision);
+ }
b3LoadSoftBodySetFrictionCoefficient(command, frictionCoeff);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);