def make_cargo(self):
orientation = pybullet.getQuaternionFromEuler([np.pi / 2, 0, 0])
cargo_shift = [0.0, 0.0, 0.325]
mesh_scale = [0.1, 0.1, 0.1]
visual_id = pybullet.createVisualShape(shapeType=pybullet.GEOM_SPHERE,
radius=0.1,\
rgbaColor=[1, 0, 1, 1],
specularColor=[0.8, .0, 0],
visualFrameOrientation=orientation,
meshScale=mesh_scale)
collision_id = pybullet.createCollisionShape(shapeType=pybullet.GEOM_SPHERE,
radius=0.1,\
halfExtents=[0.1, 0.1, 0.1],
collisionFrameOrientation=orientation,
meshScale=mesh_scale)
self.cargo_id = pybullet.createMultiBody(baseMass=0.1,\
baseCollisionShapeIndex=collision_id,\
baseVisualShapeIndex=visual_id,\
basePosition=cargo_shift)
pybullet.changeDynamics(self.cargo_id,
-1,
lateralFriction=self.k_friction)
pybullet.changeDynamics(self.cargo_id, -1, angularDamping=0.1)
pybullet.changeDynamics(self.cargo_id, -1, linearDamping=0.1)
def create_agent_from_obj(self,
visual_filename,
collision_filename,
scale=1.0,
mass=1.0,
pos=[0, 0, 0],
orient=[0, 0, 0, 1],
rgba=[1, 1, 1, 1],
maximal=False):
visual_shape = p.createVisualShape(shapeType=p.GEOM_MESH,
fileName=visual_filename,
meshScale=scale,
rgbaColor=rgba,
physicsClientId=self.id)
collision_shape = p.createCollisionShape(shapeType=p.GEOM_MESH,
fileName=collision_filename,
meshScale=scale,
physicsClientId=self.id)
body = p.createMultiBody(baseMass=mass,
baseCollisionShapeIndex=collision_shape,
baseVisualShapeIndex=visual_shape,
basePosition=pos,
baseOrientation=orient,
useMaximalCoordinates=maximal,
physicsClientId=self.id)
agent = Agent()
agent.init(body, self.id, self.np_random, indices=-1)
return agent
def create_target(self, human):
# Set target on mouth
mouth_pos = [0, -0.11, 0.03]
head_pos, head_orient = p.getLinkState(human,
23,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
target_pos, target_orient = p.multiplyTransforms(
head_pos,
head_orient,
mouth_pos, [0, 0, 0, 1],
physicsClientId=self.id) # 将mouth位置转换到相对头的位置
target_pos = np.array(target_pos)
sphere_visual = p.createVisualShape(shapeType=p.GEOM_SPHERE,
radius=0.01,
rgbaColor=[0, 1, 0, 1],
physicsClientId=self.id)
# target
target = p.createMultiBody(baseMass=0.0,
baseCollisionShapeIndex=-1,
baseVisualShapeIndex=sphere_visual,
basePosition=target_pos,
useMaximalCoordinates=False,
physicsClientId=self.id)
p.changeVisualShape(target,
-1,
rgbaColor=[0.9, 0.3, 0.3, 1],
physicsClientId=self.id)
return target, target_pos
def wall(x=0.,
y=0.,
z=0.,
width=1.,
height=1.,
thickness=0.01,
orientation=DEFAULT_ORI):
half_extents = [width / 2, thickness, height / 2]
position = [x, y, z + height / 2]
pos = [0, 0., 0.]
visual_shape_id = p.createVisualShape(shapeType=p.GEOM_BOX,
halfExtents=half_extents,
visualFramePosition=pos)
collision_shape_id = p.createCollisionShape(shapeType=p.GEOM_BOX,
halfExtents=half_extents,
collisionFramePosition=pos)
mb = p.createMultiBody(baseMass=0.,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collision_shape_id,
baseVisualShapeIndex=visual_shape_id,
basePosition=position,
baseOrientation=orientation,
useMaximalCoordinates=False)
p.changeVisualShape(mb,
-1,
rgbaColor=[1, 1, 1, 0.5],
specularColor=[0.4, 0.4, 0])
return mb
def create_body(
self,
shape=p.GEOM_CAPSULE,
radius=0,
length=0,
position_offset=[0, 0, 0],
orientation=[0, 0, 0, 1],
specular_color=[0.1, 0.1, 0.1],
rgba_Color=[0.8, 0.6, 0.4, 1],
): # 给身体不同部分创建不同颜色
visual_shape = p.createVisualShape(shape,
radius=radius,
length=length,
rgbaColor=rgba_Color,
specularColor=specular_color,
visualFramePosition=position_offset,
visualFrameOrientation=orientation,
physicsClientId=self.id) # 创建反射
collision_shape = p.createCollisionShape(
shape,
radius=radius,
height=length,
collisionFramePosition=position_offset,
collisionFrameOrientation=orientation,
physicsClientId=self.id)
# return uniqel id
return collision_shape, visual_shape
def create_bowl(self):
bowl_scale = 0.75
visual_filename = os.path.join(
self.directory, 'dinnerware',
'bowl_reduced_compressed.obj') # 重排列文件设置使导入更容易
collision_filename = os.path.join(self.directory, 'dinnerware',
'bowl_vhacd.obj')
bowl_visual = p.createVisualShape(shapeType=p.GEOM_MESH,
fileName=visual_filename,
meshScale=[bowl_scale] * 3,
physicsClientId=self.id)
bowl_collision = p.createCollisionShape(shapeType=p.GEOM_MESH,
fileName=collision_filename,
meshScale=[bowl_scale] * 3,
physicsClientId=self.id)
bowl_pos = np.array([-0.15, -0.55, 0.75]) + np.array([
self.np_random.uniform(-0.05, 0.05),
self.np_random.uniform(-0.05, 0.05), 0
])
bowl = p.createMultiBody(
baseMass=0.1,
baseCollisionShapeIndex=bowl_collision,
baseVisualShapeIndex=bowl_visual,
basePosition=bowl_pos,
baseOrientation=p.getQuaternionFromEuler([np.pi / 2.0, 0, 0],
physicsClientId=self.id),
baseInertialFramePosition=[0, 0.04 * bowl_scale, 0],
useMaximalCoordinates=False,
physicsClientId=self.id)
return bowl
def create_primitives(shapeType=2,
rgbaColor=[1, 1, 0, 1],
pos=[0, 0, 0],
radius=1,
length=2,
halfExtents=[0.5, 0.5, 0.5],
baseMass=1,
basePosition=[0, 0, 0]):
visualShapeId = p.createVisualShape(shapeType=shapeType,
rgbaColor=rgbaColor,
visualFramePosition=pos,
radius=radius,
length=length,
halfExtents=halfExtents)
collisionShapeId = p.createCollisionShape(shapeType=shapeType,
collisionFramePosition=pos,
radius=radius,
height=length,
halfExtents=halfExtents)
bodyId = p.createMultiBody(baseMass=baseMass,
baseInertialFramePosition=[0, 0, 0],
baseVisualShapeIndex=visualShapeId,
baseCollisionShapeIndex=collisionShapeId,
basePosition=basePosition,
useMaximalCoordinates=True)
return visualShapeId, collisionShapeId, bodyId
def create_targets(self):
self.target = None
self.target_visualshape = p.createVisualShape(shapeType=p.GEOM_SPHERE,
radius=0.1,
rgbaColor=[1, 0, 0, 1],
physicsClientId=self.client_ID)
self.update_targets()
def move_arm_test(env):
print(f"Running {__file__}::{move_arm_test.__name__}()")
con = SimConnection(env)
arm_ctrl = BasicController(con)
cmd = Command()
state = State()
arm_ctrl.execute(cmd, state)
print(state.tool_pose())
marker_visual_id = pb.createVisualShape(pb.GEOM_BOX,
halfExtents=[0.005, 0.005, 0.005],
rgbaColor=[1, 0, 0, 1])
#pb.createMultiBody(baseVisualShapeIndex=marker_visual_id, basePosition=state.tool_pose()[:3])
cmd.make(kind=UR_CMD_KIND_MOVE_TOOL_POSE,
target_position=Tool(-0.4, -0.4, 0.3, 0, math.pi, 0))
pb.createMultiBody(baseVisualShapeIndex=marker_visual_id,
basePosition=cmd.target_position()[:3])
arm_ctrl.execute(cmd, state)
while not state.is_goal_reached():
# st = time.time()
arm_ctrl.execute(cmd, state)
env.update()
# latency = time.time()-st
# leftover = 1/240. - latency
# if leftover > 0:
# time.sleep(leftover)
print("Passed.")
def _flag_reposition(self):
walk_target_x, walk_target_y, _ = self.robot.get_target_position()
self.flag = None
if self.gui and not self.config["display_ui"]:
self.visual_flagId = p.createVisualShape(p.GEOM_MESH, fileName=os.path.join(pybullet_data.getDataPath(), 'cube.obj'), meshScale=[0.5, 0.5, 0.5], rgbaColor=[1, 0, 0, 0])
self.last_flagId = p.createMultiBody(baseVisualShapeIndex=self.visual_flagId, baseCollisionShapeIndex=-1, basePosition=[walk_target_x, walk_target_y, 0.5])
def _lookup_model(ref):
""" Lookup collision shape from 3D-FUTURE models. """
if ref not in model_map:
return -1, -1
mid = model_map[ref]
model_file = (model_dir / mid / 'raw_model.obj')
# NOTE(ycho): Temporary workaround for degenerate model?
# if '39057a21-0a68-3494-8522-2e473dd6a38f' in str(model_file):
# return -1, -1
if not model_file.exists():
logging.warn('No such model file : {}'.format(model_file))
return -1, -1
# TODO(ycho): remove this flag ... only for visualization
# or leave it in? idk...
col_id = pb.createCollisionShape(
pb.GEOM_MESH, fileName=str(model_file),
meshScale=c['scale'],
# flags=pb.GEOM_FORCE_CONCAVE_TRIMESH,
physicsClientId=sim_id
)
vis_id = pb.createVisualShape(
pb.GEOM_MESH, fileName=str(model_file),
meshScale=c['scale'],
flags=pb.GEOM_FORCE_CONCAVE_TRIMESH,
physicsClientId=sim_id
)
return (col_id, vis_id)
def __init__(self, config, gpu_idx=0):
self.config = self.parse_config(config)
print(self.config["envname"])
assert (self.config["envname"] == self.__class__.__name__
or self.config["envname"] == "TestEnv")
CameraRobotEnv.__init__(self,
self.config,
gpu_idx,
scene_type="building",
tracking_camera=tracking_camera)
self.robot_introduce(Husky(self.config, env=self))
self.scene_introduce()
self.total_reward = 0
self.total_frame = 0
self.flag_timeout = 1
self.visualid = -1
self.lastid = None
self.gui = self.config["mode"] == "gui"
if self.gui:
self.visualid = p.createVisualShape(
p.GEOM_MESH,
fileName=os.path.join(pybullet_data.getDataPath(), 'cube.obj'),
meshScale=[0.2, 0.2, 0.2],
rgbaColor=[1, 0, 0, 0.7])
self.colisionid = p.createCollisionShape(
p.GEOM_MESH,
fileName=os.path.join(pybullet_data.getDataPath(), 'cube.obj'),
meshScale=[0.2, 0.2, 0.2])
self.lastid = None
self.obstacle_dist = 100
def __init__(
self,
width,
position,
orientation,
color=(0, 1, 0, 0.5),
**kwargs,
):
"""
Create a cube marker for visualization
Args:
width (float): Length of one side of the cube.
position: Position (x, y, z)
orientation: Orientation as quaternion (x, y, z, w)
color: Color of the cube as a tuple (r, b, g, q)
"""
self._kwargs = kwargs
self.shape_id = pybullet.createVisualShape(
shapeType=pybullet.GEOM_BOX,
halfExtents=[width / 2] * 3,
rgbaColor=color,
**self._kwargs,
)
self.body_id = pybullet.createMultiBody(
baseVisualShapeIndex=self.shape_id,
basePosition=position,
baseOrientation=orientation,
**self._kwargs,
)
def __init__(self,
position,
rotEuler,
rgb=(0, 0, 0),
collision=False,
size=1.0):
real_size = size * 0.125
position = (position[0], position[1], position[2] + real_size)
self.visual_id = p.createVisualShape(
p.GEOM_BOX,
rgbaColor=list(rgb) + [1],
halfExtents=[real_size, real_size, real_size])
if collision:
self.collision_id = p.createCollisionShape(
p.GEOM_BOX, halfExtents=[real_size, real_size, real_size])
self.body = p.createMultiBody(
baseMass=0,
baseCollisionShapeIndex=self.collision_id,
baseVisualShapeIndex=self.visual_id,
basePosition=position,
baseOrientation=euler_to_quaternion(*rotEuler))
else:
self.body = p.createMultiBody(
baseMass=0,
baseVisualShapeIndex=self.visual_id,
basePosition=position,
baseOrientation=euler_to_quaternion(*rotEuler))
def _randomly_place_objects(self, objList):
"""Randomly places the objects in the bin.
"""
objectUids = []
shift = [0, -0.02, 0]
meshScale = [0.2, 0.2, 0.2]
list_x_pos = [0.55, 0.6, 0.65]
list_y_pos = [0.2, 0.05, 0.2]
for idx, obj_path in enumerate(objList):
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
fileName=obj_path,
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=shift,
meshScale=meshScale)
collisionShapeId = p.createCollisionShape(
shapeType=p.GEOM_MESH,
fileName=obj_path,
collisionFramePosition=shift,
meshScale=meshScale)
xpos = list_x_pos[idx]
ypos = list_y_pos[idx]
uid = p.createMultiBody(baseMass=1,
baseInertialFramePosition=[-0.2, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[xpos, ypos, .15],
useMaximalCoordinates=True)
objectUids.append(uid)
for _ in range(1000):
p.stepSimulation()
return objectUids
def build_the_wall(self):
colBoxId = p.createCollisionShape(
p.GEOM_BOX, halfExtents=[0.01, self.world_size / 2, 3])
visualShapeId = p.createVisualShape(
p.GEOM_BOX, halfExtents=[0.1, self.world_size / 2, 3])
bodyUid = p.createMultiBody(baseMass=0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=colBoxId,
baseVisualShapeIndex=visualShapeId,
basePosition=[
self.world_size - 0.5,
self.world_size * 0.5 - 0.5, 0
],
baseOrientation=[0, 0, 0, 1],
useMaximalCoordinates=True)
bodyUid = p.createMultiBody(
baseMass=0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=colBoxId,
baseVisualShapeIndex=visualShapeId,
basePosition=[-0.5, self.world_size * 0.5 - 0.5, 0],
baseOrientation=[0, 0, 0, 1],
useMaximalCoordinates=True)
bodyUid = p.createMultiBody(
baseMass=0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=colBoxId,
baseVisualShapeIndex=visualShapeId,
basePosition=[
self.world_size * 0.5 - 0.5, self.world_size - 0.5, 0
],
baseOrientation=[0, 0, 0.7071068, 0.7071068],
useMaximalCoordinates=True)
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 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 __init__(self, pos, rot, scale, z_scale=1):
self.z_scale = z_scale
obj_filepath = found_object_directories[np.random.choice(
np.arange(total_num_objects), 1)[0]]
mesh_scale = [0.01 * scale, 0.01 * scale, 0.01 * scale * z_scale]
visualShapeId = pb.createVisualShape(shapeType=pb.GEOM_MESH,
fileName=obj_filepath,
rgbaColor=[
np.random.random(),
np.random.random(),
np.random.random(), 1
],
meshScale=mesh_scale)
collisionShapeId = pb.createCollisionShape(shapeType=pb.GEOM_MESH,
fileName=obj_filepath,
meshScale=mesh_scale)
# collisionShapeId = pb.createCollisionShape(shapeType=pb.GEOM_BOX,
# halfExtents=[0.024*scale, 0.024*scale, 0.024*scale])
object_id = pb.createMultiBody(
baseMass=0.1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=pos,
baseOrientation=rot)
# pb.changeDynamics(object_id, -1, mass=0.1, lateralFriction=1.0, spinningFriction=0.0, rollingFriction=0.0)
super(RandomObject, self).__init__(constants.RANDOM, object_id)
def show_spheres(self, jp, scolors='rainbow'):
spheres = []
for s in self.sphere_configs:
loc = get_sphere_loc(jp, s[:-1])
spheres.append(Sphere(loc, s[-1]))
if scolors is None:
scolors = self.gen_sphere_colors()
ids = []
for sphere, color in zip(spheres, scolors):
obstacle_visual_id = p.createVisualShape(
shapeType=p.GEOM_SPHERE,
radius=sphere.radius,
rgbaColor=color,
physicsClientId=self.clid,
)
#obstacle_collision_id = p.createCollisionShape(
# shapeType=p.GEOM_SPHERE,
# radius=sphere.radius,
# physicsClientId=self.clid,
#)
obstacle_id = p.createMultiBody(
basePosition=sphere.center,
baseVisualShapeIndex=obstacle_visual_id,
#baseCollisionShapeIndex=obstacle_collision_id,
physicsClientId=self.clid,
)
ids.append(obstacle_id)
self.obstacle_ids.extend(ids)
return ids
def build_markers(num_markers):
marker_radius = 0.02
markers = []
for i in range(num_markers):
if (i == REF_NECK_JOINT_ID) or (i == REF_PELVIS_JOINT_ID)\
or (i in REF_HIP_JOINT_IDS):
col = [0, 0, 1, 1]
elif (i in REF_TOE_JOINT_IDS):
col = [1, 0, 0, 1]
else:
col = [0, 1, 0, 1]
virtual_shape_id = pybullet.createVisualShape(
shapeType=pybullet.GEOM_SPHERE,
radius=marker_radius,
rgbaColor=col)
body_id = pybullet.createMultiBody(
baseMass=0,
baseCollisionShapeIndex=-1,
baseVisualShapeIndex=virtual_shape_id,
basePosition=[0, 0, 0],
useMaximalCoordinates=True)
markers.append(body_id)
return markers
def __init__(
self,
shape_type,
position,
orientation,
color=(0, 1, 0, 0.5),
pybullet_client_id=0,
**kwargs,
):
"""
Create a cube marker for visualization
Args:
shape_type: Shape type of the object (e.g. pybullet.GEOM_BOX).
position: Position (x, y, z)
orientation: Orientation as quaternion (x, y, z, w)
kwargs: Keyword arguments that are passed to
pybullet.createVisualShape. Use this to specify
shape-type-specify parameters like the object size.
"""
self._pybullet_client_id = pybullet_client_id
self.shape_id = pybullet.createVisualShape(
shapeType=shape_type,
rgbaColor=color,
physicsClientId=self._pybullet_client_id,
**kwargs,
)
self.body_id = pybullet.createMultiBody(
baseVisualShapeIndex=self.shape_id,
basePosition=position,
baseOrientation=orientation,
physicsClientId=self._pybullet_client_id,
)
def _setup_experiment(self):
# add plane to push on (slightly below the base of the robot)
self.planeId = p.loadURDF("plane.urdf", [0, 0, 0], useFixedBase=True)
self._setup_gripper()
self.walls = []
if self.level == 0:
pass
elif self.level == 1:
half_extents = [0.2, 0.05, 0.3]
colId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=half_extents)
visId = p.createVisualShape(p.GEOM_BOX,
halfExtents=half_extents,
rgbaColor=[0.2, 0.2, 0.2, 0.8])
wallId = p.createMultiBody(
0,
colId,
visId,
basePosition=[0.6, 0.30, 0.2],
baseOrientation=p.getQuaternionFromEuler([0, 0, 1.1]))
p.changeDynamics(wallId, -1, lateralFriction=1)
self.walls.append(wallId)
for wallId in self.walls:
p.changeVisualShape(wallId, -1, rgbaColor=[0.2, 0.2, 0.2, 0.8])
self.set_camera_position([0, 0], yaw=113, pitch=-40)
self.state = self._obs()
self._draw_state()
# set gravity
p.setGravity(0, 0, -10)
def create_spheres(self,
radius=0.01,
mass=0.0,
batch_positions=[[0, 0, 0]],
visual=True,
collision=True,
rgba=[0, 1, 1, 1]):
sphere_collision = p.createCollisionShape(
shapeType=p.GEOM_SPHERE, radius=radius,
physicsClientId=self.id) if collision else -1
sphere_visual = p.createVisualShape(
shapeType=p.GEOM_SPHERE,
radius=radius,
rgbaColor=rgba,
physicsClientId=self.id) if visual else -1
last_sphere_id = p.createMultiBody(
baseMass=mass,
baseCollisionShapeIndex=sphere_collision,
baseVisualShapeIndex=sphere_visual,
basePosition=[0, 0, 0],
useMaximalCoordinates=False,
batchPositions=batch_positions,
physicsClientId=self.id)
spheres = []
for body in list(
range(last_sphere_id - len(batch_positions) + 1,
last_sphere_id + 1)):
sphere = Agent()
sphere.init(body, self.id, self.np_random, indices=-1)
spheres.append(sphere)
return spheres
def cap(width, wall_width=0.05,
position=[
0,
0,
0,
], orientation=[0, 0, 0]):
# create a cap that can be used to close corners or tunnels.
# By default it is a wall in the y-z-plane that can be rotated and translated to the desired position
# create a corner (cube open at neg. x and pos. z) as a building block of an obstacle parcour
half_extents = [wall_width / 2, width / 2 + wall_width, width / 2]
pos = [0, 0., 0.]
visual_shape_id = p.createVisualShape(shapeType=p.GEOM_BOX,
halfExtents=half_extents,
visualFramePosition=pos)
collision_shape_id = p.createCollisionShape(shapeType=p.GEOM_BOX,
halfExtents=half_extents,
collisionFramePosition=pos)
mb = p.createMultiBody(baseMass=0.,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collision_shape_id,
baseVisualShapeIndex=visual_shape_id,
basePosition=position,
baseOrientation=orientation,
useMaximalCoordinates=False)
p.changeVisualShape(mb,
-1,
rgbaColor=[1, 1, 1, 0.5],
specularColor=[0.4, 0.4, 0])
return mb
def create_sphere(self,
radius=0.01,
mass=0.0,
pos=[0, 0, 0],
visual=True,
collision=True,
rgba=[0, 1, 1, 1],
maximal_coordinates=False,
return_collision_visual=False):
sphere_collision = p.createCollisionShape(
shapeType=p.GEOM_SPHERE, radius=radius,
physicsClientId=self.id) if collision else -1
sphere_visual = p.createVisualShape(
shapeType=p.GEOM_SPHERE,
radius=radius,
rgbaColor=rgba,
physicsClientId=self.id) if visual else -1
if return_collision_visual:
return sphere_collision, sphere_visual
body = p.createMultiBody(baseMass=mass,
baseCollisionShapeIndex=sphere_collision,
baseVisualShapeIndex=sphere_visual,
basePosition=pos,
useMaximalCoordinates=maximal_coordinates,
physicsClientId=self.id)
sphere = Agent()
sphere.init(body, self.id, self.np_random, indices=-1)
return sphere
def create_elastic_ball(ball_x, ball_y, ball_z):
'''
Create an elastic ball in the simulation.
Returns the created ball's id.
'''
ball_initial_position = [ball_x, ball_y, ball_z]
ball_visual_id = p.createVisualShape(shapeType=p.GEOM_SPHERE,
radius=0.1,
rgbaColor=[0.75, 0.3, 0, 1],
specularColor=[0.4, .4, 0])
ball_collision_id = p.createCollisionShape(shapeType=p.GEOM_SPHERE,
radius=0.15)
# create the ball with visual, collision, and initial position
ball_id = p.createMultiBody(baseMass=1,
baseCollisionShapeIndex=ball_collision_id,
baseVisualShapeIndex=ball_visual_id,
basePosition=ball_initial_position)
p.changeDynamics(ball_id,
-1,
lateralFriction=0,
spinningFriction=0,
rollingFriction=0,
restitution=1.1)
return ball_id
def add_shape(self, track):
if track.has_shape is True:
if track.shape.type == PrimitiveShape.CYLINDER:
position = []
orientation = []
visual_shape_id = p.createVisualShape(
p.GEOM_CYLINDER,
radius=track.shape.dimensions[0],
length=track.shape.dimensions[1])
collision_shape_id = p.createCollisionShape(
p.GEOM_CYLINDER,
radius=track.shape.dimensions[0],
length=track.shape.dimensions[1])
entity_id = p.createMultiBody(
baseCollisionShapeIndex=collision_shape_id,
baseVisualShapeIndex=visual_shape_id,
basePosition=position,
baseOrientation=orientation,
flags=p.URDF_ENABLE_SLEEPING
or p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES)
if entity_id >= 0:
self.entity_id_map[track.id] = entity_id
self.reverse_entity_id_map[entity_id] = track.id
elif track.shape.type == PrimitiveShape.MESH:
pass
else:
raise NotImplementedError(
"Only cylinder shapes for unknown objects supported at the moment."
)
def putBox(self, x, y, length, width, height, mass, xPix,
yPix): # xPix and yPix are the pixel location to be placed
heightOffset = self.state[yPix, xPix]
statetest = np.array(self.state)
colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[
float(length) / 2,
float(width) / 2,
float(height) / 2
])
randomColor = boxColorCollection[self.boxcount]
visualBoxId = p.createVisualShape(p.GEOM_BOX,
rgbaColor=randomColor,
halfExtents=[
float(length) / 2,
float(width) / 2,
float(height) / 2
])
p.createMultiBody(mass, colBoxId, visualBoxId, [
x + float(length) / 2, y - float(width) / 2,
heightOffset + float(height) / 2
], [0, 0, 0, 1])
16,
20,
21,
22,
20,
22,
23
]
#p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=shift,
meshScale=meshScale,
vertices=vertices,
indices=indices,
uvs=uvs,
normals=normals)
collisionShapeId = p.createCollisionShape(
shapeType=p.GEOM_BOX, halfExtents=meshScale
) #MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale)
texUid = p.loadTexture("tex256.png")
batchPositions = []
for x in range(32):
for y in range(32):
for z in range(10):
import pybullet as p
import time
import pybullet_data
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf")
p.setGravity(0,0,-10)
visualShift = [0,0,0]
collisionShift = [0,0,0]
inertiaShift = [0,0,-0.5]
meshScale=[1,1,1]
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="cube.obj", rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=visualShift, meshScale=meshScale)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="cube.obj", collisionFramePosition=collisionShift,meshScale=meshScale)
p.createMultiBody(baseMass=1,baseInertialFramePosition=inertiaShift,baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [0,0,1], useMaximalCoordinates=False)
while (1):
p.stepSimulation()
time.sleep(1./240.)
corners3D=[]
imgW = int(width/10)
imgH = int(height/10)
img = p.getCameraImage(imgW,imgH, renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgbBuffer=img[2]
depthBuffer = img[3]
print("rgbBuffer.shape=",rgbBuffer.shape)
print("depthBuffer.shape=",depthBuffer.shape)
#disable rendering temporary makes adding objects faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_SPHERE, rgbaColor=[1,1,1,1], radius=0.03 )
collisionShapeId = -1 #p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="duck_vhacd.obj", collisionFramePosition=shift,meshScale=meshScale)
for i in range (4):
w = cornersX[i]
h = cornersY[i]
rayFrom,rayTo, _= getRayFromTo(w,h)
rf = np.array(rayFrom)
rt = np.array(rayTo)
vec = rt-rf
l = np.sqrt(np.dot(vec,vec))
newTo = (0.01/l)*vec+rf
#print("len vec=",np.sqrt(np.dot(vec,vec)))
p.addUserDebugLine(rayFrom,newTo,[1,0,0])
corners3D.append(newTo)
