def __init__(self, headless=False):
if headless:
self._client = p.connect(p.DIRECT)
else:
self._client = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, 0)
self.dt = 1./240.
# Load and texture plane with features
planeId = p.loadURDF('plane.urdf')
textureId = p.loadTexture('sim_files/blank-texture.png')
p.changeVisualShape(planeId, -1, textureUniqueId=textureId)
cubeId = p.loadURDF('cube.urdf', [0, 0, 1], p.getQuaternionFromEuler(
[0, 0, 0]), globalScaling=1.5)
textureId = p.loadTexture('sim_files/dot-texture.png')
p.changeVisualShape(cubeId, -1, textureUniqueId=textureId)
# Setup camera params
self.width = 450
self.height = 450
self.fov = 45.0
self.aspect = 1
self.nearVal = 0.1
self.farVal = 5.0
self._projMatrix = p.computeProjectionMatrixFOV(fov=self.fov,
aspect=self.aspect,
nearVal=self.nearVal,
farVal=self.farVal)
self.camPosition = np.array([-0.5, 0.5, 4.])
self.targetPosition = np.array([0., 0., 0.])
self.upVector = np.array([0., 1., 0.])
def loadModels(self):
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.setGravity(0, 0, -9.81)
# p.setPhysicsEngineParameter(numSolverIterations=self.numSolverIterations)
#p.setTimeStep(self.fixedTimeStep)
orn = p.getQuaternionFromEuler([math.pi / 30 * 0, 0 * math.pi / 50, 0])
p.setAdditionalSearchPath(pybullet_data.getDataPath())
planeUid = p.loadURDF("plane_transparent.urdf", [0, 0, 0], orn)
p.changeDynamics(planeUid, -1, lateralFriction=1)
texUid = p.loadTexture("concrete.png")
p.changeVisualShape(planeUid, -1, textureUniqueId=texUid)
if self.useStairs:
stairsUid = p.loadURDF("../urdf/stairs_gen.urdf.xml", [0, -1, 0],
orn)
flags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF(
"../urdf/spotmicroai_gen.urdf.xml",
self.init_position,
self.init_oritentation,
useFixedBase=self.useFixedBase,
useMaximalCoordinates=self.useMaximalCoordinates,
flags=flags) #p.URDF_USE_IMPLICIT_CYLINDER)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.changeDynamics(quadruped, -1, lateralFriction=0.8)
return quadruped
def add_aruco_tag(
self,
tag_id=0,
rgba=(0, 1, 0, 1),
aruco_dict_id=cv2.aruco.DICT_4X4_50,
):
aruco_dict = cv2.aruco.Dictionary_get(aruco_dict_id)
tag = cv2.aruco.drawMarker(aruco_dict,
tag_id,
aruco_dict.markerSize + 2,
borderBits=1)
tag = np.pad(tag, (1, 1), mode='constant', constant_values=255)
tag_size = tag.shape[0]
texture_img = np.zeros((4 * tag_size, 4 * tag_size, 3), dtype=np.uint8)
texture_img[..., :] = [255 * c for c in rgba[:3]]
# flip image so aruco tag appears properly
texture_img[-tag_size:, -tag_size:, :] = tag[::-1, :, np.newaxis]
img = Image.fromarray(texture_img, mode="RGB")
tmp_fname = f'tmp_cube_tex{tag_id}.png'
img.save(tmp_fname)
texture_id = pb.loadTexture(tmp_fname)
pb.changeVisualShape(self.id,
-1,
rgbaColor=(1, 1, 1, rgba[-1]),
textureUniqueId=texture_id,
physicsClientId=self.client)
os.remove(tmp_fname)
def make_box(self,
size,
position=[0, 0, 0],
orientation=[0, 0, 0, 1],
mass=0,
tex=None,
color=None,
tag=None,
**kwargs):
cid = pb.createCollisionShape(pb.GEOM_BOX,
halfExtents=np.array(size) * 0.5)
vid = pb.createVisualShape(pb.GEOM_BOX,
halfExtents=np.array(size) * 0.5)
id = pb.createMultiBody(mass,
baseCollisionShapeIndex=cid,
baseVisualShapeIndex=vid,
basePosition=position,
baseOrientation=orientation)
if color is not None:
pb.changeVisualShape(id, -1, rgbaColor=color)
if tex is not None:
if type(tex) is str:
tex = pb.loadTexture(tex)
pb.changeVisualShape(id, -1, textureUniqueId=tex)
if len(kwargs):
pb.changeDynamics(id, -1, **kwargs)
if tag is not None:
self.__tag_map[id] = tag
return id
def create_object(self):
random_id = random.randint(0, 999)
visual_id = pb.createVisualShape(
shapeType=pb.GEOM_MESH,
fileName=f'random_urdfs/{random_id:03}/{random_id:03}.obj',
rgbaColor=None,
meshScale=[0.1, 0.1, 0.1])
collision_id = pb.createCollisionShape(
shapeType=pb.GEOM_MESH,
fileName=f'random_urdfs/{random_id:03}/{random_id:03}_coll.obj',
meshScale=[0.1, 0.1, 0.1])
link_id = pb.createMultiBody(
baseMass=1.0,
baseCollisionShapeIndex=visual_id,
baseVisualShapeIndex=collision_id,
basePosition=[0, 0, 3],
baseOrientation=pb.getQuaternionFromEuler([0, 0, 0]))
texture_paths = glob.glob(os.path.join('dtd', '**', '*.jpg'), recursive=True)
random_texture_path = texture_paths[random.randint(0, len(texture_paths) - 1)]
texture_id = pb.loadTexture(random_texture_path)
pb.changeVisualShape(link_id, -1, textureUniqueId=texture_id)
self._loaded_objects.append(link_id)
def add_mesh(self,
path="./data/chair/",
mesh_name="chair.obj",
tex_name="diffuse.tga",
position=[0, 0, 1],
orientation=[0.7071068, 0, 0, 0.7071068],
scale=0.1):
shift = [0, -0.02, 0]
meshScale = [scale, scale, scale]
# the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
fileName=path + mesh_name,
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=shift,
meshScale=meshScale)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
fileName=path + mesh_name,
collisionFramePosition=shift,
meshScale=meshScale)
rangex = 5
rangey = 5
bodyUid = p.createMultiBody(baseMass=0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=position,
baseOrientation=orientation,
useMaximalCoordinates=True)
if tex_name != None:
texUid = p.loadTexture(path + tex_name)
p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid)
return bodyUid
def _reset(self):
#print("KukaGymEnv _reset")
self.terminated = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
texUid = p.loadTexture("./cube_new/aaa.png")
cube_objects = p.loadSDF("./cube_new/model.sdf")
self.cubeId = cube_objects[0]
p.changeVisualShape(cube_objects[0], -1,rgbaColor =[1,1,1,1])
p.changeVisualShape(cube_objects[0], -1, textureUniqueId = texUid)
p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])
self.tray = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"tray/tray.urdf"), 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000)
self.table = p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-0.820000,0.000000,0.000000,0.0,1.0)
#print("table:::",self.table)
#print("tray:::",self.tray)
#This is where the object spawn at random i should adjust this so it spawn at random position with specific area bound if reward increased
xpos = 0.58
ypos = 0.04
ang = 3.14*0.5
orn = p.getQuaternionFromEuler([0,0,ang])
self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.15,orn[0],orn[1],orn[2],orn[3])
#This where robot is reset i should modefy this to adjust height of end-effector
p.setGravity(0,0,-10)
self._kuka_hand = Kuka_Handlit(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array([self._observation])
def load_urdf(self):
"""Load the URDF of the field into the environment
The field URDF comes with its own dimensions and
textures, collidables. Note that this also calls
the method responsible for setting button joint IDs.
"""
self.model_id = p.loadURDF(Utilities.gen_urdf_path("field/field.urdf"),
useFixedBase=True)
# Load the texture as well
# Not done automatically for some reason
p.changeVisualShape(self.model_id,
-1,
textureUniqueId=p.loadTexture(
Utilities.gen_urdf_path("field/field.png")))
self.buttons.populate_joint_ids([1, 3, 5, 7, 9, 11, 13, 15, 17, 19])
# TODO: replace with setJointMotorControlArray
for b in self.buttons:
p.setJointMotorControl2(self.model_id,
b.joint_id,
controlMode=p.POSITION_CONTROL,
targetPosition=0.0,
force=0.2,
positionGain=0.8)
def make_ball(self,
radius,
position=[0, 0, 0],
mass=0,
tex=None,
color=None,
tag=None,
**kwargs):
cid = pb.createCollisionShape(pb.GEOM_SPHERE, radius=radius)
vid = pb.createVisualShape(pb.GEOM_SPHERE, radius=radius)
id = pb.createMultiBody(mass,
baseCollisionShapeIndex=cid,
baseVisualShapeIndex=vid,
basePosition=position)
if color is not None:
pb.changeVisualShape(id, -1, rgbaColor=color)
if tex is not None:
if type(tex) is str:
tex = pb.loadTexture(tex)
pb.changeVisualShape(id, -1, textureUniqueId=tex)
if len(kwargs):
pb.changeDynamics(id, -1, **kwargs)
if tag is not None:
self.__tag_map[id] = tag
return id
def load_rope_geometry_from_urdf(modelUrdfFile, texturePath=""):
print("Loading urdf '{}'".format(modelUrdfFile))
if (not os.path.exists(modelUrdfFile)):
print("Failed to find model paths '{}'".format(modelUrdfFile))
return RopeObj.INVALID_UID, np.array([])
multiBodyId = ""
if (modelUrdfFile.endswith(ModelFormats.Urdf)):
multiBodyId = p.loadURDF(modelUrdfFile)
print("(INFO) Loaded new model '{}'".format(multiBodyId))
else:
print("Unknown model format '{}'".format(modelUrdfFile))
return RopeObj.INVALID_UID, np.array([])
# Override textures
if (texturePath != ""):
p.changeVisualShape(multiBodyId, -1, textureUniqueId=p.loadTexture(texturePath))
link_poses, _, _ = link_states(multiBodyId, p.getNumJoints(multiBodyId))
linkPositions = link_poses[:, 0:3]
if len(linkPositions) > 0:
print("(INFO) Found {} links and {} joints".format(len(linkPositions), p.getNumJoints(multiBodyId)))
else:
print("Failed to get link states.")
return RopeObj.INVALID_UID, np.array([])
return multiBodyId, np.array(linkPositions)
def LoadModel(modelGeometryFile, modelCollisionFile="", texturePath=""):
print("Loading urdf")
if (not os.path.exists(modelGeometryFile)
and not os.path.exists(modelCollisionFile)):
print("Failed to find model paths")
return
meshScale = [0.1, 0.1, 0.1]
visId = p.createVisualShape(shapeType=p.GEOM_MESH,
fileName=modelGeometryFile,
rgbaColor=None,
meshScale=meshScale)
colId = p.createCollisionShape(shapeType=p.GEOM_MESH,
fileName=modelCollisionFile,
meshScale=meshScale)
multiBodyId = p.createMultiBody(baseMass=1.0,
baseCollisionShapeIndex=colId,
baseVisualShapeIndex=visId,
basePosition=MODEL_INITIAL_POS,
baseOrientation=p.getQuaternionFromEuler(
[0, 0, 0]))
# Override textures
p.changeVisualShape(multiBodyId,
-1,
textureUniqueId=p.loadTexture(textPath))
return
def apply_texture(self, obj_id, patternPath="envs/dtd/images"):
"""
Apply texture to pybullet object
Parameters:
:param obj_id: (int) ID obtained from `p.loadURDF/SDF/..()`
:param path: (str) Relative path to *.jpg (recursive) with textures
"""
if patternPath is None:
return
if patternPath == self.textures_path: # no change, can use cached value
texture_paths = self.all_textures_
else:
pp = os.path.abspath(os.path.join(repodir, str(patternPath)))
texture_paths = glob.glob(os.path.join(pp, '**', '*.jpg'),
recursive=True)
random_texture_path = texture_paths[random.randint(
0,
len(texture_paths) - 1)]
textureId = p.loadTexture(random_texture_path)
try:
p.changeVisualShape(obj_id,
-1,
rgbaColor=[1, 1, 1, 1],
textureUniqueId=textureId)
except:
print("Failed to apply texture to obj ID:" + str(obj_id) +
" from path=" + str(pp))
def load_scene_mesh(self):
"""
Load scene mesh
"""
filename = os.path.join(get_scene_path(self.scene_id),
"mesh_z_up_downsampled.obj")
if not os.path.isfile(filename):
filename = os.path.join(get_scene_path(self.scene_id),
"mesh_z_up.obj")
collision_id = p.createCollisionShape(
p.GEOM_MESH, fileName=filename, flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
if self.pybullet_load_texture:
visual_id = p.createVisualShape(p.GEOM_MESH, fileName=filename)
else:
visual_id = -1
self.mesh_body_id = p.createMultiBody(
baseCollisionShapeIndex=collision_id,
baseVisualShapeIndex=visual_id)
p.changeDynamics(self.mesh_body_id, -1, lateralFriction=1)
if self.pybullet_load_texture:
texture_filename = get_texture_file(filename)
if texture_filename is not None:
texture_id = p.loadTexture(texture_filename)
p.changeVisualShape(self.mesh_body_id,
-1,
textureUniqueId=texture_id)
def random_texture(fig):
texture_paths = glob.glob(os.path.join('dtd', '**', '*.jpg'),
recursive=True)
random_texture_path = texture_paths[random.randint(0,
len(texture_paths) - 1)]
textureId = p.loadTexture(random_texture_path)
p.changeVisualShape(fig, -1, textureUniqueId=textureId)
def get_texture(self):
if len(VisualRandomizer.loaded_textures
) < VisualRandomizer.max_textures:
random_texture = random.choice(os.listdir(self.texture_dir))
VisualRandomizer.loaded_textures.append(
p.loadTexture(os.path.join(self.texture_dir, random_texture)))
return VisualRandomizer.loaded_textures[-1]
else:
return random.choice(VisualRandomizer.loaded_textures)
def init_table(self):
table_path = os.path.join(self.env_root, 'urdf/table/table.urdf')
self.table_id = p.loadURDF(
table_path, [0.42, 0, 0], [0, 0, math.pi * 0.32, 1],
globalScaling=0.6) #,physicsClientId=self.physical_id)
texture_path = os.path.join(
self.env_root, 'texture/table_textures/table_texture.jpg')
table_textid = p.loadTexture(texture_path)
p.changeVisualShape(self.table_id, -1, textureUniqueId=table_textid)
def draw_bullet(shape, color, size, r, c, data):
p.resetSimulation()
#p.setRealTimeSimulation(True)
p.setGravity(0, 9.8, GRAVITY)
p.setTimeStep(dt)
planeId = p.loadURDF("plane.urdf")
sizes = ['small', 'big']
colors = ['red', 'green', 'blue']
shapes = ['sphere', 'cube', 'capsule']
sizename = sizes[size]
shapename = shapes[shape]
colorname = colors[color]
body_name = 'shapes/bullet_data/my_{}_{}_{}.urdf'.format(
shapename, colorname, sizename)
if size == 0:
factor = 0.5
else:
factor = 1
if shape == 2:
body_name = 'shapes/bullet_data/my_{}_{}.urdf'.format(
shapename, sizename)
z_centre = 0.5 * factor
cubeStartPos = [(r - 1), (c - 1), z_centre]
cubeStartOrientation = p.getQuaternionFromEuler([3.14 / 2, 0, 0])
botId = p.loadURDF(body_name, cubeStartPos, cubeStartOrientation)
if shape == 2:
textureId = p.loadTexture(
'shapes/bullet_data/{}.png'.format(colorname))
p.changeVisualShape(botId, -1, textureUniqueId=textureId)
viewMatrix = p.computeViewMatrix(cameraEyePosition=[1.5, 1.5, 2],
cameraTargetPosition=[0, 0, 0],
cameraUpVector=[-1, -1, 1])
projectionMatrix = p.computeProjectionMatrixFOV(fov=90.0,
aspect=1.0,
nearVal=0.1,
farVal=30.1)
import time
p.setRealTimeSimulation(1)
st = 1500
p.setGravity(0, 0, GRAVITY)
width, height, rgbImg, depthImg, segImg = p.getCameraImage(
width=30,
height=30,
viewMatrix=viewMatrix,
projectionMatrix=projectionMatrix)
data = np.array(rgbImg)
return data
def add_obj(path_to_obj,
position, orientation,
fix_base, scale,
base_mass, base_inertialframe_position,
texture_file=None, rgb=None):
'''
path_to_obj: str
orientation: np.ndarray, (3,3) or (4,)
scale: float
fix_base: bool
texture_file: str or None
rgb: (float, float, float) or (3,) np.ndarray
'''
R = orientation
assert isinstance(R, np.ndarray)
if R.shape == (3,3):
quat = R2quat(R)
else:
assert len(R) == 4
quat = R
collision_shape_id = pb.createCollisionShape(shapeType=pb.GEOM_MESH, meshScale=np.full(3, scale), fileName=path_to_obj)
visual_shape_id = pb.createVisualShape(shapeType=pb.GEOM_MESH, meshScale=np.full(3, scale), fileName=path_to_obj)
if fix_base:
# simply omitting baseMass makes it fixed object.
bullet_body_id = pb.createMultiBody(
basePosition=position, baseOrientation=quat,
baseCollisionShapeIndex=collision_shape_id, baseVisualShapeIndex=visual_shape_id,
)
else:
bullet_body_id = pb.createMultiBody(
baseMass=base_mass, baseInertialFramePosition=base_inertialframe_position,
basePosition=position, baseOrientation=quat,
baseCollisionShapeIndex=collision_shape_id, baseVisualShapeIndex=visual_shape_id,
)
if texture_file is not None:
texture_id = pb.loadTexture(texture_file)
else:
texture_id = None
if rgb is not None:
rgba = [c for c in rgb] + [1.]
else:
rgba = None
if texture_id is not None and rgba is not None:
pb.changeVisualShape(bullet_body_id, -1, textureUniqueId=texture_id, rgbaColor=rgba)
elif texture_id is not None and rgba is None:
pb.changeVisualShape(bullet_body_id, -1, textureUniqueId=texture_id)
elif texture_id is None and rgba is not None:
pb.changeVisualShape(bullet_body_id, -1, rgbaColor=rgba)
return bullet_body_id
def from_array(bitmap, client_id):
""" Load a texture from memory and return a non-negative
texture unique id if the loading succeeds. """
# PyBullet can only load textures from file, so ...
with tempfile.NamedTemporaryFile() as tmp_file:
image = Image.fromarray(bitmap, 'RGB')
image.save(tmp_file, 'PNG')
tmp_file.flush()
return pb.loadTexture(tmp_file.name, physicsClientId=client_id)
def set_marker(self, dir_path, file, pic_file):
tmp = os.getcwd()
os.chdir(dir_path)
# c = p.loadURDF(file, (0, 0, 1), useFixedBase=True)
c = p.loadURDF(file, (0, 1, 0), useFixedBase=True, globalScaling=0.1)
# for i in range(1, 6):
# p.loadURDF(file, (0.2*i, 1, 0), useFixedBase=True, globalScaling=0.1)
os.chdir(tmp)
x = p.loadTexture(pic_file)
p.changeVisualShape(c, -1, textureUniqueId=x)
def floor(clientId, texture=False, wall=False):
floorId = p.loadURDF('env/model/floor.urdf', physicsClientId=clientId)
if texture:
textureId = p.loadTexture(
'env/model/texture/wood1.jpg', physicsClientId=clientId)
p.changeVisualShape(
floorId, -1, textureUniqueId=textureId, physicsClientId=clientId)
if wall:
p.loadURDF('samurai.urdf', physicsClientId=clientId)
return floorId
def add_drone(self, RobotController, name, image):
if self.get_drone_by_name(name) is not None:
raise Exception(f'drone with name "{name}" already exists')
try:
# create instance of controller
controller = RobotController(self.enforce_motor_limits)
# get color
color = controller.get_color()
assert (len(color) == 3)
color.append(1.)
# get label
if image is not None:
texture_id = pybullet.loadTexture(image)
# load urdf
id = pybullet.loadURDF(
os.path.join('.', 'urdf', 'drone.urdf'),
basePosition=np.array([0., 0., 0.3]),
baseOrientation=pybullet.getQuaternionFromEuler([0., 0., 0.]),
useFixedBase=0,
flags=(pybullet.URDF_USE_IMPLICIT_CYLINDER
| pybullet.URDF_USE_INERTIA_FROM_FILE))
# apply color and label
pybullet.changeVisualShape(id, -1, rgbaColor=color)
if image is None:
pybullet.changeVisualShape(id, 1, rgbaColor=[1., 1., 1., 0.])
else:
pybullet.changeVisualShape(id,
1,
rgbaColor=[1., 1., 1., 0.75],
textureUniqueId=texture_id)
# set contact parameters
pybullet.changeDynamics(id,
-1,
lateralFriction=1.0,
spinningFriction=0.0,
rollingFriction=0.0,
restitution=0.5,
contactDamping=-1,
contactStiffness=-1)
self.drones.append({
'id': id,
'module': None,
'RobotController': RobotController,
'name': name,
'controller': controller,
})
except Exception as err:
print(f'Failed to add {name} because of the following error:')
print(f'\n==========\n{traceback.format_exc()}==========\n')
def load(self, parent):
self.parent = parent
num_planes = p.getNumJoints(parent)
self.joints = [-1] + list(range(num_planes))
self.textures = []
for j in self.joints:
p.changeVisualShape(parent, j, rgbaColor=[1, 1, 1, 1])
t = p.loadTexture(DATA_DIR + "tex256.png")
self.textures.append(t)
p.changeTexture(t, self.random_pixels(), self.w, self.h)
p.changeVisualShape(parent, j, textureUniqueId=t)
def main():
if len(sys.argv) > 1:
model_path = sys.argv[1]
else:
model_path = os.path.join(get_model_path('Rs'), 'mesh_z_up.obj')
p.connect(p.GUI)
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 240.)
# Load scenes
collision_id = p.createCollisionShape(p.GEOM_MESH,
fileName=model_path,
meshScale=1.0,
flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
visual_id = p.createVisualShape(p.GEOM_MESH,
fileName=model_path,
meshScale=1.0)
texture_filename = get_texture_file(model_path)
texture_id = p.loadTexture(texture_filename)
mesh_id = p.createMultiBody(baseCollisionShapeIndex=collision_id,
baseVisualShapeIndex=visual_id)
# Load robots
turtlebot_urdf = os.path.join(gibson2.assets_path,
'models/turtlebot/turtlebot.urdf')
robot_id = p.loadURDF(turtlebot_urdf,
flags=p.URDF_USE_MATERIAL_COLORS_FROM_MTL)
# Load objects
obj_visual_filename = os.path.join(
gibson2.assets_path,
'models/ycb/002_master_chef_can/textured_simple.obj')
obj_collision_filename = os.path.join(
gibson2.assets_path,
'models/ycb/002_master_chef_can/textured_simple_vhacd.obj')
collision_id = p.createCollisionShape(p.GEOM_MESH,
fileName=obj_collision_filename,
meshScale=1.0)
visual_id = p.createVisualShape(p.GEOM_MESH,
fileName=obj_visual_filename,
meshScale=1.0)
object_id = p.createMultiBody(baseCollisionShapeIndex=collision_id,
baseVisualShapeIndex=visual_id,
basePosition=[1.0, 0.0, 1.0],
baseMass=0.1)
for _ in range(10000):
p.stepSimulation()
p.disconnect()
def set_texture(body, image_path):
#if color is not None:
# set_color(body, color)
assert image_path.endswith('.png')
texture = p.loadTexture(image_path)
assert 0 <= texture
p.changeVisualShape(
body,
linkIndex=NULL_ID,
shapeIndex=NULL_ID, #rgbaColor=WHITE,
textureUniqueId=texture, #specularColor=(0, 0, 0),
physicsClientId=get_client())
return texture
def load_floor_planes(self):
if self.is_interactive:
for f in range(len(self.floors)):
# load the floor plane with the original floor texture for each floor
plane_name = os.path.join(get_model_path(self.model_id),
"plane_z_up_{}.obj".format(f))
collision_id = p.createCollisionShape(p.GEOM_MESH,
fileName=plane_name)
visual_id = p.createVisualShape(p.GEOM_MESH,
fileName=plane_name)
texture_filename = get_texture_file(plane_name)
if texture_filename is not None:
texture_id = p.loadTexture(texture_filename)
else:
texture_id = -1
floor_body_id = p.createMultiBody(
baseCollisionShapeIndex=collision_id,
baseVisualShapeIndex=visual_id)
if texture_id != -1:
p.changeVisualShape(floor_body_id,
-1,
textureUniqueId=texture_id)
floor_height = self.floors[f]
p.resetBasePositionAndOrientation(floor_body_id,
posObj=[0, 0, floor_height],
ornObj=[0, 0, 0, 1])
# Since both the floor plane and the scene mesh have mass 0 (static),
# PyBullet seems to have already disabled collision between them.
# Just to be safe, explicit disable collision between them.
p.setCollisionFilterPair(self.mesh_body_id,
floor_body_id,
-1,
-1,
enableCollision=0)
self.floor_body_ids.append(floor_body_id)
else:
# load the default floor plane (only once) and later reset it to different floor heiights
plane_name = os.path.join(pybullet_data.getDataPath(),
"mjcf/ground_plane.xml")
floor_body_id = p.loadMJCF(plane_name)[0]
p.resetBasePositionAndOrientation(floor_body_id,
posObj=[0, 0, 0],
ornObj=[0, 0, 0, 1])
p.setCollisionFilterPair(self.mesh_body_id,
floor_body_id,
-1,
-1,
enableCollision=0)
self.floor_body_ids.append(floor_body_id)
def init_plane(self):
self.plane_id = self.p.loadURDF(os.path.join(self.env_root,
'urdf/table/plane.urdf'),
[0.7, 0, 0],
[0, 0, -math.pi * 0.02, 1],
globalScaling=0.7)
texture_path = os.path.join(self.opt.project_root, 'scripts', 'Envs',
'texture/real_textures')
texture_file = os.path.join(
texture_path,
random.sample(os.listdir(texture_path), 1)[0])
self.textid = p.loadTexture(texture_file)
self.p.changeVisualShape(self.plane_id, -1, rgbaColor=[1, 1, 1, 0.9])
self.p.changeVisualShape(self.plane_id,
-1,
textureUniqueId=self.textid)
def __init__(self):
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.81)
planeId = p.loadURDF("plane.urdf")
# load models from URDF
lowerBallSpawnPos = [0, 0, 3]
robotSpawnPos = [0, 0, 8]
defaultOrientation = p.getQuaternionFromEuler([0, 0, 0])
self.lower_ball_id = p.loadURDF(
"../model_3d/urdf/lower_ball.urdf",
lowerBallSpawnPos,
defaultOrientation,
flags=p.URDF_USE_INERTIA_FROM_FILE)
self.robot_id = p.loadURDF(
"../model_3d/urdf/robot.urdf",
robotSpawnPos,
defaultOrientation,
flags=p.URDF_USE_INERTIA_FROM_FILE)
# extract joint indices
if p.getNumJoints(self.robot_id) != 2:
print("robot does not have 2 joints!")
exit()
name_to_joint_idx = {}
for i in range(p.getNumJoints(self.robot_id)):
name_to_joint_idx.update({p.getJointInfo(self.robot_id, i)[1]: i})
self.motor_x_idx = name_to_joint_idx['lateral_rotation_axis']
self.motor_y_idx = name_to_joint_idx['primary_rotation_axis']
# add texture for better visualization
texUid = p.loadTexture("../model_3d/urdf/media/circles.png")
p.changeVisualShape(self.lower_ball_id, -1, textureUniqueId=texUid)
p.changeVisualShape(self.robot_id, -1, textureUniqueId=texUid)
# load controller
param = ModelParam()
param.l = 1.0
param.r1 = 3.0
param.r2 = 2.0
self.controller = Controller(param)
def create_textured_square(size,
color=None,
width=MAX_TEXTURE_WIDTH,
height=MAX_TEXTURE_WIDTH):
body = load_model('models/square.urdf', scale=size)
if color is not None:
set_color(body, color)
path = os.path.join(TEMP_DIR, 'texture.png')
image = MAX_PIXEL_VALUE * np.ones((width, height, 3), dtype=np.uint8)
import scipy.misc
scipy.misc.imsave(path, image)
texture = p.loadTexture(path)
p.changeVisualShape(body,
NULL_ID,
textureUniqueId=texture,
physicsClientId=CLIENT)
return body, texture
def readTexture(self, texture):
'''Convert the texture into the color format taken by pybullet'''
if (texture[0] == '#'):
r = float(
self.hexToInt(texture[1]) * 16 +
self.hexToInt(texture[2])) / 255.0
g = float(
self.hexToInt(texture[3]) * 16 +
self.hexToInt(texture[4])) / 255.0
b = float(
self.hexToInt(texture[5]) * 16 +
self.hexToInt(texture[5])) / 255.0
color = [r, g, b, 1]
return color
else:
textId = p.loadTexture(texture)
return textId
import pybullet as p
from time import sleep
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("plane.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):
camera = p.getDebugVisualizerCamera()
viewMat = camera[2]
projMat = camera[3]
#An example of setting the view matrix for the projective texture.
#viewMat = p.computeViewMatrix(cameraEyePosition=[7,0,0], cameraTargetPosition=[0,0,0], cameraUpVector=[0,0,1])
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)
