def __init__(self,
connection_mode=None,
realtime=False,
opengl_render=True):
self._in_realtime_mode = realtime
self.opengl_render = opengl_render
self._realtime_lock = threading.RLock()
if connection_mode is None:
self._client = p.connect(p.SHARED_MEMORY)
if self._client >= 0:
return
else:
connection_mode = p.DIRECT
self._client = p.connect(connection_mode)
is_linux = platform.system() == 'Linux'
if connection_mode == p.DIRECT and is_linux and opengl_render:
# # using the eglRendererPlugin (hardware OpenGL acceleration)
egl = pkgutil.get_loader('eglRenderer')
if egl:
p.loadPlugin(egl.get_filename(),
"_eglRendererPlugin",
physicsClientId=self._client)
else:
p.loadPlugin("eglRendererPlugin", physicsClientId=self._client)
self._gui_mode = connection_mode == p.GUI
p.setGravity(0, 0, GRAVITY_CONST, physicsClientId=self._client)
self.set_step_sim(not self._in_realtime_mode)
def __init__(self, height, width, window_name, fov=60):
camPos = [0, 0, 0]
camUpVector = [0, -1, 0]
camTargetPos = [0, 0, 1]
nearPlane = 0.01
farPlane = 100
self.window_name = window_name
self.height = height
self.width = width
############## VTK stuff ##############
# Create a rendering window and renderer
self.ren = vtk.vtkRenderer()
self.renWin = vtk.vtkRenderWindow()
self.renWin.SetOffScreenRendering(1)
self.renWin.SetAlphaBitPlanes(1) # Enable usage of alpha channel
self.renWin.AddRenderer(self.ren)
self.renWin.SetSize(width, height)
self.camera = self.ren.GetActiveCamera()
self.camera.SetPosition(camPos)
self.camera.SetViewUp(camUpVector)
self.camera.SetFocalPoint(camTargetPos)
self.camera.SetViewAngle(fov)
self.w2if = vtk.vtkWindowToImageFilter()
self.w2if.SetInput(self.renWin)
self.w2if.SetInputBufferTypeToRGBA()
############## pybullet stuff ##############
p.connect(p.DIRECT)
# conid = p.connect(p.SHARED_MEMORY)
# if (conid<0):
# p.connect(p.GUI)
p.resetSimulation()
p.setGravity(0, 0, -10)
# p.setPhysicsEngineParameter(numSolverIterations=5)
# p.setPhysicsEngineParameter(fixedTimeStep=1./240.)
# p.setPhysicsEngineParameter(numSubSteps=10)
self.viewMatrix = p.computeViewMatrix(camPos, camTargetPos,
camUpVector)
aspect = width / height
self.projectionMatrix = p.computeProjectionMatrixFOV(
fov, aspect, nearPlane, farPlane)
egl = pkgutil.get_loader('eglRenderer')
if (egl):
p.loadPlugin(egl.get_filename(), "_eglRendererPlugin")
else:
p.loadPlugin("eglRendererPlugin")
self.play = True
self.physics_thread = Thread(target=self.update_physics, args=())
self.physics_thread.daemon = True
print("READY")
def pybullet_test():
print(f"Running {__file__}::{pybullet_test.__name__}()")
import pybullet as pb
import pkgutil
egl = pkgutil.get_loader('eglRenderer')
pb.connect(
pb.DIRECT
) # EGL is not supported in GUI mode, obviously, since it's about headless support
pb.loadPlugin(egl.get_filename(), '_eglRendererPlugin')
print("EGL ON")
print("Passed.")
def _init_physic_clients(self):
self._num_physic_clients = 0
if self._render_video:
pybullet_options = "--width={} --height={}".format(
self._video_width, self._video_height)
else:
pybullet_options = ""
if self._use_gui and not self._switch_gui:
self._simulation_client_id = p.connect(p.GUI,
options=pybullet_options)
self._gui_client_id = self._simulation_client_id
self._num_physic_clients += 1
else:
if not self._use_real_robot:
self._simulation_client_id = p.connect(
p.DIRECT, options=pybullet_options)
self._num_physic_clients += 1
else:
self._simulation_client_id = None
self._egl_plugin = None
if self._simulation_client_id is not None:
if self._renderer == OPENGL_GUI_RENDERER and self._render_video and not self._use_gui:
raise ValueError(
"renderer==OPENGL_GUI_RENDERER requires use_gui==True")
if self._renderer == OPENGL_GUI_RENDERER or self._renderer == OPENGL_EGL_RENDERER:
self._pybullet_renderer = p.ER_BULLET_HARDWARE_OPENGL
if self._renderer == OPENGL_EGL_RENDERER and self._render_video:
import pkgutil
egl_renderer = pkgutil.get_loader('eglRenderer')
logging.warning(
"The usage of the egl renderer might lead to a segmentation fault on systems without "
"a GPU.")
if egl_renderer:
self._egl_plugin = p.loadPlugin(
egl_renderer.get_filename(), "_eglRendererPlugin")
else:
self._egl_plugin = p.loadPlugin("eglRendererPlugin")
else:
self._pybullet_renderer = p.ER_TINY_RENDERER
else:
self._pybullet_renderer = None
if self._use_gui and self._switch_gui:
self._obstacle_client_id = p.connect(p.GUI,
options=pybullet_options)
self._gui_client_id = self._obstacle_client_id
else:
self._obstacle_client_id = p.connect(p.DIRECT)
self._num_physic_clients += 1
def __init__(self, render_mode):
# self.ros_root = rospkg.get_ros_root()
# self.rospkg = rospkg.RosPack()
# self.pkg_path = self.rospkg.get_path('worldmodel_bullet')
self.pkg_path = os.path.join(Path(__file__).resolve().parent, '../../')
self.tracks_path = os.path.join(self.pkg_path, 'tracks/')
self.render_mode = render_mode
if render_mode == 'human':
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
egl = pkgutil.get_loader('eglRenderer')
self.plugin = p.loadPlugin(egl.get_filename(),
"_eglRendererPlugin")
print("plugin=", self.plugin)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
# p.setPhysicsEngineParameter(enableFileCaching=0)
p.resetSimulation()
p.setGravity(0, 0, -9.81)
p.setRealTimeSimulation(0)
self.bodies = dict()
def connect(self):
options = ''
if pb.GUI == self._conn_mode:
options = '--width={} --height={}'.format(*self._gui_resolution)
self.client_id = pb.connect(self._conn_mode, options=options)
if self._load_egl:
print('Loading egl plugin...')
import pkgutil
egl = pkgutil.get_loader('eglRenderer')
self._plugin = pb.loadPlugin(egl.get_filename(),
"_eglRendererPlugin",
physicsClientId=self.client_id)
if self.client_id < 0:
raise Exception('Cannot connect to pybullet')
if self._conn_mode == pb.GUI:
pb.configureDebugVisualizer(pb.COV_ENABLE_GUI, 0)
pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1)
pb.configureDebugVisualizer(pb.COV_ENABLE_TINY_RENDERER, 0)
pb.resetSimulation(physicsClientId=self.client_id)
pb.setPhysicsEngineParameter(numSolverIterations=50,
fixedTimeStep=self.simulation_step,
physicsClientId=self.client_id)
pb.setGravity(0, 0, -9.8, physicsClientId=self.client_id)
pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING,
0,
physicsClientId=self.client_id)
self._connected = True
def connect(self):
if self._useGUI:
pb.connect(pb.GUI_SERVER)
if self._instance_key is not None:
raise Exception(
"Invalid simulator configuration. instance_key cannot be specified together with use_gui."
)
pb.resetDebugVisualizerCamera(1.5,
-30,
-15,
cameraTargetPosition=[-0.4, 0, 0.3])
else:
if self._instance_key is None:
pb.connect(pb.SHARED_MEMORY_SERVER)
else:
pb.connect(pb.SHARED_MEMORY_SERVER, key=self._instance_key)
if self._useGPU and sys.platform == 'linux':
# EGL is an OpenGL replacement in headless mode. It can only be used when _useGUI=False
import pkgutil
egl = pkgutil.get_loader('eglRenderer')
self.__egl_plugin = pb.loadPlugin(egl.get_filename(),
'_eglRendererPlugin')
pb.setAdditionalSearchPath(pybullet_data.getDataPath())
pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0)
self.__load_robot()
if self._useGUI:
pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1)
def connect(self):
if self._renders:
self.cid = p.connect(p.SHARED_MEMORY)
if self.cid < 0:
self.cid = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3, 180.0, -41.0,
[-0.35, -0.58, -0.88])
if not self._gui_debug:
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
else:
self.cid = p.connect(p.DIRECT)
egl = pkgutil.get_loader("eglRenderer")
if self._egl_render and egl:
p.loadPlugin(egl.get_filename(), "_eglRendererPlugin")
self.connected = True
def connect(self, gpu_renderer=True, gui=False):
assert not self._connected, 'Already connected'
if gui:
self._client_id = pb.connect(pb.GUI, '--width=640 --height=480')
pb.configureDebugVisualizer(pb.COV_ENABLE_GUI,
1,
physicsClientId=self._client_id)
pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING,
1,
physicsClientId=self._client_id)
pb.configureDebugVisualizer(pb.COV_ENABLE_TINY_RENDERER,
0,
physicsClientId=self._client_id)
else:
self._client_id = pb.connect(pb.DIRECT)
if self._client_id < 0:
raise Exception('Cannot connect to pybullet')
if gpu_renderer and not gui:
os.environ['MESA_GL_VERSION_OVERRIDE'] = '3.3'
os.environ['MESA_GLSL_VERSION_OVERRIDE'] = '330'
# Get EGL device
#assert 'CUDA_VISIBLE_DEVICES' in os.environ
devices = [0] #os.environ.get('CUDA_VISIBLE_DEVICES', ).split(',')
assert len(devices) == 1
out = subprocess.check_output([
'nvidia-smi', '--id=' + str(devices[0]), '-q', '--xml-format'
])
tree = ET.fromstring(out)
gpu = tree.findall('gpu')[0]
dev_id = gpu.find('minor_number').text
#os.environ['EGL_VISIBLE_DEVICES'] = str(dev_id)
egl = pkgutil.get_loader('eglRenderer')
pb.loadPlugin(egl.get_filename(),
"_eglRendererPlugin",
physicsClientId=self._client_id)
pb.resetSimulation(physicsClientId=self._client_id)
self._connected = True
self._client = BulletClient(self._client_id)
self.client.setPhysicsEngineParameter(numSolverIterations=50)
self.client.setPhysicsEngineParameter(
fixedTimeStep=self._simulation_step)
self.client.setGravity(0, 0, -9.8)
def enable_gpu(self):
import GPUtil as GPU
os.environ['MESA_GL_VERSION_OVERRIDE'] = '3.3'
os.environ['MESA_GLSL_VERSION_OVERRIDE'] = '330'
enableGPU = False
# Get all device ids and their processing and memory utiliazion
# (deviceIds, gpuUtil, memUtil) = GPU.getGPUs()
# Print os and python version information
print('OS: ' + sys.platform)
print(sys.version)
# Print package name and version number
print(GPU.__name__ + ' ' + GPU.__version__)
# Show the utilization of all GPUs in a nice table
GPU.showUtilization()
# Show all stats of all GPUs in a nice table
GPU.showUtilization(all=True)
# NOTE: If all your GPUs currently have a memory consumption larger than 1%, this step will fail. It's not a bug! It is intended to do so, if it does not find an available GPU.
GPUs = GPU.getGPUs()
numGPUs = len(GPU.getGPUs())
print("numGPUs=", numGPUs)
if numGPUs > 0:
enableGPU = True
eglPluginId = -1
if enableGPU:
import pkgutil
egl = pkgutil.get_loader('eglRenderer')
if (egl):
eglPluginId = p.loadPlugin(egl.get_filename(),
"_eglRendererPlugin",
physicsClientId=self.id)
else:
eglPluginId = p.loadPlugin("eglRendererPlugin",
physicsClientId=self.id)
if eglPluginId >= 0:
print("Using GPU hardware (eglRenderer)")
else:
print("Using CPU renderer (TinyRenderer)")
def __init__(self, max_step=1000, GUI=False):
# pybullet setting
self._timeStep = 1. / 240.
self._p = p
self._GUI = GUI
if GUI:
p.connect(p.GUI)
# p.configureDebugVisualizer(p.COV_ENABLE_GUI, False)
# p.resetDebugVisualizerCamera(1.3, 180, -41, [0.52, -0.2, -0.33])
else:
p.connect(p.DIRECT)
egl = pkgutil.get_loader('eglRenderer')
if egl:
plugin = p.loadPlugin(egl.get_filename(), "_eglRendererPlugin")
self.renderer = Camera()
pose = np.eye(4)
pose[:3, :3] = R.from_euler('xyz', [210, 0, 30],
degrees=True).as_matrix()
pose[:3, 3] = np.array([0.5, -0.2, 1])
self.renderer.set_pose(pose)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# env setting
self.cam_width = 640
self.cam_height = 480
# pixel and normal in camera frame
self.action_space = spaces.Box(
low=np.array([0, 0]),
high=np.array([self.cam_height - 1, self.cam_width - 1]),
dtype=np.uint8,
)
self.color_space = spaces.Box(
low=0,
high=255,
shape=(self.cam_height, self.cam_width, 3),
dtype=np.uint8,
)
self.depth_space = spaces.Box(
low=0,
high=2,
shape=(self.cam_height, self.cam_width),
dtype=np.float32,
)
self._envStepCounter = 0
self._max_step = max_step
self._observation = []
self.distance_threshold = 0.01
self.terminated = 0
self.seed()
def __init__(self,
gui=False,
use_egl=True,
timeout=2,
timestep=1e-3,
debug=False,
epochs=10000,
stop_th=1e-4,
g=-10,
bin_pos=[1.5, 1.5, 0.01],
visual_model=False,
pre_grasp_distance=0.2):
self.gui = gui
self.debug = debug
self.epochs = epochs
self.epoch = 0
self.stop_th = stop_th
self.timestep = timestep
self.timeout = timeout
self.bin_pos = bin_pos
self.visual_model = visual_model
if gui:
self.client = p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
else:
self.client = p.connect(p.DIRECT)
if use_egl:
plugin = p.loadPlugin(egl.get_filename(), "_eglRendererPlugin")
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.setTimeStep(self.timestep)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
#p.setPhysicsEngineParameter(erp=0.4, contactERP=0.4, frictionERP=0.4)
p.setGravity(0, 0, g)
self.planeId = p.loadURDF("plane.urdf")
self.gid = None
self.bin = None
self.old_poses = {}
self.obj_ids = {}
self.cam = Camera(debug=debug)
self.logger = None
self.pre_grasp_distance = pre_grasp_distance
self.debug_z_id = None
self.debug_x_id = None
def __init__(
self,
renderer='tiny', # ('tiny', 'egl', 'debug')
):
self.action_space = spaces.Discrete(2)
self.iter = cycle(range(0, 360, 10))
# how we want to show
assert renderer in ('tiny', 'egl', 'debug', 'plugin')
self._renderer = renderer
self._render_width = 84
self._render_height = 84
# connecting
if self._renderer == "tiny" or self._renderer == "plugin":
optionstring = '--width={} --height={}'.format(
self._render_width, self._render_height)
p.connect(p.DIRECT, options=optionstring)
if self._renderer == "plugin":
plugin_fn = os.path.join(
p.__file__.split("bullet3")[0],
"bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so"
)
plugin = p.loadPlugin(plugin_fn, "_tinyRendererPlugin")
if plugin < 0:
print(
"\nPlugin Failed to load! Try installing via `pip install -e .`\n"
)
sys.exit()
print("plugin =", plugin)
elif self._renderer == "egl":
optionstring = '--width={} --height={}'.format(
self._render_width, self._render_height)
optionstring += ' --window_backend=2 --render_device=0'
p.connect(p.GUI, options=optionstring)
elif self._renderer == "debug":
#print("Connection: SHARED_MEMORY")
#cid = p.connect(p.SHARED_MEMORY)
#if (cid<0):
cid = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3, 180, -41, [0.52, -0.2, -0.33])
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
def __init__(
self,
renderer='tiny', # ('tiny', 'egl', 'debug')
):
self.action_space = spaces.Discrete(2)
self.iter = cycle(range(0, 360, 10))
# how we want to show
assert renderer in ('tiny', 'egl', 'debug', 'plugin')
self._renderer = renderer
self._render_width = 84
self._render_height = 84
# connecting
if self._renderer == "tiny" or self._renderer == "plugin":
optionstring = '--width={} --height={}'.format(self._render_width, self._render_height)
p.connect(p.DIRECT, options=optionstring)
if self._renderer == "plugin":
plugin_fn = os.path.join(
p.__file__.split("bullet3")[0],
"bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so")
plugin = p.loadPlugin(plugin_fn, "_tinyRendererPlugin")
if plugin < 0:
print("\nPlugin Failed to load! Try installing via `pip install -e .`\n")
sys.exit()
print("plugin =", plugin)
elif self._renderer == "egl":
optionstring = '--width={} --height={}'.format(self._render_width, self._render_height)
optionstring += ' --window_backend=2 --render_device=0'
p.connect(p.GUI, options=optionstring)
elif self._renderer == "debug":
#print("Connection: SHARED_MEMORY")
#cid = p.connect(p.SHARED_MEMORY)
#if (cid<0):
cid = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3, 180, -41, [0.52, -0.2, -0.33])
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
def __init__(self, client=0, renderer: BaseRenderer = None):
"""Load plugin
Arguments:
physicsClientId {int or BulletClient} -- physics client
Keyword Arguments:
renderer {BaseRenderer} -- renderer to load (default: None)
"""
self._client_id = client if isinstance(client, int) else client._client
# load plugin
self._plugin_id = pb.loadPlugin(plugin_lib_file,
'_RenderingPlugin',
physicsClientId=self._client_id)
assert self._plugin_id != -1, 'Cannot load PyBullet plugin'
# workaround to connect python bindings with a physicsClientId
retcode = pb.executePluginCommand(self._plugin_id,
"register",
intArgs=[self._client_id],
physicsClientId=self._client_id)
assert retcode != -1, 'Cannot register render interface'
# bind a renderer
if renderer is not None:
self.set_renderer(renderer)
p.addUserDebugText("stablePD", [shift, 0, .1],
textColor,
parentObjectUniqueId=pole4,
parentLinkIndex=1)
p.addUserDebugText("position constraint", [shift, 0, -.1],
textColor,
parentObjectUniqueId=pole3,
parentLinkIndex=1)
desiredPosPoleId = p.addUserDebugParameter("desiredPosPole", -10, 10, 0)
desiredVelPoleId = p.addUserDebugParameter("desiredVelPole", -10, 10, 0)
kpPoleId = p.addUserDebugParameter("kpPole", 0, 500, 1200)
kdPoleId = p.addUserDebugParameter("kdPole", 0, 300, 100)
maxForcePoleId = p.addUserDebugParameter("maxForcePole", 0, 5000, 1000)
pd = p.loadPlugin("pdControlPlugin")
p.setGravity(0, 0, -10)
useRealTimeSim = False
p.setRealTimeSimulation(useRealTimeSim)
timeStep = 0.001
while p.isConnected():
#p.getCameraImage(320,200)
timeStep = p.readUserDebugParameter(timeStepId)
p.setTimeStep(timeStep)
desiredPosCart = p.readUserDebugParameter(desiredPosCartId)
def __init__(self,
assets_root,
task=None,
disp=False,
shared_memory=False,
hz=240):
"""Creates OpenAI Gym-style environment with PyBullet.
Args:
assets_root: root directory of assets.
task: the task to use. If None, the user must call set_task for the
environment to work properly.
disp: show environment with PyBullet's built-in display viewer.
shared_memory: run with shared memory.
hz: PyBullet physics simulation step speed. Set to 480 for deformables.
Raises:
RuntimeError: if pybullet cannot load fileIOPlugin.
"""
self.pix_size = 0.003125
self.obj_ids = {'fixed': [], 'rigid': [], 'deformable': []}
self.homej = np.array([-1, -0.5, 0.5, -0.5, -0.5, 0]) * np.pi
self.agent_cams = cameras.RealSenseD415.CONFIG
self.assets_root = assets_root
color_tuple = [
gym.spaces.Box(0,
255,
config['image_size'] + (3, ),
dtype=np.uint8) for config in self.agent_cams
]
depth_tuple = [
gym.spaces.Box(0.0, 20.0, config['image_size'], dtype=np.float32)
for config in self.agent_cams
]
self.observation_space = gym.spaces.Dict({
'color':
gym.spaces.Tuple(color_tuple),
'depth':
gym.spaces.Tuple(depth_tuple),
})
# TODO(ayzaan): Delete below and uncomment vector box bounds.
position_bounds = gym.spaces.Box(low=-1.0,
high=1.0,
shape=(3, ),
dtype=np.float32)
# position_bounds = gym.spaces.Box(
# low=np.array([0.25, -0.5, 0.], dtype=np.float32),
# high=np.array([0.75, 0.5, 0.28], dtype=np.float32),
# shape=(3,),
# dtype=np.float32)
self.action_space = gym.spaces.Dict({
'pose0':
gym.spaces.Tuple((position_bounds,
gym.spaces.Box(-1.0,
1.0,
shape=(4, ),
dtype=np.float32))),
'pose1':
gym.spaces.Tuple((position_bounds,
gym.spaces.Box(-1.0,
1.0,
shape=(4, ),
dtype=np.float32)))
})
# Start PyBullet.
disp_option = p.DIRECT
if disp:
disp_option = p.GUI
if shared_memory:
disp_option = p.SHARED_MEMORY
client = p.connect(disp_option)
file_io = p.loadPlugin('fileIOPlugin', physicsClientId=client)
if file_io < 0:
raise RuntimeError('pybullet: cannot load FileIO!')
if file_io >= 0:
p.executePluginCommand(file_io,
textArgument=assets_root,
intArgs=[p.AddFileIOAction, p.CNSFileIO],
physicsClientId=client)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.setPhysicsEngineParameter(enableFileCaching=0)
p.setAdditionalSearchPath(assets_root)
p.setAdditionalSearchPath(tempfile.gettempdir())
p.setTimeStep(1. / hz)
# If using --disp, move default camera closer to the scene.
if disp:
target = p.getDebugVisualizerCamera()[11]
p.resetDebugVisualizerCamera(cameraDistance=1.1,
cameraYaw=90,
cameraPitch=-25,
cameraTargetPosition=target)
if task:
self.set_task(task)
import pybullet as p
p.connect(p.GUI)
plugin = p.loadPlugin("D:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll",
"_testPlugin")
print("plugin=", plugin)
p.executePluginCommand(plugin, "r2d2.urdf", [1, 2, 3], [50.0, 3.3])
while (1):
p.getCameraImage(320, 200)
p.stepSimulation()
import numpy as np
import matplotlib.pyplot as plt
import pybullet
import time
plt.ion()
img = np.random.rand(200, 320)
#img = [tandard_normal((50,100))
image = plt.imshow(img,interpolation='none',animated=True,label="blah")
ax = plt.gca()
pybullet.connect(pybullet.DIRECT)
pybullet.loadPlugin("eglRendererPlugin")
pybullet.loadURDF("plane.urdf",[0,0,-1])
pybullet.loadURDF("r2d2.urdf")
camTargetPos = [0,0,0]
cameraUp = [0,0,1]
cameraPos = [1,1,1]
pybullet.setGravity(0,0,-10)
pitch = -10.0
roll=0
upAxisIndex = 2
camDistance = 4
pixelWidth = 320
pixelHeight = 200
import pybullet as p
import time
useDirect = False
usePort = True
p.connect(p.GUI)
id = p.loadPlugin("grpcPlugin")
#dynamically loading the plugin
#id = p.loadPlugin("E:/develop/bullet3/bin/pybullet_grpcPlugin_vs2010_x64_debug.dll", postFix="_grpcPlugin")
#start the GRPC server at hostname, port
if (id < 0):
print("Cannot load grpcPlugin")
exit(0)
if usePort:
p.executePluginCommand(id, "localhost:12345")
else:
p.executePluginCommand(id, "localhost")
while p.isConnected():
if (useDirect):
#Only in DIRECT mode, since there is no 'ping' you need to manually call to handle RCPs:
numRPC = 10
p.executePluginCommand(id, intArgs=[numRPC])
else:
dt = 1. / 240.
time.sleep(dt)
p.addUserDebugText("stablePD", [shift, 0, .1],
textColor,
parentObjectUniqueId=pole4,
parentLinkIndex=1)
p.addUserDebugText("position constraint", [shift, 0, -.1],
textColor,
parentObjectUniqueId=pole3,
parentLinkIndex=1)
desiredPosPoleId = p.addUserDebugParameter("desiredPosPole", -10, 10, 0)
desiredVelPoleId = p.addUserDebugParameter("desiredVelPole", -10, 10, 0)
kpPoleId = p.addUserDebugParameter("kpPole", 0, 500, 1200)
kdPoleId = p.addUserDebugParameter("kdPole", 0, 300, 100)
maxForcePoleId = p.addUserDebugParameter("maxForcePole", 0, 5000, 1000)
pd = p.loadPlugin("pdControlPlugin")
p.setGravity(0, 0, -10)
useRealTimeSim = False
p.setRealTimeSimulation(useRealTimeSim)
timeStep = 0.001
while p.isConnected():
p.getCameraImage(320, 200)
timeStep = p.readUserDebugParameter(timeStepId)
p.setTimeStep(timeStep)
desiredPosCart = p.readUserDebugParameter(desiredPosCartId)
import pybullet as p
import pybullet_data as pd
import time
import math
usePhysX = True
useMaximalCoordinates = True
if usePhysX:
p.connect(p.PhysX,options="--numCores=8 --solver=pgs")
p.loadPlugin("eglRendererPlugin")
else:
p.connect(p.GUI)
p.setPhysicsEngineParameter(fixedTimeStep=1./240.,numSolverIterations=4, minimumSolverIslandSize=1024)
p.setPhysicsEngineParameter(contactBreakingThreshold=0.01)
p.setAdditionalSearchPath(pd.getDataPath())
#Always make ground plane maximal coordinates, to avoid performance drop in PhysX
#See https://github.com/NVIDIAGameWorks/PhysX/issues/71
p.loadURDF("plane.urdf", useMaximalCoordinates=True)#useMaximalCoordinates)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"physx_create_dominoes.json")
jran = 50
iran = 100
num=64
radius=0.1
numDominoes=0
plt.ion()
img = [[1, 2, 3] * 50] * 100 #np.random.rand(200, 320)
#img = [tandard_normal((50,100))
image = plt.imshow(img, interpolation='none', animated=True, label="blah")
ax = plt.gca()
import pybullet_data
pybullet.connect(pybullet.DIRECT)
if 1:
import pkgutil
egl = pkgutil.get_loader('eglRenderer')
if (egl):
pluginId = pybullet.loadPlugin(egl.get_filename(),
"_eglRendererPlugin")
else:
pluginId = pybullet.loadPlugin("eglRendererPlugin")
print("pluginId=", pluginId)
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
#pybullet.loadPlugin("eglRendererPlugin")
usePlane = False
updateHeightfield = True
textureId = -1
if usePlane:
pybullet.loadURDF("plane.urdf", [0, 0, -1])
else:
useProgrammatic = 0
import pybullet as p
p.connect(p.GUI)
plugin = p.loadPlugin("e:/develop/bullet3/bin/pybullet_tinyRendererPlugin_vs2010_x64_debug.dll","_tinyRendererPlugin")
print("plugin=",plugin)
p.loadURDF("r2d2.urdf")
while (1):
p.getCameraImage(320,200)
import pybullet as p
import time
p.connect(p.GUI)
fileIO = p.loadPlugin("fileIOPlugin")
if (fileIO>=0):
p.executePluginCommand(fileIO, "pickup.zip", [p.AddFileIOAction, p.ZipFileIO])
objs= p.loadSDF("pickup/model.sdf")
dobot =objs[0]
p.changeVisualShape(dobot,-1,rgbaColor=[1,1,1,1])
else:
print("fileIOPlugin is disabled.")
p.setPhysicsEngineParameter(enableFileCaching=False)
while (1):
p.stepSimulation()
time.sleep(1./240.)
# Global simulatior setup
CONTROL_DT = 1. / 240.
p.connect(p.GUI)
p.setTimeStep(CONTROL_DT)
p.setGravity(0, 0, -9.8)
p.resetDebugVisualizerCamera(cameraDistance=0.25,
cameraYaw=240,
cameraPitch=-40,
cameraTargetPosition=[-0.25, 0.20, 0.8])
# GPU acceleration (linux only) Reference: https://colab.research.google.com/drive/1u6j7JOqM05vUUjpVp5VNk0pd8q-vqGlx#scrollTo=fJXFN4U7NIRC
import pkgutil
egl = pkgutil.get_loader('eglRenderer')
if (egl):
eglPluginId = p.loadPlugin(egl.get_filename(), "_eglRendererPlugin")
# Init environments and agents
binpick_env = BinPickEnv(customURDFPath)
panda = FrankaPanda()
# Simulation loop
step_count = 0
while True:
# For smooth rendering
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)
# Render depth camera
binpick_env.render()
if step_count >= 50:
def __init__(self,
debug=False,
urdf_root=pybullet_data.getDataPath(),
urdf_version=None,
distance_weight=1.0,
energy_weight=0.0005,
shake_weight=0.005,
drift_weight=2.0,
distance_limit=float("inf"),
observation_noise_stdev=SENSOR_NOISE_STDDEV,
self_collision_enabled=True,
motor_velocity_limit=np.inf,
pd_control_enabled=False,
leg_model_enabled=True,
accurate_motor_model_enabled=False,
remove_default_joint_damping=False,
motor_kp=2.0,
motor_kd=0.03,
control_latency=0.0,
pd_latency=0.0,
torque_control_enabled=False,
motor_overheat_protection=False,
hard_reset=True,
on_rack=False,
render=True,
num_steps_to_log=1000,
action_repeat=1,
control_time_step=None,
env_randomizer=None,
forward_reward_cap=float("inf"),
reflection=True,
log_path=None,
target_orient=None,
init_orient=None,
target_position=None,
start_position=None,
base_y=0.0,
base_z=0.0,
base_roll=0.0,
base_pitch=0.0,
base_yaw=0.0,
step_length=None,
step_rotation=None,
step_angle=None,
step_period=None,
backwards=None,
signal_type="ik",
terrain_type="plane",
terrain_id=None,
mark='base'):
""" Initialize the rex gym environment.
Args:
urdf_root: The path to the urdf data folder.
urdf_version: [DEFAULT_URDF_VERSION] are allowable
versions. If None, DEFAULT_URDF_VERSION is used.
distance_weight: The weight of the distance term in the reward.
energy_weight: The weight of the energy term in the reward.
shake_weight: The weight of the vertical shakiness term in the reward.
drift_weight: The weight of the sideways drift term in the reward.
distance_limit: The maximum distance to terminate the episode.
observation_noise_stdev: The standard deviation of observation noise.
self_collision_enabled: Whether to enable self collision in the sim.
motor_velocity_limit: The velocity limit of each motor.
pd_control_enabled: Whether to use PD controller for each motor.
leg_model_enabled: Whether to use a leg motor to reparameterize the action
space.
accurate_motor_model_enabled: Whether to use the accurate DC motor model.
remove_default_joint_damping: Whether to remove the default joint damping.
motor_kp: proportional gain for the accurate motor model.
motor_kd: derivative gain for the accurate motor model.
control_latency: It is the delay in the controller between when an
observation is made at some point, and when that reading is reported
back to the Neural Network.
pd_latency: latency of the PD controller loop. PD calculates PWM based on
the motor angle and velocity. The latency measures the time between when
the motor angle and velocity are observed on the microcontroller and
when the true state happens on the motor. It is typically (0.001-
0.002s).
torque_control_enabled: Whether to use the torque control, if set to
False, pose control will be used.
motor_overheat_protection: Whether to shutdown the motor that has exerted
large torque (OVERHEAT_SHUTDOWN_TORQUE) for an extended amount of time
(OVERHEAT_SHUTDOWN_TIME). See ApplyAction() in rex.py for more
details.
hard_reset: Whether to wipe the simulation and load everything when reset
is called. If set to false, reset just place Rex back to start
position and set its pose to initial configuration.
on_rack: Whether to place Rex on rack. This is only used to debug
the walk gait. In this mode, Rex's base is hanged midair so
that its walk gait is clearer to visualize.
render: Whether to render the simulation.
num_steps_to_log: The max number of control steps in one episode that will
be logged. If the number of steps is more than num_steps_to_log, the
environment will still be running, but only first num_steps_to_log will
be recorded in logging.
action_repeat: The number of simulation steps before actions are applied.
control_time_step: The time step between two successive control signals.
env_randomizer: An instance (or a list) of EnvRandomizer(s). An
EnvRandomizer may randomize the physical property of rex, change
the terrrain during reset(), or add perturbation forces during step().
forward_reward_cap: The maximum value that forward reward is capped at.
Disabled (Inf) by default.
log_path: The path to write out logs. For the details of logging, refer to
rex_logging.proto.
Raises:
ValueError: If the urdf_version is not supported.
"""
self.mark = mark
self.num_motors = mark_constants.MARK_DETAILS['motors_num'][self.mark]
self.motor_velocity_obs_index = MOTOR_ANGLE_OBSERVATION_INDEX + self.num_motors
self.motor_torque_obs_index = self.motor_velocity_obs_index + self.num_motors
self.base_orientation_obs_index = self.motor_torque_obs_index + self.num_motors
# Set up logging.
self._log_path = log_path
# @TODO fix logging
self.logging = None
# PD control needs smaller time step for stability.
if control_time_step is not None:
self.control_time_step = control_time_step
self._action_repeat = action_repeat
self._time_step = control_time_step / action_repeat
else:
# Default values for time step and action repeat
if accurate_motor_model_enabled or pd_control_enabled:
self._time_step = 0.002
self._action_repeat = 5
else:
self._time_step = 0.01
self._action_repeat = 1
self.control_time_step = self._time_step * self._action_repeat
# TODO: Fix the value of self._num_bullet_solver_iterations.
self._num_bullet_solver_iterations = int(
NUM_SIMULATION_ITERATION_STEPS / self._action_repeat)
self._urdf_root = urdf_root
self._self_collision_enabled = self_collision_enabled
self._motor_velocity_limit = motor_velocity_limit
self._observation = []
self._true_observation = []
self._objectives = []
self._objective_weights = [
distance_weight, energy_weight, drift_weight, shake_weight
]
self._env_step_counter = 0
self._num_steps_to_log = num_steps_to_log
self._is_render = render
self._is_debug = debug
self._last_base_position = [0, 0, 0]
self._last_base_orientation = [0, 0, 0, 1]
self._distance_weight = distance_weight
self._energy_weight = energy_weight
self._drift_weight = drift_weight
self._shake_weight = shake_weight
self._distance_limit = distance_limit
self._observation_noise_stdev = observation_noise_stdev
self._action_bound = 1
self._pd_control_enabled = pd_control_enabled
self._leg_model_enabled = leg_model_enabled
self._accurate_motor_model_enabled = accurate_motor_model_enabled
self._remove_default_joint_damping = remove_default_joint_damping
self._motor_kp = motor_kp
self._motor_kd = motor_kd
self._torque_control_enabled = torque_control_enabled
self._motor_overheat_protection = motor_overheat_protection
self._on_rack = on_rack
self._cam_dist = 1.0
self._cam_yaw = 0
self._cam_pitch = -30
self._forward_reward_cap = forward_reward_cap
self._hard_reset = True
self._last_frame_time = 0.0
self._control_latency = control_latency
self._pd_latency = pd_latency
self._urdf_version = urdf_version
self._ground_id = None
self._reflection = reflection
self._env_randomizers = convert_to_list(
env_randomizer) if env_randomizer else []
# @TODO fix logging
self._episode_proto = None
if self._is_render:
self._pybullet_client = bullet_client.BulletClient(
connection_mode=pybullet.GUI)
else:
self._pybullet_client = bullet_client.BulletClient(
connection_mode=pybullet.DIRECT)
if 0: #enable faster training
import pkgutil
egl = pkgutil.get_loader('eglRenderer')
if egl:
pluginId = pybullet.loadPlugin(egl.get_filename(),
"_eglRendererPlugin")
else:
pluginId = pybullet.loadPlugin("eglRendererPlugin")
print("pluginId=", pluginId)
if self._urdf_version is None:
self._urdf_version = DEFAULT_URDF_VERSION
self._pybullet_client.setPhysicsEngineParameter(enableConeFriction=0)
self._signal_type = signal_type
# gait inputs
self.step_length = step_length
self.step_rotation = step_rotation
self.step_angle = step_angle
self.step_period = step_period
# poses inputs
self._base_x = 0.01
self._base_y = base_y
self._base_z = base_z
self._base_roll = base_roll
self._base_pitch = base_pitch
self._base_yaw = base_yaw
# envs inputs
self._target_orient = target_orient
self._init_orient = init_orient
self._target_position = target_position
self._start_position = start_position
# computation support params
self._random_pos_target = False
self._random_pos_start = False
self._random_orient_target = False
self._random_orient_start = False
self._companion_obj = {}
self._queue = collections.deque(
["base_y", "base_z", "roll", "pitch", "yaw"])
self._ranges = {
"base_x": (-0.02, 0.02, 0.01),
"base_y": (-0.007, 0.007, 0),
"base_z": (-0.048, 0.021, 0),
"roll": (-np.pi / 4, np.pi / 4, 0),
"pitch": (-np.pi / 4, np.pi / 4, 0),
"yaw": (-np.pi / 4, np.pi / 4, 0)
}
self.seed()
self._backwards = backwards
self._terrain_type = "plane"
self._terrain_id = terrain_id
self.reset()
self._terrain_type = terrain_type
self.terrain = Terrain(self._terrain_type, self._terrain_id)
if self._terrain_type is not "plane":
self.terrain.generate_terrain(self)
observation_high = (self._get_observation_upper_bound() +
OBSERVATION_EPS)
observation_low = (self._get_observation_lower_bound() -
OBSERVATION_EPS)
action_dim = self.num_motors
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high)
self.observation_space = spaces.Box(observation_low, observation_high)
self.viewer = None
self._hard_reset = hard_reset # This assignment need to be after reset()
self.env_goal_reached = False
if __name__ == "__main__":
res = []
with BulletSim(pybullet.DIRECT):
print("\nTesting DIRECT")
mean_time = test(log=False,plot=True)
res.append(("tiny",mean_time))
with BulletSim(pybullet.DIRECT):
plugin_fn = os.path.join(pybullet.__file__.split("bullet3")[0],"bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so")
plugin = pybullet.loadPlugin(plugin_fn,"_tinyRendererPlugin")
if plugin < 0:
print("\nPlugin Failed to load!\n")
sys.exit()
print("\nTesting DIRECT+OpenGL")
mean_time = test(log=True)
res.append(("plugin",mean_time))
with BulletSim(pybullet.GUI):
print("\nTesting GUI")
mean_time = test(log=False)
res.append(("egl",mean_time))
print()
print("rendertest.py")
import pybullet
import time
import pkgutil
plt.ion()
img = np.random.rand(200, 320)
#img = [tandard_normal((50,100))
image = plt.imshow(img,interpolation='none',animated=True,label="blah")
ax = plt.gca()
pybullet.connect(pybullet.DIRECT)
egl = pkgutil.get_loader('eglRenderer')
if (egl):
pybullet.loadPlugin(egl.get_filename(), "_eglRendererPlugin")
else:
pybullet.loadPlugin("eglRendererPlugin")
pybullet.loadURDF("plane.urdf",[0,0,-1])
pybullet.loadURDF("r2d2.urdf")
camTargetPos = [0,0,0]
cameraUp = [0,0,1]
cameraPos = [1,1,1]
pybullet.setGravity(0,0,-10)
pitch = -10.0
roll=0
upAxisIndex = 2
camDistance = 4
POSITION = 1
ORIENTATION = 2
BUTTONS = 6
controllerId = -1
print("waiting for VR controller trigger")
while (controllerId<0):
events = p.getVREvents()
for e in (events):
if (e[BUTTONS][33]==p.VR_BUTTON_IS_DOWN):
controllerId = e[CONTROLLER_ID]
if (e[BUTTONS][32]==p.VR_BUTTON_IS_DOWN):
controllerId = e[CONTROLLER_ID]
print("Using controllerId="+str(controllerId))
#plugin = p.loadPlugin("d:/develop/bullet3/bin/pybullet_vrSyncPlugin_vs2010_x64_release.dll","_vrSyncPlugin")
plugin = p.loadPlugin("vrSyncPlugin")
print("PluginId="+str(plugin))
p.executePluginCommand(plugin ,"bla", [controllerId,pr2_cid, pr2_cid2,pr2_gripper],[50,3])
while (1):
#b = p.getJointState(pr2_gripper,2)[0]
#print("b = " + str(b))
#p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3)
p.setGravity(0,0,-10)
p.disconnect()
import pkgutil
import os
from threading import Thread
# https://stackoverflow.com/questions/19448078/python-opencv-access-webcam-maximum-resolution
def set_res(cap, x,y):
cap.set(cv2.CAP_PROP_FRAME_WIDTH, int(x))
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, int(y))
return int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)),int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
p.connect(p.DIRECT)
egl = pkgutil.get_loader('eglRenderer')
if (egl):
p.loadPlugin(egl.get_filename(), "_eglRendererPlugin")
else:
p.loadPlugin("eglRendererPlugin")
dataDir = '/home/biomed/Art/bullet3/data'
#p.loadPlugin("eglRendererPlugin")
p.loadURDF(os.path.join(dataDir, "plane.urdf"),[0,0,0])
p.loadURDF(os.path.join(dataDir, "r2d2.urdf"))
camTargetPos = [0,0,0]
cameraUp = [0,0,1]
cameraPos = [1,1,2]
p.setGravity(0,0,-10)
BUTTONS = 6
controllerId = -1
print("waiting for VR controller trigger")
while (controllerId < 0):
events = p.getVREvents()
for e in (events):
if (e[BUTTONS][33] == p.VR_BUTTON_IS_DOWN):
controllerId = e[CONTROLLER_ID]
if (e[BUTTONS][32] == p.VR_BUTTON_IS_DOWN):
controllerId = e[CONTROLLER_ID]
print("Using controllerId=" + str(controllerId))
plugin = p.loadPlugin(
"d:/develop/bullet3/bin/pybullet_vrSyncPlugin_vs2010_x64_release.dll",
"_vrSyncPlugin")
#plugin = p.loadPlugin("vrSyncPlugin")
print("PluginId=" + str(plugin))
p.executePluginCommand(plugin, "bla",
[controllerId, pr2_cid, pr2_cid2, pr2_gripper], [50, 3])
while (1):
#b = p.getJointState(pr2_gripper,2)[0]
#print("b = " + str(b))
#p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3)
p.setGravity(0, 0, -10)
p.disconnect()
def reset(self):
""" reset the environment to the orignal state, i.e. an empty environment """
if self.physics_client is None:
self.physics_client = p.connect(self.gui)
p.setGravity(0, 0, GRAVITY)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# Loading egl plugin for faster rendering
if self.use_egl_plugin:
self._plugin = p.loadPlugin(self._egl.get_filename(),
'_eglRendererPlugin')
if self.gui != p.GUI:
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
# Create plane, the plan urdf has original dims of 200
self.plane_id = p.loadURDF('misc/plane100.urdf',
globalScaling=self._env_dim / 200)
if self.walls:
color = [0, 0, 0, 1]
a, b = 0, 0
for _ in range(4):
c = b
d = a ^ 1 # Flip a
# print("(%i,%i)-(%i,%i) and wall is in (%i,%i)" % (a,b,c,d,(a^c),(b^d)))
length = self._env_dim / 2
height = 2.5
width = 0.1
dimensions = [(a ^ c) * length + (1 - (a ^ c)) * width,
(1 - (a ^ c)) * length + (a ^ c) * width,
height]
position = [((a + c) / 2 - 0.5) * (self._env_dim +
(1 - (a ^ c)) * width),
((b + d) / 2 - 0.5) *
(self._env_dim + (a ^ c) * width), height]
visual_wall = p.createVisualShape(p.GEOM_BOX,
halfExtents=dimensions)
collision_wall = p.createCollisionShape(
p.GEOM_BOX, halfExtents=dimensions)
wall_id = p.createMultiBody(MASS,
collision_wall,
visual_wall,
basePosition=position)
textureId = p.loadTexture("misc/pattern.jpg")
p.changeVisualShape(wall_id, -1, textureUniqueId=textureId)
self.walls_id.append(wall_id)
a, b = c, d
# Remove objects
while self.objects_id:
p.removeBody(self.objects_id[0])
del self.objects_id[0]
assert not self.objects_id
import pybullet as p
import time
p.connect(p.GUI)
fileIO = p.loadPlugin("fileIOPlugin")
if (fileIO >= 0):
p.executePluginCommand(fileIO, "pickup.zip",
[p.AddFileIOAction, p.ZipFileIO])
objs = p.loadSDF("pickup/model.sdf")
dobot = objs[0]
p.changeVisualShape(dobot, -1, rgbaColor=[1, 1, 1, 1])
else:
print("fileIOPlugin is disabled.")
p.setPhysicsEngineParameter(enableFileCaching=False)
while (1):
p.stepSimulation()
time.sleep(1. / 240.)
import pybullet as p
import time
useDirect = False
usePort = True
p.connect(p.GUI)
id = p.loadPlugin("grpcPlugin")
#dynamically loading the plugin
#id = p.loadPlugin("E:/develop/bullet3/bin/pybullet_grpcPlugin_vs2010_x64_debug.dll", postFix="_grpcPlugin")
#start the GRPC server at hostname, port
if (id < 0):
print("Cannot load grpcPlugin")
exit(0)
if usePort:
p.executePluginCommand(id, "localhost:12345")
else:
p.executePluginCommand(id, "localhost")
while p.isConnected():
if (useDirect):
#Only in DIRECT mode, since there is no 'ping' you need to manually call to handle RCPs:
numRPC = 10
p.executePluginCommand(id, intArgs=[numRPC])
else:
dt = 1. / 240.
time.sleep(dt)
POSITION = 1
ORIENTATION = 2
BUTTONS = 6
controllerId = -1
print("waiting for VR controller trigger")
while (controllerId < 0):
events = p.getVREvents()
for e in (events):
if (e[BUTTONS][33] == p.VR_BUTTON_IS_DOWN):
controllerId = e[CONTROLLER_ID]
if (e[BUTTONS][32] == p.VR_BUTTON_IS_DOWN):
controllerId = e[CONTROLLER_ID]
print("Using controllerId=" + str(controllerId))
#plugin = p.loadPlugin("d:/develop/bullet3/bin/pybullet_vrSyncPlugin_vs2010_x64_release.dll","_vrSyncPlugin")
plugin = p.loadPlugin("vrSyncPlugin")
print("PluginId=" + str(plugin))
p.executePluginCommand(plugin, "bla",
[controllerId, pr2_cid, pr2_cid2, pr2_gripper], [50, 3])
while (1):
#b = p.getJointState(pr2_gripper,2)[0]
#print("b = " + str(b))
#p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3)
p.setGravity(0, 0, -10)
p.disconnect()
import pybullet as p
import time
import pkgutil
egl = pkgutil.get_loader('eglRenderer')
p.connect(p.DIRECT)
plugin = p.loadPlugin(egl.get_filename(), "_eglRendererPlugin")
print("plugin=",plugin)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.setGravity(0,0,-10)
p.loadURDF("plane.urdf",[0,0,-1])
p.loadURDF("r2d2.urdf")
pixelWidth = 320
pixelHeight = 220
camTargetPos = [0,0,0]
camDistance = 4
pitch = -10.0
roll=0
upAxisIndex = 2
while (p.isConnected()):
for yaw in range (0,360,10) :
start = time.time()
p.stepSimulation()
if __name__ == "__main__":
res = []
with BulletSim(pybullet.DIRECT):
print("\nTesting DIRECT")
mean_time = test(log=False, plot=True)
res.append(("tiny", mean_time))
with BulletSim(pybullet.DIRECT):
plugin_fn = os.path.join(
pybullet.__file__.split("bullet3")[0],
"bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so"
)
plugin = pybullet.loadPlugin(plugin_fn, "_tinyRendererPlugin")
if plugin < 0:
print("\nPlugin Failed to load!\n")
sys.exit()
print("\nTesting DIRECT+OpenGL")
mean_time = test(log=True)
res.append(("plugin", mean_time))
with BulletSim(pybullet.GUI):
print("\nTesting GUI")
mean_time = test(log=False)
res.append(("egl", mean_time))
print()
print("rendertest.py")
import pybullet as p
p.connect(p.GUI)
plugin = p.loadPlugin(
"d:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll",
"_testPlugin")
print("plugin=", plugin)
p.loadURDF("r2d2.urdf")
while (1):
p.getCameraImage(320, 200)
def __init__(self,
assets_root,
task=None,
disp=False,
shared_memory=False,
hz=240,
use_egl=False):
"""Creates OpenAI Gym-style environment with PyBullet.
Args:
assets_root: root directory of assets.
task: the task to use. If None, the user must call set_task for the
environment to work properly.
disp: show environment with PyBullet's built-in display viewer.
shared_memory: run with shared memory.
hz: PyBullet physics simulation step speed. Set to 480 for deformables.
use_egl: Whether to use EGL rendering. Only supported on Linux. Should get
a significant speedup in rendering when using.
Raises:
RuntimeError: if pybullet cannot load fileIOPlugin.
"""
if use_egl and disp:
raise ValueError('EGL rendering cannot be used with `disp=True`.')
self.pix_size = 0.003125
self.obj_ids = {'fixed': [], 'rigid': [], 'deformable': []}
self.homej = np.array([-1, -0.5, 0.5, -0.5, -0.5, 0]) * np.pi
self.agent_cams = cameras.RealSenseD415.CONFIG
self.assets_root = assets_root
color_tuple = [
gym.spaces.Box(0,
255,
config['image_size'] + (3, ),
dtype=np.uint8) for config in self.agent_cams
]
depth_tuple = [
gym.spaces.Box(0.0, 20.0, config['image_size'], dtype=np.float32)
for config in self.agent_cams
]
self.observation_space = gym.spaces.Dict({
'color':
gym.spaces.Tuple(color_tuple),
'depth':
gym.spaces.Tuple(depth_tuple),
})
self.position_bounds = gym.spaces.Box(low=np.array([0.25, -0.5, 0.],
dtype=np.float32),
high=np.array([0.75, 0.5, 0.28],
dtype=np.float32),
shape=(3, ),
dtype=np.float32)
self.action_space = gym.spaces.Dict({
'pose0':
gym.spaces.Tuple((self.position_bounds,
gym.spaces.Box(-1.0,
1.0,
shape=(4, ),
dtype=np.float32))),
'pose1':
gym.spaces.Tuple((self.position_bounds,
gym.spaces.Box(-1.0,
1.0,
shape=(4, ),
dtype=np.float32)))
})
# Start PyBullet.
disp_option = p.DIRECT
if disp:
disp_option = p.GUI
if shared_memory:
disp_option = p.SHARED_MEMORY
client = p.connect(disp_option)
file_io = p.loadPlugin('fileIOPlugin', physicsClientId=client)
if file_io < 0:
raise RuntimeError('pybullet: cannot load FileIO!')
if file_io >= 0:
p.executePluginCommand(file_io,
textArgument=assets_root,
intArgs=[p.AddFileIOAction],
physicsClientId=client)
self._egl_plugin = None
if use_egl:
assert sys.platform == 'linux', (
'EGL rendering is only supported on '
'Linux.')
egl = pkgutil.get_loader('eglRenderer')
if egl:
self._egl_plugin = p.loadPlugin(egl.get_filename(),
'_eglRendererPlugin')
else:
self._egl_plugin = p.loadPlugin('eglRendererPlugin')
print('EGL renderering enabled.')
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.setPhysicsEngineParameter(enableFileCaching=0)
p.setAdditionalSearchPath(assets_root)
p.setAdditionalSearchPath(tempfile.gettempdir())
p.setTimeStep(1. / hz)
# If using --disp, move default camera closer to the scene.
if disp:
target = p.getDebugVisualizerCamera()[11]
p.resetDebugVisualizerCamera(cameraDistance=1.1,
cameraYaw=90,
cameraPitch=-25,
cameraTargetPosition=target)
if task:
self.set_task(task)
