def episode_restart(self): Scene.episode_restart(self) # contains cpp_world.clean_everything() # 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_no_collision.sdf") self.stadium = p.loadSDF(filename) planeName = os.path.join(pybullet_data.getDataPath(),"mjcf/ground_plane.xml") self.ground_plane_mjcf = p.loadMJCF(planeName) for i in self.ground_plane_mjcf: p.changeVisualShape(i,-1,rgbaColor=[0,0,0,0])
def episode_restart(self): Scene.episode_restart(self) # contains cpp_world.clean_everything() if (self.stadiumLoaded==0): self.stadiumLoaded=1 # 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(),"plane_stadium.sdf") self.ground_plane_mjcf=p.loadSDF(filename) #filename = os.path.join(pybullet_data.getDataPath(),"stadium_no_collision.sdf") #self.ground_plane_mjcf = p.loadSDF(filename) # for i in self.ground_plane_mjcf: p.changeDynamics(i,-1,lateralFriction=0.8, restitution=0.5) p.changeVisualShape(i,-1,rgbaColor=[1,1,1,0.8]) p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)
ellipse = p.createCollisionShape(p.GEOM_MESH,
fileName='sphere.obj',
meshScale=[
random.random() * 0.2 + 0.3,
random.random() * 0.3 + 0.2, 0.5
])
physics_body = p.createMultiBody(mass, ellipse, -1, basePosition,
baseOrientation)
p.resetBaseVelocity(physics_body, baseVelocity, baseVelocity)
p.changeDynamics(physics_body, -1, mass=mass)
p.changeDynamics(physics_body, -1, restitution=restitution)
p.changeDynamics(planeId, -1, restitution=0.99)
# p.changeVisualShape(planeId, -1, rgbaColor=[random.random(), random.random(), random.random(), 1], textureUniqueId=perlin_id, physicsClientId=physicsClient)
p.changeVisualShape(planeId,
-1,
textureUniqueId=plane_texture,
physicsClientId=physicsClient)
p.changeVisualShape(
physics_body,
-1,
rgbaColor=[random.random(),
random.random(),
random.random(), 1],
textureUniqueId=white_id,
physicsClientId=physicsClient)
# randomize camera parameters
max_camera_dist = 20
min_camera_dist = 5
camera_fov = random.random() * 20 * (1
if random.random() > 0.5 else -1) + 40
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
p.connect(p.GUI)
plane = p.loadURDF("plane.urdf")
visualData = p.getVisualShapeData(plane, p.VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS)
print(visualData)
curTexUid = visualData[0][8]
print(curTexUid)
texUid = p.loadTexture("tex256.png")
print("texUid=", texUid)
p.changeVisualShape(plane, -1, textureUniqueId=texUid)
for i in range(100):
p.getCameraImage(320, 200)
p.changeVisualShape(plane, -1, textureUniqueId=curTexUid)
for i in range(100):
p.getCameraImage(320, 200)
batchPositions = []
for x in range(32):
for y in range(32):
for z in range(10):
batchPositions.append(
[x * meshScale[0] * 5.5, y * meshScale[1] * 5.5, (0.5 + z) * meshScale[2] * 2.5])
bodyUid = p.createMultiBody(baseMass=0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[0, 0, 2],
batchPositions=batchPositions,
useMaximalCoordinates=True)
p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid)
p.syncBodyInfo()
print("numBodies=", p.getNumBodies())
p.stopStateLogging(logId)
p.setGravity(0, 0, -10)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
currentColor = 0
while (1):
p.stepSimulation()
#time.sleep(1./120.)
#p.getCameraImage(320,200)
meshScale=meshScale)
texUid = p.loadTexture("tex256.png")
rangex = 1
rangey = 1
for i in range(rangex):
for j in range(rangey):
bodyUid = p.createMultiBody(baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[((-rangex / 2) + i) * meshScale[0] * 2,
(-rangey / 2 + j) * meshScale[1] * 2, 1],
useMaximalCoordinates=True)
p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.stopStateLogging(logId)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(1)
colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
currentColor = 0
while (1):
p.getCameraImage(320, 200)
mouseEvents = p.getMouseEvents()
for e in mouseEvents:
if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)):
mouseX = e[1]
mouseY = e[2]
import pybullet as p
import pybullet_data
import time
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadSDF("stadium.sdf")
p.setGravity(0,0,-10)
objects = p.loadMJCF("mjcf/sphere.xml")
sphere = objects[0]
p.resetBasePositionAndOrientation(sphere,[0,0,1],[0,0,0,1])
p.changeDynamics(sphere,-1,linearDamping=0.9)
p.changeVisualShape(sphere,-1,rgbaColor=[1,0,0,1])
forward = 0
turn = 0
forwardVec = [2,0,0]
cameraDistance = 1
cameraYaw = 35
cameraPitch = -35
while (1):
spherePos, orn = p.getBasePositionAndOrientation(sphere)
cameraTargetPosition = spherePos
p.resetDebugVisualizerCamera(cameraDistance,cameraYaw,cameraPitch,cameraTargetPosition)
camInfo = p.getDebugVisualizerCamera()
camForward = camInfo[5]
for x in range(32):
for y in range(32):
for z in range(10):
batchPositions.append([
x * meshScale[0] * 5.5, y * meshScale[1] * 5.5,
(0.5 + z) * meshScale[2] * 2.5
])
bodyUid = p.createMultiBody(baseMass=0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[0, 0, 2],
batchPositions=batchPositions,
useMaximalCoordinates=True)
p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid)
p.syncBodyInfo()
print("numBodies=", p.getNumBodies())
p.stopStateLogging(logId)
p.setGravity(0, 0, -10)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
currentColor = 0
while (1):
p.stepSimulation()
#time.sleep(1./120.)
#p.getCameraImage(320,200)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
p.resetDebugVisualizerCamera(3,-420,-30,[0.3,0.9,-2])
p.setGravity(0, 0, -9000)
tex = p.loadTexture("uvmap.png")
planeId = p.loadURDF("plane.urdf", [0,0,-2])
#boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
cubeStartOrientation = p.getQuaternionFromEuler([0,100,10])
i = 0
while(i<3):
bunnyId = p.loadSoftBody("torus/torus_textured.obj",[0,1 , i] ,cubeStartOrientation ,simFileName="torus.vtk", mass = 3, useNeoHookean = 1, NeoHookeanMu = 180, NeoHookeanLambda = 600, NeoHookeanDamping = 0.01, collisionMargin = 0.006, useSelfCollision = 1, frictionCoeff = 0.5, repulsionStiffness = 800)
p.changeVisualShape(bunnyId, -1, rgbaColor=[1,1,1,1], textureUniqueId=tex, flags=0)
i = i + 1
a=0
while( a < 4 ):
bunny3 = p.loadURDF("torus_deform.urdf", [0,1 + a/2,-0.5 + a], flags=p.URDF_USE_SELF_COLLISION)
a = a + 1
p.changeVisualShape(bunny3, -1, rgbaColor=[1,1,1,1], textureUniqueId=tex, flags=0)
p.setPhysicsEngineParameter(sparseSdfVoxelSize=20)
p.setRealTimeSimulation(0)
while p.isConnected():
p.stepSimulation()
p.getCameraImage(320,200)
p.setGravity(0,0,-10)
texUid = p.loadTexture("tex256.png")
rangex = 1
rangey = 1
for i in range(rangex):
for j in range(rangey):
bodyUid = p.createMultiBody(baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[
((-rangex / 2) + i) * meshScale[0] * 2,
(-rangey / 2 + j) * meshScale[1] * 2, 1
],
useMaximalCoordinates=True)
p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.stopStateLogging(logId)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(1)
colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
currentColor = 0
while (1):
p.getCameraImage(320, 200)
mouseEvents = p.getMouseEvents()
for e in mouseEvents:
if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)):
mouseX = e[1]
mouseY = e[2]
def reset(self, env):
# Add container box.
self.zone_size = self.random_size(0.05, 0.3, 0.05, 0.3, 0.05, 0.05)
self.zone_pose = self.random_pose(env, self.zone_size)
container_template = 'assets/container/container-template.urdf'
half = np.float32(self.zone_size) / 2
replace = {'DIM': self.zone_size, 'HALF': half}
container_urdf = self.fill_template(container_template, replace)
env.add_object(container_urdf, self.zone_pose, fixed=True)
os.remove(container_urdf)
margin = 0.01
min_object_dim = 0.05
bboxes = []
class TreeNode:
def __init__(self, parent, children, bbox):
self.parent = parent
self.children = children
self.bbox = bbox # min x, min y, min z, max x, max y, max z
def KDTree(node):
size = node.bbox[3:] - node.bbox[:3]
# Choose which axis to split.
split = size > 2 * min_object_dim
if np.sum(split) == 0:
bboxes.append(node.bbox)
return
split = np.float32(split) / np.sum(split)
split_axis = np.random.choice(range(len(split)), 1, p=split)[0]
# Split along chosen axis and create 2 children
cut_ind = np.random.rand() * \
(size[split_axis] - 2 * min_object_dim) + \
node.bbox[split_axis] + min_object_dim
child1_bbox = node.bbox.copy()
child1_bbox[3 + split_axis] = cut_ind - margin / 2.
child2_bbox = node.bbox.copy()
child2_bbox[split_axis] = cut_ind + margin / 2.
node.children = [TreeNode(node, [], bbox=child1_bbox),
TreeNode(node, [], bbox=child2_bbox)]
KDTree(node.children[0])
KDTree(node.children[1])
# Split container space with KD trees.
stack_size = np.array(self.zone_size)
stack_size[0] -= 0.01
stack_size[1] -= 0.01
root_size = (0.01, 0.01, 0) + tuple(stack_size)
root = TreeNode(None, [], bbox=np.array(root_size))
KDTree(root)
colors = [utils.COLORS['purple'],
utils.COLORS['blue'],
utils.COLORS['green'],
utils.COLORS['yellow'],
utils.COLORS['orange'],
utils.COLORS['red'],
utils.COLORS['pink'],
utils.COLORS['cyan'],
utils.COLORS['gray']]
# Add objects in container.
self.object_points = {}
bboxes = np.array(bboxes)
object_template = 'assets/box/box-template.urdf'
for bbox in bboxes:
size = bbox[3:] - bbox[:3]
position = size / 2. + bbox[:3]
position[0] += -self.zone_size[0] / 2
position[1] += -self.zone_size[1] / 2
pose = (position, (0, 0, 0, 1))
pose = self.multiply(self.zone_pose, pose)
urdf = self.fill_template(object_template, {'DIM': size})
box_id = env.add_object(urdf, pose)
os.remove(urdf)
icolor = np.random.choice(range(len(colors)), 1).squeeze()
p.changeVisualShape(box_id, -1, rgbaColor=colors[icolor] + [1])
self.object_points[box_id] = self.get_object_points(box_id)
# Randomly select object in box and save ground truth pose.
object_volumes = []
self.goal = {'places': {}, 'steps': []}
for object_id in env.objects:
true_pose = p.getBasePositionAndOrientation(object_id)
object_size = p.getVisualShapeData(object_id)[0][3]
object_volumes.append(np.prod(np.array(object_size) * 100))
pose = self.random_pose(env, object_size)
p.resetBasePositionAndOrientation(object_id, pose[0], pose[1])
self.goal['places'][object_id] = true_pose
symmetry = 0 # zone-evaluation: symmetry does not matter
self.goal['steps'].append({object_id: (symmetry, [object_id])})
self.total_rewards = 0
self.max_steps = len(self.goal['steps']) * 2
# Sort oracle picking order by object size.
self.goal['steps'] = [self.goal['steps'][i]
for i in np.argsort(-1 * np.array(object_volumes))]
def reset(self):
super(FeedingEnv, self).reset()
self.build_assistive_env('wheelchair')
self.furniture.set_on_ground()
if self.robot.wheelchair_mounted:
wheelchair_pos, wheelchair_orient = self.furniture.get_base_pos_orient(
)
self.robot.set_base_pos_orient(
wheelchair_pos +
np.array(self.robot.toc_base_pos_offset[self.task]),
[0, 0, -np.pi / 2.0])
if self.general_model:
# Randomize the human body shape
gender = self.np_random.choice(['male', 'female'])
# body_shape = self.np_random.randn(1, self.human.num_body_shape)
body_shape = self.np_random.uniform(-2, 5,
(1, self.human.num_body_shape))
# human_height = self.np_random.uniform(1.59, 1.91) if gender == 'male' else self.np_random.uniform(1.47, 1.78)
human_height = self.np_random.uniform(1.5, 1.9)
else:
gender = self.gender
body_shape = self.body_shape_filename
human_height = self.human_height
# Randomize human pose
joint_angles = [(self.human.j_left_hip_x, -90),
(self.human.j_right_hip_x, -90),
(self.human.j_left_knee_x, 70),
(self.human.j_right_knee_x, 70),
(self.human.j_left_shoulder_z, -45),
(self.human.j_right_shoulder_z, 45),
(self.human.j_left_elbow_y, -90),
(self.human.j_right_elbow_y, 90)]
# u = self.np_random.uniform
# joint_angles += [(self.human.j_waist_x, u(-30, 45)), (self.human.j_waist_y, u(-45, 45)), (self.human.j_waist_z, u(-30, 30)), (self.human.j_lower_neck_x, u(-30, 30)), (self.human.j_lower_neck_y, u(-30, 30)), (self.human.j_lower_neck_z, u(-10, 10)), (self.human.j_upper_neck_x, u(-45, 45)), (self.human.j_upper_neck_y, u(-30, 30)), (self.human.j_upper_neck_z, u(-30, 30))]
# joint_angles += [(self.human.j_waist_x, u(-20, 30)), (self.human.j_waist_y, u(-45, 0)), (self.human.j_waist_z, u(0, 30)), (self.human.j_lower_neck_x, u(-30, 30)), (self.human.j_lower_neck_y, u(-30, 30)), (self.human.j_lower_neck_z, u(-10, 10)), (self.human.j_upper_neck_x, u(-30, 30)), (self.human.j_upper_neck_y, u(-30, 30)), (self.human.j_upper_neck_z, u(-30, 30))]
joint_angles += [
(j, self.np_random.uniform(-10, 10))
for j in (self.human.j_waist_x, self.human.j_waist_y,
self.human.j_waist_z, self.human.j_lower_neck_x,
self.human.j_lower_neck_y, self.human.j_lower_neck_z,
self.human.j_upper_neck_x, self.human.j_upper_neck_y,
self.human.j_upper_neck_z)
]
self.human.init(self.directory,
self.id,
self.np_random,
gender=gender,
height=human_height,
body_shape=body_shape,
joint_angles=joint_angles,
position=[0, 0, 0],
orientation=[0, 0, 0])
# Place human in chair
chair_seat_position = np.array([0, 0.05, 0.6])
self.human.set_base_pos_orient(
self.furniture.get_base_pos_orient()[0] + chair_seat_position -
self.human.get_vertex_positions(self.human.bottom_index),
[0, 0, 0, 1])
# Create a table
self.table = Furniture()
self.table.init('table', self.directory, self.id, self.np_random)
self.generate_target()
p.resetDebugVisualizerCamera(cameraDistance=1.10,
cameraYaw=40,
cameraPitch=-45,
cameraTargetPosition=[-0.2, 0, 0.75],
physicsClientId=self.id)
# Open gripper to hold the tool
self.robot.set_gripper_open_position(self.robot.right_gripper_indices,
self.robot.gripper_pos[self.task],
set_instantly=True)
# Initialize the tool in the robot's gripper
self.tool.init(self.robot,
self.task,
self.directory,
self.id,
self.np_random,
right=True,
mesh_scale=[0.08] * 3)
target_ee_orient = np.array(
p.getQuaternionFromEuler(np.array(
self.robot.toc_ee_orient_rpy[self.task]),
physicsClientId=self.id))
while True:
# Continually resample initial end effector poses until we find one where the robot isn't colliding with the person
# target_ee_pos = np.array([-0.1, -0.6, 1.2]) + np.array([self.np_random.uniform(-0.1, 0.1), self.np_random.uniform(-0.2, 0.3), self.np_random.uniform(-0.1, 0.1)])
mouth_pos = self.human.get_pos_orient(self.human.mouth)[0]
target_ee_pos = mouth_pos + np.array([
self.np_random.uniform(-0.3, 0),
self.np_random.uniform(-0.6, -0.3),
self.np_random.uniform(-0.3, 0.1)
])
if self.robot.mobile:
# Randomize robot base pose
pos = np.array(self.robot.toc_base_pos_offset[self.task])
pos[:2] += self.np_random.uniform(-0.1, 0.1, size=2)
orient = np.array(self.robot.toc_ee_orient_rpy[self.task])
orient[2] += self.np_random.uniform(-np.deg2rad(30),
np.deg2rad(30))
self.robot.set_base_pos_orient(pos, orient)
# Randomize starting joint angles
self.robot.set_joint_angles(
[3], [0.75 + self.np_random.uniform(-0.1, 0.1)])
# Randomly set friction of the ground
self.plane.set_frictions(
self.plane.base,
lateral_friction=self.np_random.uniform(0.025, 0.5),
spinning_friction=0,
rolling_friction=0)
elif self.robot.wheelchair_mounted:
# Use IK to find starting joint angles for mounted robots
self.robot.ik_random_restarts(right=True,
target_pos=target_ee_pos,
target_orient=target_ee_orient,
max_iterations=1000,
max_ik_random_restarts=40,
success_threshold=0.03,
step_sim=False,
check_env_collisions=False)
else:
# Use TOC with JLWKI to find an optimal base position for the robot near the person
self.robot.position_robot_toc(
self.task,
'right', [(target_ee_pos, target_ee_orient)],
[(self.target_pos, None)],
self.human,
step_sim=False,
check_env_collisions=False,
attempts=50)
# Check if the robot is colliding with the person or table
self.tool.reset_pos_orient()
_, _, _, _, dists_human = self.robot.get_closest_points(self.human,
distance=0)
_, _, _, _, dists_table = self.robot.get_closest_points(self.table,
distance=0)
_, _, _, _, dists_tool = self.tool.get_closest_points(self.human,
distance=0)
if not dists_human and not dists_table and not dists_tool:
break
# Open gripper to hold the tool
self.robot.set_gripper_open_position(self.robot.right_gripper_indices,
self.robot.gripper_pos[self.task],
set_instantly=True)
# Initialize the tool in the robot's gripper
# self.tool.init(self.robot, self.task, self.directory, self.id, self.np_random, right=True, mesh_scale=[0.08]*3)
# Place a bowl on a table
self.bowl = Furniture()
self.bowl.init('bowl', self.directory, self.id, self.np_random)
if not self.robot.mobile:
self.robot.set_gravity(0, 0, 0)
self.human.set_gravity(0, 0, 0)
self.tool.set_gravity(0, 0, 0)
p.setPhysicsEngineParameter(numSubSteps=4,
numSolverIterations=10,
physicsClientId=self.id)
# Generate food
spoon_pos, spoon_orient = self.tool.get_base_pos_orient()
food_radius = 0.005
food_mass = 0.001
batch_positions = []
for i in range(2):
for j in range(2):
for k in range(2):
batch_positions.append(
np.array([
i * 2 * food_radius - 0.005, j * 2 *
food_radius, k * 2 * food_radius + 0.01
]) + spoon_pos)
self.foods = self.create_spheres(radius=food_radius,
mass=food_mass,
batch_positions=batch_positions,
visual=False,
collision=True)
colors = [[60. / 256., 186. / 256., 84. / 256., 1],
[244. / 256., 194. / 256., 13. / 256., 1],
[219. / 256., 50. / 256., 54. / 256., 1],
[72. / 256., 133. / 256., 237. / 256., 1]]
for i, f in enumerate(self.foods):
p.changeVisualShape(f.body,
-1,
rgbaColor=colors[i % len(colors)],
physicsClientId=self.id)
self.total_food_count = len(self.foods)
self.foods_active = [f for f in self.foods]
# Enable rendering
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,
1,
physicsClientId=self.id)
# Drop food in the spoon
for _ in range(25):
p.stepSimulation(physicsClientId=self.id)
self.init_env_variables()
return self._get_obs()
import matplotlib.pyplot as plt
import numpy as np
import pybullet as p
import time
direct = p.connect(p.GUI)#, options="--window_backend=2 --render_device=0")
#egl = p.loadPlugin("eglRendererPlugin")
p.loadURDF('plane.urdf')
p.loadURDF("r2d2.urdf",[0,0,1])
p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.0, 0.025])
cube_trans = p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.1, 0.025])
p.changeVisualShape(cube_trans,-1,rgbaColor=[1,1,1,0.1])
width = 128
height = 128
fov = 60
aspect = width / height
near = 0.02
far = 1
view_matrix = p.computeViewMatrix([0, 0, 0.5], [0, 0, 0], [1, 0, 0])
projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far)
# Get depth values using the OpenGL renderer
images = p.getCameraImage(width, height, view_matrix, projection_matrix, shadow=True,renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgb_opengl= np.reshape(images[2], (height, width, 4))*1./255.
depth_buffer_opengl = np.reshape(images[3], [width, height])
depth_opengl = far * near / (far - (far - near) * depth_buffer_opengl)
def changeVisualShapeColor(objectUniqueId: int, color: List[float]):
p.changeVisualShape(objectUniqueId=objectUniqueId, linkIndex=-1, rgbaColor=color)
direct = p.connect(p.GUI, options="--window scale=2")
# direct = p.connect(p.GUI)
#, options="--window_backend=2 --render_device=0")
#egl = p.loadPlugin("eglRendererPlugin")
# Add a path where we want to search for data
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# Load plane, R2D2 and two small cubes from URDF
p.loadURDF('plane.urdf')
p.loadURDF("r2d2.urdf", [0, 0, 1])
p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.0, 0.025])
cube_trans = p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.1, 0.025])
# Change color of the second cube
p.changeVisualShape(cube_trans, -1, rgbaColor=[1, 1, 1, 0.1])
# Height and width
width = 512
height = 512
# Rendering parameters
fov = 60
aspect = width / height
near = 0.02
far = 1
# Compute view matrix and project matrix
view_matrix = p.computeViewMatrix([0, 0, 0.5], [0, 0, 0], [1, 0, 0])
projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far)
import pybullet as p
import pybullet_data
import time
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadSDF("stadium.sdf")
p.setGravity(0, 0, -10)
objects = p.loadMJCF("mjcf/sphere.xml")
sphere = objects[0]
p.resetBasePositionAndOrientation(sphere, [0, 0, 1], [0, 0, 0, 1])
p.changeDynamics(sphere, -1, linearDamping=0.9)
p.changeVisualShape(sphere, -1, rgbaColor=[1, 0, 0, 1])
forward = 0
turn = 0
forwardVec = [2, 0, 0]
cameraDistance = 1
cameraYaw = 35
cameraPitch = -35
while (1):
spherePos, orn = p.getBasePositionAndOrientation(sphere)
cameraTargetPosition = spherePos
p.resetDebugVisualizerCamera(cameraDistance, cameraYaw, cameraPitch, cameraTargetPosition)
camInfo = p.getDebugVisualizerCamera()
camForward = camInfo[5]
keys = p.getKeyboardEvents()
rayFrom,rayTo, alpha = getRayFromTo(w*(width/imgW),h*(height/imgH))
rf = np.array(rayFrom)
rt = np.array(rayTo)
vec = rt-rf
l = np.sqrt(np.dot(vec,vec))
depthImg = float(depthBuffer[h,w])
far=1000.
near=0.01
depth = far * near / (far - (far - near) * depthImg)
depth/=math.cos(alpha)
newTo = (depth/l)*vec+rf
p.addUserDebugLine(rayFrom,newTo,[1,0,0])
mb = p.createMultiBody(baseMass=0,baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = newTo, useMaximalCoordinates=True)
color = rgbBuffer[h,w]
color=[color[0]/255.,color[1]/255.,color[2]/255.,1]
p.changeVisualShape(mb,-1,rgbaColor=color)
p.addUserDebugLine(corners3D[0],corners3D[1],[1,0,0])
p.addUserDebugLine(corners3D[1],corners3D[2],[1,0,0])
p.addUserDebugLine(corners3D[2],corners3D[3],[1,0,0])
p.addUserDebugLine(corners3D[3],corners3D[0],[1,0,0])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
print("ready\n")
#p.removeBody(plane)
#p.removeBody(cube)
while (1):
p.setGravity(0,0,-10)
force4Diff = 5
if k == p.B3G_DOWN_ARROW and (v & p.KEY_WAS_RELEASED):
force4Diff = 0.
if k == ord('n') and (v & p.KEY_IS_DOWN):
baseForce = max(0, baseForce - 0.1)
print("BaseForce = {}".format(baseForce))
if k == ord('m') and (v & p.KEY_IS_DOWN):
baseForce = min(10, baseForce + 0.1)
print("BaseForce = {}".format(baseForce))
# apply force to the four rotors
force1 = [0., 0., force1Diff + baseForce]
force2 = [0., 0., force2Diff + baseForce]
force3 = [0., 0., force3Diff + baseForce]
force4 = [0., 0., force4Diff + baseForce]
p.applyExternalForce(copter, -1, force1, [0, 0.25, 0], flags=p.LINK_FRAME)
p.applyExternalForce(copter, -1, force2, [0, -0.25, 0], flags=p.LINK_FRAME)
p.applyExternalForce(copter, -1, force3, [0.25, 0, 0], flags=p.LINK_FRAME)
p.applyExternalForce(copter, -1, force4, [-0.25, 0, 0], flags=p.LINK_FRAME)
# change color of copter in case of collision
if p.getContactPoints(copter):
p.changeVisualShape(copter, -1, rgbaColor=contactColor)
else:
p.changeVisualShape(copter, -1, rgbaColor=color)
# advance the simulation
p.stepSimulation()
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
p.setJointMotorControl2(humanoid,j,p.VELOCITY_CONTROL,targetVelocity=0, force=0)
p.setJointMotorControl2(humanoid3,j,p.VELOCITY_CONTROL,targetVelocity=0, force=31)
p.setJointMotorControl2(humanoid4,j,p.VELOCITY_CONTROL,targetVelocity=0, force=10)
#print(ji)
print("joint[",j,"].type=",jointTypes[ji[2]])
print("joint[",j,"].name=",ji[1])
jointIds=[]
paramIds=[]
for j in range (p.getNumJoints(humanoid)):
#p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0)
p.changeVisualShape(humanoid,j,rgbaColor=[1,1,1,1])
info = p.getJointInfo(humanoid,j)
#print(info)
if (not useMotionCapture):
jointName = info[1]
jointType = info[2]
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
jointIds.append(j)
#paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0))
#print("jointName=",jointName, "at ", j)
p.changeVisualShape(humanoid,2,rgbaColor=[1,0,0,1])
chest=1
neck=2
rightHip=3
rightKnee=4
rangey = 3 for i in range (rangex): for j in range (rangey ): p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [((-rangex/2)+i)*meshScale[0]*2,(-rangey/2+j)*meshScale[1]*2,1], useMaximalCoordinates=True) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) p.stopStateLogging(logId) p.setGravity(0,0,-10) p.setRealTimeSimulation(1) colors = [[1,0,0,1],[0,1,0,1],[0,0,1,1],[1,1,1,1]] currentColor = 0 while (1): mouseEvents = p.getMouseEvents() for e in mouseEvents: if ((e[0] == 2) and (e[3]==0) and (e[4]& p.KEY_WAS_TRIGGERED)): mouseX = e[1] mouseY = e[2] rayFrom,rayTo=getRayFromTo(mouseX,mouseY) rayInfo = p.rayTest(rayFrom,rayTo) #p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3) for l in range(len(rayInfo)): hit = rayInfo[l] objectUid = hit[0] if (objectUid>=1): #p.removeBody(objectUid) p.changeVisualShape(objectUid,-1,rgbaColor=colors[currentColor]) currentColor+=1 if (currentColor>=len(colors)): currentColor=0
from time import sleep
from PIL import Image
import matplotlib.pyplot as plt
import numpy as np
physicsClient = p.connect(p.GUI)
p.setGravity(0,0,0)
bearStartPos1 = [-3.3,0,0]
bearStartOrientation1 = p.getQuaternionFromEuler([0,0,0])
bearId1 = p.loadURDF("teddy_large.urdf", bearStartPos1, bearStartOrientation1)
bearStartPos2 = [0,0,0]
bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0])
bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2)
textureId = p.loadTexture("checker_grid.jpg")
p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId)
p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId)
useRealTimeSimulation = 1
if (useRealTimeSimulation):
p.setRealTimeSimulation(1)
while 1:
if (useRealTimeSimulation):
p.setGravity(0,0,0)
sleep(0.01) # Time in seconds.
else:
p.stepSimulation()
rayFrom, rayTo, alpha = getRayFromTo(w * (width / imgW),
h * (height / imgH))
rf = np.array(rayFrom)
rt = np.array(rayTo)
vec = rt - rf
l = np.sqrt(np.dot(vec, vec))
depthImg = float(depthBuffer[h, w])
far = 1000.
near = 0.01
depth = far * near / (far - (far - near) * depthImg)
depth /= math.cos(alpha)
newTo = (depth / l) * vec + rf
p.addUserDebugLine(rayFrom, newTo, [1, 0, 0])
mb = p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=newTo,
useMaximalCoordinates=True)
color = rgbBuffer[h, w]
color = [color[0] / 255., color[1] / 255., color[2] / 255., 1]
p.changeVisualShape(mb, -1, rgbaColor=color)
p.addUserDebugLine(corners3D[0], corners3D[1], [1, 0, 0])
p.addUserDebugLine(corners3D[1], corners3D[2], [1, 0, 0])
p.addUserDebugLine(corners3D[2], corners3D[3], [1, 0, 0])
p.addUserDebugLine(corners3D[3], corners3D[0], [1, 0, 0])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
print("ready\n")
#p.removeBody(plane)
#p.removeBody(cube)
while (1):
p.setGravity(0, 0, -10)
import pybullet as p
import time
p.connect(p.GUI)
p.loadURDF("plane.urdf")
sphereUid = p.loadURDF("sphere_transparent.urdf", [0, 0, 2])
redSlider = p.addUserDebugParameter("red", 0, 1, 1)
greenSlider = p.addUserDebugParameter("green", 0, 1, 0)
blueSlider = p.addUserDebugParameter("blue", 0, 1, 0)
alphaSlider = p.addUserDebugParameter("alpha", 0, 1, 0.5)
while (1):
red = p.readUserDebugParameter(redSlider)
green = p.readUserDebugParameter(greenSlider)
blue = p.readUserDebugParameter(blueSlider)
alpha = p.readUserDebugParameter(alphaSlider)
p.changeVisualShape(sphereUid, -1, rgbaColor=[red, green, blue, alpha])
p.getCameraImage(320,
200,
flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
time.sleep(0.01)
def add_object(graphic_file="duck.obj",
collision_file="duck_vhacd.obj",
texture_file=(),
mass=1,
base_position=(0., 0., 0.),
base_orientation=(0., 0., 0., 1.),
mesh_scale=(1, 1, 1),
COM_shift=(0, 0.0, 0),
color=(),
diagonal_inertial=None,
virtual_links=False,
constrained=False):
"""
Adds a textured mesh into the scene.
Args:
graphic_file : Relative or absolute path to the mesh file.
collision_file : Relative or absolute path to the mesh file.
texture_file : Relative or absolute path to the texture file.
If empty, no texture will be loaded.
mass : Mass of the object in Kg.
base_position : Position of the base of the object.
base_orientation : Orientation of the base of the object.
mesh_scale : Scale of the mesh.
COM_shift : COM position of the object w.r.t the local coordinates.
color : Color of the object.
virtual_links : If true, defines virtual links for moving the base in the space.
constrained : If true, creates a dynamics constraint.
Returns:
int : Unique ID of the object in Pybullet
(int) : Constraint ID (only returns if constrained is True).
"""
shift = [0, 0, 0]
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
fileName=str(graphic_file),
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=[0, 0, 0],
meshScale=mesh_scale)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
fileName=str(collision_file),
collisionFramePosition=shift,
meshScale=mesh_scale)
if virtual_links:
n_links = 6
link_mass = [0.001] * n_links
link_col_shape = [-1] * n_links
link_vis_shape = [-1] * n_links
link_position = [[0, 0, 0]] * n_links
link_orientation = [[0, 0, 0, 1]] * n_links
link_inertial_frame_position = [[0, 0, 0]] * n_links
link_inertial_frame_orientation = [[0, 0, 0, 1]] * n_links
link_parent = [0, 1, 2, 3, 4, 5]
link_joint_type = [p.JOINT_PRISMATIC, p.JOINT_PRISMATIC, p.JOINT_PRISMATIC, p.JOINT_REVOLUTE, p.JOINT_REVOLUTE, p.JOINT_REVOLUTE]
link_joint_axis = [[1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 0, 0], [0, 1, 0], [0, 0, 1]]
obj_id = p.createMultiBody(baseMass=mass,
baseInertialFramePosition=COM_shift,
baseInertialFrameOrientation=[0, 0, 0, 1],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=base_position,
baseOrientation=base_orientation,
linkMasses=link_mass,
linkCollisionShapeIndices=link_col_shape,
linkVisualShapeIndices=link_vis_shape,
linkPositions=link_position,
linkOrientations=link_orientation,
linkInertialFramePositions=link_inertial_frame_position,
linkInertialFrameOrientations=link_inertial_frame_orientation,
linkParentIndices=link_parent,
linkJointTypes=link_joint_type,
linkJointAxis=link_joint_axis,
useMaximalCoordinates=False)
for i in range(n_links):
p.changeDynamics(bodyUniqueId=obj_id,
linkIndex=i,
jointDamping=0.1,
jointLowerLimit=-100,
jointUpperLimit=100,
localInertiaDiagonal=[0.001, 0.001, 0.001])
else:
obj_id = p.createMultiBody(baseMass=mass,
baseInertialFramePosition=COM_shift,
baseInertialFrameOrientation=[0, 0, 0, 1],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=base_position,
baseOrientation=base_orientation,
useMaximalCoordinates=False)
if texture_file:
textureId = p.loadTexture(textureFilename=str(texture_file))
p.changeVisualShape(objectUniqueId=obj_id, linkIndex=-1, textureUniqueId=textureId)
if color:
p.changeVisualShape(objectUniqueId=obj_id, linkIndex=-1, rgbaColor=color)
if diagonal_inertial:
assert isinstance(diagonal_inertial, list)
p.changeDynamics(bodyUniqueId=obj_id, linkIndex=-1, localInertiaDiagonal=diagonal_inertial)
if constrained:
constraint_id = p.createConstraint(parentBodyUniqueId=obj_id,
parentLinkIndex=-1,
childBodyUniqueId=-1,
childLinkIndex=-1,
jointType=p.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
childFrameOrientation=[0, 0, 0])
return obj_id, constraint_id
return obj_id
def init_simulator():
global boxId, reset, our_mpc, normal_mpc, terrain
robot_start_pos = [0, 0, 0.42]
p.connect(p.GUI) # or p.DIRECT for non-graphical version
#p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
reset = p.addUserDebugParameter("reset", 1, 0, 0)
our_mpc = p.addUserDebugParameter("our_mpc", 1, 0, 0)
normal_mpc = p.addUserDebugParameter("normal_mpc", 1, 0, 0)
p.setGravity(0, 0, -9.8)
p.setTimeStep(1.0/freq)
p.resetDebugVisualizerCamera(0.2, 45, -15, [1, -1, 1])
# p.setPhysicsEngineParameter(numSolverIterations=30)
# p.setPhysicsEngineParameter(enableConeFriction=0)
# planeId = p.loadURDF("plane.urdf")
# p.changeDynamics(planeId, -1, lateralFriction=1.0)
# boxId = p.loadURDF("/home/wgx/Workspace_Ros/src/cloudrobot/src/quadruped_robot.urdf", robot_start_pos,
# useFixedBase=FixedBase)
heightPerturbationRange = 0.06
numHeightfieldRows = 256
numHeightfieldColumns = 256
if terrain == "plane":
planeShape = p.createCollisionShape(shapeType=p.GEOM_PLANE)
ground_id = p.createMultiBody(0, planeShape)
p.resetBasePositionAndOrientation(ground_id, [0, 0, 0], [0, 0, 0, 1])
elif terrain == "random1":
heightfieldData = [0]*numHeightfieldRows*numHeightfieldColumns
for j in range(int(numHeightfieldColumns/2)):
for i in range(int(numHeightfieldRows/2)):
height = random.uniform(0, heightPerturbationRange)
heightfieldData[2*i+2*j*numHeightfieldRows] = height
heightfieldData[2*i+1+2*j*numHeightfieldRows] = height
heightfieldData[2*i+(2*j+1)*numHeightfieldRows] = height
heightfieldData[2*i+1+(2*j+1)*numHeightfieldRows] = height
terrainShape = p.createCollisionShape(shapeType=p.GEOM_HEIGHTFIELD, meshScale=[.05, .05, 1], heightfieldTextureScaling=(
numHeightfieldRows-1)/2, heightfieldData=heightfieldData, numHeightfieldRows=numHeightfieldRows, numHeightfieldColumns=numHeightfieldColumns)
ground_id = p.createMultiBody(0, terrainShape)
p.resetBasePositionAndOrientation(ground_id, [0, 0, 0], [0, 0, 0, 1])
elif terrain == "random2":
terrain_shape = p.createCollisionShape(
shapeType=p.GEOM_HEIGHTFIELD,
meshScale=[.5, .5, .5],
fileName="heightmaps/ground0.txt",
heightfieldTextureScaling=128)
ground_id = p.createMultiBody(0, terrain_shape)
path = rospack.get_path('quadruped_ctrl')
textureId = p.loadTexture(path+"/model/grass.png")
p.changeVisualShape(ground_id, -1, textureUniqueId=textureId)
p.resetBasePositionAndOrientation(ground_id, [1, 0, 0.2], [0, 0, 0, 1])
elif terrain == "stairs":
planeShape = p.createCollisionShape(shapeType=p.GEOM_PLANE)
ground_id = p.createMultiBody(0, planeShape)
# p.resetBasePositionAndOrientation(ground_id, [0, 0, 0], [0, 0.0872, 0, 0.9962])
p.resetBasePositionAndOrientation(ground_id, [0, 0, 0], [0, 0, 0, 1])
# many box
colSphereId = p.createCollisionShape(
p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.01])
colSphereId1 = p.createCollisionShape(
p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.02])
colSphereId2 = p.createCollisionShape(
p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.03])
colSphereId3 = p.createCollisionShape(
p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.04])
# colSphereId4 = p.createCollisionShape(
# p.GEOM_BOX, halfExtents=[0.03, 0.03, 0.03])
p.createMultiBody(100, colSphereId, basePosition=[1.0, 1.0, 0.0])
p.changeDynamics(colSphereId, -1, lateralFriction=lateralFriction)
p.createMultiBody(100, colSphereId1, basePosition=[1.2, 1.0, 0.0])
p.changeDynamics(colSphereId1, -1, lateralFriction=lateralFriction)
p.createMultiBody(100, colSphereId2, basePosition=[1.4, 1.0, 0.0])
p.changeDynamics(colSphereId2, -1, lateralFriction=lateralFriction)
p.createMultiBody(100, colSphereId3, basePosition=[1.6, 1.0, 0.0])
p.changeDynamics(colSphereId3, -1, lateralFriction=lateralFriction)
# p.createMultiBody(10, colSphereId4, basePosition=[2.7, 1.0, 0.0])
# p.changeDynamics(colSphereId4, -1, lateralFriction=0.5)
p.changeDynamics(ground_id, -1, lateralFriction=lateralFriction)
urdf_file = os.path.dirname(os.path.abspath(
__file__))+"/../urdf/mini_cheetah.urdf"
boxId = p.loadURDF(urdf_file, robot_start_pos,
useFixedBase=False)
p.changeDynamics(boxId, 3, spinningFriction=spinningFriction)
p.changeDynamics(boxId, 7, spinningFriction=spinningFriction)
p.changeDynamics(boxId, 11, spinningFriction=spinningFriction)
p.changeDynamics(boxId, 15, spinningFriction=spinningFriction)
jointIds = []
for j in range(p.getNumJoints(boxId)):
p.getJointInfo(boxId, j)
jointIds.append(j)
# slope terrain
# colSphereId = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.5, 0.5, 0.001])
# colSphereId1 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2, 0.5, 0.06])
# BoxId = p.createMultiBody(100, colSphereId, basePosition=[1.0, 1.0, 0.0])
# BoxId = p.createMultiBody(100, colSphereId1, basePosition=[1.6, 1.0, 0.0])
# stairs
# colSphereId = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.02])
# colSphereId1 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.04])
# colSphereId2 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.06])
# colSphereId3 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.1, 0.4, 0.08])
# colSphereId4 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[1.0, 0.4, 0.1])
# BoxId = p.createMultiBody(100, colSphereId, basePosition=[1.0, 1.0, 0.0])
# BoxId = p.createMultiBody(100, colSphereId1, basePosition=[1.2, 1.0, 0.0])
# BoxId = p.createMultiBody(100, colSphereId2, basePosition=[1.4, 1.0, 0.0])
# BoxId = p.createMultiBody(100, colSphereId3, basePosition=[1.6, 1.0, 0.0])
# BoxId = p.createMultiBody(100, colSphereId4, basePosition=[2.7, 1.0, 0.0])
reset_robot()
import pybullet as p
import time
p.connect(p.GUI)
planeUidA = p.loadURDF("plane_transparent.urdf", [0, 0, 0])
planeUid = p.loadURDF("plane_transparent.urdf", [0, 0, -1])
texUid = p.loadTexture("tex256.png")
p.changeVisualShape(planeUidA, -1, rgbaColor=[1, 1, 1, 0.5])
p.changeVisualShape(planeUid, -1, rgbaColor=[1, 1, 1, 0.5])
p.changeVisualShape(planeUid, -1, textureUniqueId=texUid)
width = 256
height = 256
pixels = [255] * width * height * 3
colorR = 0
colorG = 0
colorB = 0
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
#p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
blue = 0
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "renderbench.json")
for i in range(100000):
p.stepSimulation()
for i in range(width):
for j in range(height):
pixels[(i + j * width) * 3 + 0] = i
pixels[(i + j * width) * 3 + 1] = (j + blue) % 256
pixels[(i + j * width) * 3 + 2] = blue