def load_blocks(self, thing_below, num_bases=None):
self.num_blocks = len(thing_below)
self.num_bases = self.num_blocks if num_bases is None else num_bases
self.blocks = ["b%d" % b for b in range(self.num_blocks)]
self.bases = ["t%d" % t for t in range(self.num_bases)]
self.thing_below = dict(thing_below) # copy
self.block_above = self.invert(thing_below)
# # cube urdf path
# fpath = os.path.dirname(os.path.abspath(__file__)) + '/../../urdfs/objects'
# pb.setAdditionalSearchPath(fpath)
# re-usable collision shape
cube_mass = 2
cube_side = .02
half_exts = (cube_side * .5, ) * 3
if not hasattr(self, "cube_collision"):
self.cube_collision = pb.createCollisionShape(
pb.GEOM_BOX, halfExtents=half_exts)
colors = list(it.product([0, 1], repeat=3))
self.block_id = {}
for b, block in enumerate(self.blocks):
base, level = self.base_and_level_of(block)
height = .01 + level * .0201
pos, quat = self.placement_of(base)
pos = (pos[0], pos[1], height)
rgba = colors[b % len(colors)] + (1, )
# self.block_id[block] = pb.loadURDF(
# 'cube.urdf', basePosition=pos, baseOrientation=quat, useFixedBase=False)
# pb.changeVisualShape(self.block_id[block], linkIndex=-1, rgbaColor=rgba)
cube_visual = pb.createVisualShape(pb.GEOM_BOX,
halfExtents=half_exts,
rgbaColor=rgba)
self.block_id[block] = pb.createMultiBody(cube_mass,
self.cube_collision,
cube_visual,
basePosition=pos,
baseOrientation=quat)
self.step() # let blocks settle
def single_attack(self):
V = 3
robot_base_position = p.getBasePositionAndOrientation(self.biped_robot)
robot_base_position = robot_base_position[0]
robot_base_position = [
robot_base_position[0] + self.init_pos[0],
robot_base_position[1] + self.init_pos[1],
robot_base_position[2] + self.init_pos[2] - 0.3
]
Velocity = [
random.random() * 5 - 2.5,
random.random() * 5 - 2.5,
random.random() * 5 - 2.5
]
mass = self.attack_strength / sqrt(Velocity[0]**2 + Velocity[1]**2 +
Velocity[2]**2)
mass = mass / V
sphereRadius = 0.02 * random.randint(2, 4)
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,
radius=sphereRadius)
visualShapeId = -1
# basePosition = robot_base_position + Velocity
basePosition = [
robot_base_position[0] + Velocity[0],
robot_base_position[1] + Velocity[1],
robot_base_position[2] + Velocity[2]
]
baseOrientation = [0, 0, 0, 1]
sphereUid = p.createMultiBody(mass, colSphereId, visualShapeId,
basePosition, baseOrientation)
p.changeDynamics(sphereUid,
-1,
spinningFriction=0.001,
rollingFriction=0.001,
linearDamping=0.0,
contactStiffness=10000,
contactDamping=0)
p.resetBaseVelocity(sphereUid, [
-V * Velocity[0] * random.uniform(0.8, 1.2),
-V * Velocity[1] * random.uniform(0.8, 1.2),
-V * Velocity[2] * random.uniform(0.8, 1.2)
])
def __init__(self, robot_init_range: float,
ball_init_x: float, ball_init_y: float, ball_x_vel_range: float,
ball_y_vel: float, ball_radius: float, camera_height: float,
camera_angle: float, dt: float=(1/30.0), brick_texture='brick2.jpg', ball_color=(0, 1, 0), mode=pb.DIRECT, noise=0.0):
self._robot_init_range = robot_init_range
self._ball_init_x = ball_init_x
self._ball_x_vel_range = ball_x_vel_range
self._ball_init_y = ball_init_y
self._ball_y_vel = ball_y_vel
self._ball_radius = ball_radius
self._camera_height = camera_height
self._camera_angle = camera_angle
self._dt = dt
self._gravity = 9.8
self.device = pt.device('cpu')
self._noise = noise
pb.connect(mode)
pb.setGravity(0, 0, 0)
pb.setAdditionalSearchPath(pybullet_data.getDataPath())
self._text_id = pb.loadTexture(brick_texture)
self._back_id = pb.loadURDF('plane.urdf', globalScaling=4)
self._floor_id = pb.loadURDF('plane.urdf')
pb.changeVisualShape(self._back_id, -1, textureUniqueId=self._text_id)
self._ball_vis_id = pb.createVisualShape(shapeType=pb.GEOM_SPHERE,
radius=self._ball_radius,
rgbaColor=[ball_color[0], ball_color[1], ball_color[2], 1],
specularColor=[0.4, .4, 0],
visualFramePosition=[0, 0, 0])
self._ball_col_id = pb.createCollisionShape(shapeType=pb.GEOM_SPHERE,
radius=self._ball_radius,
collisionFramePosition=[0, 0, 0])
self._ball_id = pb.createMultiBody(baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=self._ball_col_id,
baseVisualShapeIndex=self._ball_vis_id,
basePosition=[0, 0, 0],
useMaximalCoordinates=True)
def load_agent(self):
xyz = [1, -1, 0]
orientation = [0, 1, 0, 1]
self.agent = pb.createCollisionShape(pb.GEOM_CYLINDER,
height=0.7,
radius=0.05)
self.agent_mb = pb.createMultiBody(1,
self.agent,
-1,
basePosition=xyz,
baseOrientation=orientation)
pb.changeVisualShape(self.agent, -1, rgbaColor=[1, 1, 0, 1])
self.agent_cid = pb.createConstraint(self.agent,
-1,
-1,
-1,
pb.JOINT_FIXED, [0, 0, 0],
[0, 0, 0],
xyz,
childFrameOrientation=orientation)
def __init__(self, width, position, orientation, color=(0, 1, 0, 0.5)):
"""
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.shape_id = pybullet.createVisualShape(
shapeType=pybullet.GEOM_BOX,
halfExtents=[width / 2] * 3,
rgbaColor=color,
)
self.body_id = pybullet.createMultiBody(
baseVisualShapeIndex=self.shape_id,
basePosition=position,
baseOrientation=orientation,
)
def load_agent(self):
obj = pb.getBasePositionAndOrientation(self.obj_to_classify)
target = obj[0]
xyz=[target[0] - 1.25, target[1], target[2]]
#xyz=[0,-1.25,0]
orientation = [0,1,0,1]
self.agent = pb.createCollisionShape(pb.GEOM_BOX,
halfExtents=[self.wandSide,
self.wandSide,
self.wandLength])
self.agent_mb= pb.createMultiBody(1,self.agent,-1,basePosition=xyz,
baseOrientation=orientation)
pb.changeVisualShape(self.agent_mb,-1,rgbaColor=[1,1,0,1])
self.agent_cid = pb.createConstraint(self.agent_mb,-1,-1,-1,pb.JOINT_FIXED,
[0,0,0],[0,0,0],xyz,
childFrameOrientation=orientation)
self.fov = 90
def _load(self):
if self.visual_shape == p.GEOM_BOX:
shape = p.createVisualShape(self.visual_shape,
rgbaColor=self.rgba_color,
halfExtents=self.half_extents,
visualFramePosition=self.initial_offset)
elif self.visual_shape in [p.GEOM_CYLINDER, p.GEOM_CAPSULE]:
shape = p.createVisualShape(self.visual_shape,
rgbaColor=self.rgba_color,
radius=self.radius,
length=self.length,
visualFramePosition=self.initial_offset)
else:
shape = p.createVisualShape(self.visual_shape,
rgbaColor=self.rgba_color,
radius=self.radius,
visualFramePosition=self.initial_offset)
body_id = p.createMultiBody(baseVisualShapeIndex=shape, baseCollisionShapeIndex=-1)
return body_id
def __createCollider(self, mass):
if self.model is None:
return
col_pos = [
self.model.position.x + self.col_local_pos[0],
self.model.position.y + self.col_local_pos[1],
self.model.position.z + self.col_local_pos[2]
]
self.colId = p.createCollisionShape(p.GEOM_CYLINDER,
radius=self.col_radius,
height=self.col_height)
boxId = p.createMultiBody(baseMass=mass,
baseCollisionShapeIndex=self.colId,
basePosition=col_pos)
p.changeDynamics(self.colId,
-1,
linearDamping=5.0,
lateralFriction=1,
restitution=0.0)
def _load_geometry(self):
shiftX = np.random.uniform(-0.5, 0.5)
shiftY = np.random.uniform(-0.5, 0.5)
shift = [shiftX, shiftY, -0.15]
meshScale = [0.15, 0.15, 0.1]
path = os.path.abspath(os.path.dirname(__file__))
self.groundColId = p.createCollisionShape(
shapeType=p.GEOM_MESH,
fileName=os.path.join(path, "ground.obj"),
collisionFramePosition=shift,
meshScale=meshScale,
flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
self.groundId = p.createMultiBody(
baseMass=0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=self.groundColId,
basePosition=[0, 0, 0],
useMaximalCoordinates=True)
def __init__(self, radius, position, color=(0, 1, 0, 0.5)):
"""
Create a sphere 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.shape_id = p.createVisualShape(
shapeType=p.GEOM_SPHERE,
radius=radius,
rgbaColor=color,
)
self.body_id = p.createMultiBody(
baseVisualShapeIndex=self.shape_id,
basePosition=position,
baseOrientation=[0, 0, 0, 1],
)
def load(self):
collision_id = p.createCollisionShape(p.GEOM_MESH, fileName=self.collision_filename)
visual_id = p.createVisualShape(p.GEOM_MESH, fileName=self.visual_filename)
body_id = p.createMultiBody(basePosition=[0, 0, 0],
baseMass=60,
baseCollisionShapeIndex=collision_id,
baseVisualShapeIndex=visual_id)
self.body_id = body_id
p.resetBasePositionAndOrientation(self.body_id, self.pos, [-0.5, -0.5, -0.5, 0.5])
self.cid = p.createConstraint(self.body_id,
-1,
-1,
-1,
p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
self.pos,
parentFrameOrientation=[-0.5, -0.5, -0.5,
0.5]) # facing x axis
return body_id
def _createHeightmap(self, **kwargs):
loadKeys = {
"shapeType":
pybullet.GEOM_MESH,
"fileName":
self.mesh_file,
"flags":
pybullet.GEOM_FORCE_CONCAVE_TRIMESH
| pybullet.GEOM_CONCAVE_INTERNAL_EDGE,
}
if "scale" in kwargs:
loadKeys.update({"meshScale": kwargs["scale"]})
# Create the terrain via the obj data
terrain_collision_shape = pybullet.createCollisionShape(**loadKeys)
# Create the heightmap
loadKeys = {
"baseMass": 0,
"baseCollisionShapeIndex": terrain_collision_shape,
"baseVisualShapeIndex": -1,
"basePosition": [0, 0, 0],
"baseOrientation": [0, 0, 0, 1],
}
if "basePosition" in kwargs:
loadKeys.update({"basePosition": kwargs["basePosition"]})
if "baseOrientation" in kwargs:
loadKeys.update({
"baseOrientation":
pybullet.getQuaternionFromEuler(kwargs["baseOrientation"])
})
self.id = pybullet.createMultiBody(**loadKeys)
if "texture" in kwargs:
self.texture_id = pybullet.loadTexture(kwargs["texture"])
print(self.texture_id)
pybullet.changeVisualShape(self.id, -1, -1, self.texture_id)
def _add_capsule_cylinder(self,
obj_type,
radius,
length,
color,
position,
orientation=None):
if orientation is None:
orientation = [0, 0, 0]
# Check for color and orientation
self.check_color_orientation(color, position, orientation)
orientation = p.getQuaternionFromEuler(orientation)
if not (radius > 0 and radius * 2 < MAX_OBJ_DIM):
raise AttributeError(
"Ivalid radius passed to _add_capsule_cylinder")
if not (length > 0 and length + radius <= MAX_OBJ_DIM):
raise AttributeError(
"Ivalid length passed to _add_capsule_cylinder")
valid_pos = lambda p: abs(p) + radius <= self._env_dim / 2
if not all(map(valid_pos, position)):
raise AttributeError(
"Ivalid position passed to _add_capsule_cylinder")
visual_shp = p.createVisualShape(obj_type,
radius=radius,
length=length,
rgbaColor=color)
collision_shp = p.createCollisionShape(obj_type,
radius=radius,
height=length)
obj_id = p.createMultiBody(MASS,
collision_shp,
visual_shp,
basePosition=position,
baseOrientation=orientation)
self.objects_id.append(obj_id)
return obj_id
def _place_single_object(self, urdfList):
objectUids = []
objectClasses = []
shift = [0, -0.02, 0]
# meshScale = [0.3, 0.3, 0.3]
meshScale = [0.6, 0.6, 0.6]
list_x_pos = []
list_y_pos = []
xpos = 0.5
ypos = 0.15
for idx, (urdf_path, class_name) in enumerate(urdfList):
# print(urdf_path)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
fileName=urdf_path,
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=shift,
meshScale=meshScale)
try:
collisionShapeId = p.createCollisionShape(
shapeType=p.GEOM_MESH,
fileName=urdf_path,
collisionFramePosition=shift,
meshScale=meshScale)
except:
print(urdf_path)
continue
uid = p.createMultiBody(baseMass=1,
baseInertialFramePosition=[-0.2, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[xpos, ypos, .11],
useMaximalCoordinates=True)
objectUids.append(uid)
objectClasses.append(class_name)
for _ in range(150):
p.stepSimulation()
return objectUids, objectClasses
def init_tool(self, robot, mesh_scale=[1]*3, pos_offset=[0]*3, orient_offset=[0, 0, 0, 1], left=True, maximal=False, alpha=1.0):
if left:
gripper_pos, gripper_orient = p.getLinkState(robot, 76 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 47 if self.robot_type=='baxter' else 8, computeForwardKinematics=True, physicsClientId=self.id)[:2]
else:
gripper_pos, gripper_orient = p.getLinkState(robot, 54 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 25 if self.robot_type=='baxter' else 8, computeForwardKinematics=True, physicsClientId=self.id)[:2]
transform_pos, transform_orient = p.multiplyTransforms(positionA=gripper_pos, orientationA=gripper_orient, positionB=pos_offset, orientationB=orient_offset, physicsClientId=self.id)
if self.task == 'scratch_itch':
tool = p.loadURDF(os.path.join(self.directory, 'scratcher', 'tool_scratch.urdf'), basePosition=transform_pos, baseOrientation=transform_orient, physicsClientId=self.id)
elif self.task == 'bed_bathing':
tool = p.loadURDF(os.path.join(self.directory, 'bed_bathing', 'wiper.urdf'), basePosition=transform_pos, baseOrientation=transform_orient, physicsClientId=self.id)
elif self.task == 'bite_transfer' and self.robot_type == "panda":
tool = p.loadURDF(os.path.join(self.directory, 'dinnerware', 'drop_fork.urdf'), basePosition=transform_pos, baseOrientation=transform_orient, physicsClientId=self.id)
elif self.task in ['drinking', 'scooping', 'feeding', 'arm_manipulation']:
if self.task == 'drinking':
visual_filename = os.path.join(self.directory, 'dinnerware', 'plastic_coffee_cup.obj')
collision_filename = os.path.join(self.directory, 'dinnerware', 'plastic_coffee_cup_vhacd.obj')
elif self.task in ['scooping', 'feeding']:
visual_filename = os.path.join(self.directory, 'dinnerware', 'spoon_reduced_compressed.obj')
collision_filename = os.path.join(self.directory, 'dinnerware', 'spoon_vhacd.obj')
elif self.task == 'arm_manipulation':
visual_filename = os.path.join(self.directory, 'arm_manipulation', 'arm_manipulation_scooper.obj')
collision_filename = os.path.join(self.directory, 'arm_manipulation', 'arm_manipulation_scooper_vhacd.obj')
tool_visual = p.createVisualShape(shapeType=p.GEOM_MESH, fileName=visual_filename, meshScale=mesh_scale, rgbaColor=[1, 1, 1, alpha], physicsClientId=self.id)
tool_collision = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName=collision_filename, meshScale=mesh_scale, physicsClientId=self.id)
tool = p.createMultiBody(baseMass=0.01, baseCollisionShapeIndex=tool_collision, baseVisualShapeIndex=tool_visual, basePosition=transform_pos, baseOrientation=transform_orient, useMaximalCoordinates=maximal, physicsClientId=self.id)
if left:
# Disable collisions between the tool and robot
for j in (range(71, 86) if self.robot_type == 'pr2' else [18, 20, 21, 22, 23] if self.robot_type == 'sawyer' else [47, 49, 50, 51, 52] if self.robot_type == 'baxter' else [7, 8, 9, 10, 11, 12, 13, 14]):
for tj in list(range(p.getNumJoints(tool, physicsClientId=self.id))) + [-1]:
p.setCollisionFilterPair(robot, tool, j, tj, False, physicsClientId=self.id)
# Create constraint that keeps the tool in the gripper
constraint = p.createConstraint(robot, 76 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 47 if self.robot_type=='baxter' else 8, tool, -1, p.JOINT_FIXED, [0, 0, 0], parentFramePosition=pos_offset, childFramePosition=[0, 0, 0], parentFrameOrientation=orient_offset, physicsClientId=self.id)
else:
# Disable collisions between the tool and robot
for j in (range(49, 64) if self.robot_type == 'pr2' else [18, 20, 21, 22, 23] if self.robot_type == 'sawyer' else [25, 27, 28, 29, 30] if self.robot_type == 'baxter' else [7, 8, 9, 10, 11, 12, 13, 14]):
for tj in list(range(p.getNumJoints(tool, physicsClientId=self.id))) + [-1]:
p.setCollisionFilterPair(robot, tool, j, tj, False, physicsClientId=self.id)
# Create constraint that keeps the tool in the gripper
constraint = p.createConstraint(robot, 54 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 25 if self.robot_type=='baxter' else 8, tool, -1, p.JOINT_FIXED, [0, 0, 0], parentFramePosition=pos_offset, childFramePosition=[0, 0, 0], parentFrameOrientation=orient_offset, physicsClientId=self.id)
p.changeConstraint(constraint, maxForce=500, physicsClientId=self.id)
return tool
def initSim(nbobj, rad, pos1):
# créé le sol
p.createCollisionShape(p.GEOM_PLANE)
basePlane = p.createMultiBody(0, 0)
# met une seed au rng
random.seed(time.time())
# random.seed("Thibault")
# créé la sphere qui sera le point centrale de la "Shape"
nbPop = 100
pop = []
for i in range(0, nbPop - 1):
body = p.createCollisionShape(p.GEOM_SPHERE, radius=rad * 4)
temp = s.Shape(body, pos1)
temp.createRandom(nbobj, rad)
idtmp = temp.createBody()
pop.append(temp)
pop[i].setId(idtmp)
for i in range(-1, nbobj):
for j in range(-1, nbobj):
for elem1 in range(0, nbPop - 1):
for elem2 in range(elem1 + 1, nbPop - 1):
p.setCollisionFilterPair(pop[elem1].getId(),
pop[elem2].getId(), i, j, 0)
print((i + 2) / (nbobj + 1))
# ==========================================================
# Truc lié a la simulation
# ==========================================================
for elem in pop:
for i in range(0, nbobj):
p.setJointMotorControl2(bodyUniqueId=elem.getId(),
jointIndex=i,
controlMode=p.VELOCITY_CONTROL,
targetVelocity=random.randint(0, 3))
# met la gravité
p.setGravity(0, 0, -9.81)
return pop
def make_heightfield(self, height_map=None):
if self.config["terrain_name"] == "flat":
return
if hasattr(self, 'terrain'):
p.removeBody(self.terrain, physicsClientId=self.client_ID)
if height_map is None:
heightfieldData = np.zeros(self.config["env_width"] *
self.config["max_steps"])
terrainShape = p.createCollisionShape(
shapeType=p.GEOM_HEIGHTFIELD,
meshScale=[
self.config["mesh_scale_lat"],
self.config["mesh_scale_lat"],
self.config["mesh_scale_vert"]
],
heightfieldTextureScaling=(self.config["env_width"] - 1) / 2,
heightfieldData=heightfieldData,
numHeightfieldRows=self.config["max_steps"],
numHeightfieldColumns=self.config["env_width"],
physicsClientId=self.client_ID)
else:
heightfieldData = height_map.ravel(order='F')
terrainShape = p.createCollisionShape(
shapeType=p.GEOM_HEIGHTFIELD,
meshScale=[
self.config["mesh_scale_lat"],
self.config["mesh_scale_lat"],
self.config["mesh_scale_vert"]
],
heightfieldTextureScaling=(self.config["env_width"] - 1) / 2,
heightfieldData=heightfieldData,
numHeightfieldRows=height_map.shape[0],
numHeightfieldColumns=height_map.shape[1],
physicsClientId=self.client_ID)
terrain = p.createMultiBody(0,
terrainShape,
physicsClientId=self.client_ID)
p.resetBasePositionAndOrientation(terrain, [0, 0, 0], [0, 0, 0, 1],
physicsClientId=self.client_ID)
return terrain
def ObjectSpawner():
print("Objectspawner")
p.connect(p.DIRECT)
cube_visual = p.createVisualShape(p.GEOM_BOX,
halfExtents=[0.125, 0.125, 0.125])
cube_collision = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[0.25, 0.25, 0.25])
cube_body = p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=cube_collision,
baseVisualShapeIndex=cube_visual,
basePosition=[5, 1, 0.725])
p.loadURDF("./urdf/table/table.urdf",
basePosition=[5, 1, 0],
globalScaling=1)
p.loadURDF("./urdf/chair/chair.urdf",
basePosition=[6, 1, 0],
globalScaling=1)
p.loadURDF("./urdf/chair/chair.urdf",
basePosition=[7, 1, 0],
globalScaling=1)
def __init__(self, config, obj_dir, forward=True):
p.resetSimulation()
self.obj_dir = obj_dir
self.objects = config
self.forward = forward
self.object_ids = []
self.types = []
self.scales = []
self.colors = []
p.setAdditionalSearchPath(pybullet_data.getDataPath())
for obj_params in self.objects:
self.object_ids.append(self.add_object(**obj_params))
plane_id = p.createCollisionShape(p.GEOM_MESH,
fileName="plane.obj",
meshScale=[10, 10, 10])
self.ground_id = p.createMultiBody(0.,
baseCollisionShapeIndex=plane_id,
basePosition=[0, 0, 0])
def _create_object(self, pybullet_client_id, **kwargs):
"""
:param pybullet_client_id: (list) specifies the pybullet client ids.
:param kwargs: (params) parameters for the goal shape creation.
:return: (tuple) the first position specifies the shape_id and the
second specifies the block id for pybullet.
"""
position = np.array(self._position)
position[-1] += WorldConstants.FLOOR_HEIGHT
shape_id = pybullet.createVisualShape(
shapeType=pybullet.GEOM_BOX,
halfExtents=self._size / 2,
rgbaColor=np.append(self._color, self._alpha),
physicsClientId=pybullet_client_id)
block_id = pybullet.createMultiBody(baseVisualShapeIndex=shape_id,
basePosition=position,
baseOrientation=self._orientation,
physicsClientId=pybullet_client_id)
return shape_id, block_id
def _initialize_dataset(self):
for i, (obj_id, model_path, obj_com) in enumerate(
zip(self.dataset.obj_ids, self.dataset.collider_paths,
self.dataset.obj_coms)):
try:
collision_shape = pybullet.createCollisionShape(
shapeType=pybullet.GEOM_MESH,
fileName=model_path,
meshScale=[self.dataset.obj_scale] * 3)
model = pybullet.createMultiBody(
baseMass=1.0,
baseVisualShapeIndex=-1,
baseCollisionShapeIndex=collision_shape,
basePosition=[i * 0.5, 0, 0],
baseOrientation=[1, 0, 0, 0],
baseInertialFramePosition=obj_com)
self.models[obj_id] = model
except Exception as _:
self.models.clear()
raise ValueError("Could not load models for simulation.")
def __init__(self, n_spheres, r_min, r_max, duration, physics_client_id):
self.duration = duration
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,
radius=0.001,
physicsClientId=physics_client_id)
self.list_of_spheres = []
for i in range(n_spheres):
r = r_min + (r_max - r_min) * i / float(n_spheres - 1)
visSphereId = p.createVisualShape(
p.GEOM_SPHERE, radius=r, physicsClientId=physics_client_id)
sphere_id = p.createMultiBody(0,
colSphereId,
visSphereId, [0, 0, 100],
physicsClientId=physics_client_id)
sdl = p.getVisualShapeData(sphere_id,
physicsClientId=physics_client_id)
p.changeVisualShape(sphere_id,
sdl[0][1],
rgbaColor=[1, 1, 0.8, 0.5],
physicsClientId=physics_client_id)
self.list_of_spheres.append(sphere_id)
def update_targets(self):
if self.target is None:
self.target = np.array(
self.config["target_spawn_mu"]) + np.random.rand(2) * np.array(
self.config["target_spawn_sigma"]) - np.array(
self.config["target_spawn_sigma"]) / 2
self.target_body = p.createMultiBody(
baseMass=0,
baseVisualShapeIndex=self.target_visualshape,
basePosition=[self.target[0], self.target[1], 0],
physicsClientId=self.client_ID)
else:
self.target = np.array(
self.config["target_spawn_mu"]) + np.random.rand(2) * np.array(
self.config["target_spawn_sigma"]) - np.array(
self.config["target_spawn_sigma"]) / 2
p.resetBasePositionAndOrientation(
self.target_body, [self.target[0], self.target[1], 0],
[0, 0, 0, 1],
physicsClientId=self.client_ID)
def __init__(self, pos, rot, x_scale, y_scale, z_scale):
visualShapeId = pb.createVisualShape(shapeType=pb.GEOM_BOX,
halfExtents=[
0.05 / 2 * x_scale,
0.15 / 2 * y_scale,
0.05 / 2 * z_scale
])
collisionShapeId = pb.createCollisionShape(shapeType=pb.GEOM_BOX,
halfExtents=[
0.05 / 2 * x_scale,
0.15 / 2 * y_scale,
0.05 / 2 * z_scale
])
object_id = pb.createMultiBody(
baseMass=0.1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=pos,
baseOrientation=rot)
super(RandomBrick, self).__init__(constants.BRICK, object_id)
def __init__(
self,
number_of_goals,
goal_size=0.015,
initial_position=[0.18, 0.18, 0.08],
):
"""
Import a marker for visualization
Args:
number_of_goals (int): the desired number of goals to display
goal_size (float): how big should this goal be
initial_position (list of floats): where in xyz space should the
goal first be displayed
"""
color_cycle = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1]]
goal_shape_ids = [None] * number_of_goals
self.goal_ids = [None] * number_of_goals
self.goal_orientations = [None] * number_of_goals
# Can use both a block, or a sphere: uncomment accordingly
for i in range(number_of_goals):
color = color_cycle[i % len(color_cycle)]
goal_shape_ids[i] = pybullet.createVisualShape(
# shapeType=pybullet.GEOM_BOX,
# halfExtents=[goal_size] * number_of_goals,
shapeType=pybullet.GEOM_SPHERE,
radius=goal_size,
rgbaColor=color,
)
self.goal_ids[i] = pybullet.createMultiBody(
baseVisualShapeIndex=goal_shape_ids[i],
basePosition=initial_position,
baseOrientation=[0, 0, 0, 1],
)
(
_,
self.goal_orientations[i],
) = pybullet.getBasePositionAndOrientation(self.goal_ids[i])
def add_object(self,
shape,
mass=1,
init_pos=(0, 0, 1),
init_orn=(0, 0, 0),
scale=(1, 1, 1),
init_v=(0, 0, 0),
lat_fric=0.,
restitution=.9,
lin_damp=0,
angular_damp=0,
disappear_time=100000,
appear_time=0,
**kwargs):
"""
create an pybullet base object from a wavefront .obj file
set up initial parameters and physical properties
"""
scale = [x * y for x, y in zip(scale, SHAPE_DIMENSIONS[shape])]
shape = "cube"
obj_path = os.path.join(self.obj_dir, "shapes", '%s.obj' % shape)
init_orn_quat = p.getQuaternionFromEuler(deg2rad(init_orn))
col_id = p.createCollisionShape(p.GEOM_MESH,
fileName=obj_path,
meshScale=scale)
obj_id = p.createMultiBody(mass,
col_id,
basePosition=init_pos,
baseOrientation=init_orn_quat)
p.resetBaseVelocity(obj_id, linearVelocity=init_v)
p.changeDynamics(obj_id,
-1,
lateralFriction=lat_fric,
restitution=restitution,
linearDamping=lin_damp,
angularDamping=angular_damp)
self.init_positions.append(init_pos)
self.disappear_time.append(disappear_time)
self.appear_time.append(appear_time)
return obj_id
def _add_crate_floor(self):
"""Add a crate floor."""
x_length = self.crate_wall_width / 2
y_length = self.crate_wall_width / 2
z_length = self.crate_wall_thickness / 2
collision_shape_id = pb.createCollisionShape(
pb.GEOM_BOX, halfExtents=[x_length, y_length, z_length],
collisionFramePosition=[0, 0, 0], physicsClientId=self.client_id
)
visual_shape_id = -1
if self.client_mode == pb.GUI:
visual_shape_id = pb.createVisualShape(
pb.GEOM_BOX, halfExtents=[x_length, y_length, z_length],
visualFramePosition=[0, 0, 0], physicsClientId=self.client_id
)
multibody_id = pb.createMultiBody(
baseCollisionShapeIndex=collision_shape_id,
baseVisualShapeIndex=visual_shape_id,
physicsClientId=self.client_id
)
self.crate_side_ids.append(multibody_id)
def create_flying_body(group, collision_id=NULL_ID, visual_id=NULL_ID, mass=STATIC_MASS):
# TODO: more generally clone the body
indices = list(range(len(group) + 1))
masses = len(group) * [STATIC_MASS] + [mass]
visuals = len(group) * [NULL_ID] + [visual_id]
collisions = len(group) * [NULL_ID] + [collision_id]
types = [CARTESIAN_TYPES[joint][0] for joint in group] + [p.JOINT_FIXED]
#parents = [BASE_LINK] + indices[:-1]
parents = indices
assert len(indices) == len(visuals) == len(collisions) == len(types) == len(parents)
link_positions = len(indices) * [unit_point()]
link_orientations = len(indices) * [unit_quat()]
inertial_positions = len(indices) * [unit_point()]
inertial_orientations = len(indices) * [unit_quat()]
axes = len(indices) * [unit_point()]
axes = [CARTESIAN_TYPES[joint][1] for joint in group] + [unit_point()]
# TODO: no way of specifying joint limits
return p.createMultiBody(
baseMass=STATIC_MASS,
baseCollisionShapeIndex=NULL_ID,
baseVisualShapeIndex=NULL_ID,
basePosition=unit_point(),
baseOrientation=unit_quat(),
baseInertialFramePosition=unit_point(),
baseInertialFrameOrientation=unit_quat(),
linkMasses=masses,
linkCollisionShapeIndices=collisions,
linkVisualShapeIndices=visuals,
linkPositions=link_positions,
linkOrientations=link_orientations,
linkInertialFramePositions=inertial_positions,
linkInertialFrameOrientations=inertial_orientations,
linkParentIndices=parents,
linkJointTypes=types,
linkJointAxis=axes,
physicsClientId=CLIENT,
)
def build_scene():
map_label = np.zeros((MAP_SIZE, MAP_SIZE), dtype=np.uint8)
bodies = []
walls = np.random.sample((4, )) * MAX_WALL_INNESS
wall_pixels = np.ceil(walls / PIXEL_FRACTION).astype(np.int)
map_label[wall_pixels[0]:MAP_SIZE - wall_pixels[2],
wall_pixels[1]:MAP_SIZE - wall_pixels[3]] = 1
orients = [
p.getQuaternionFromEuler([0, 0, math.pi / 2]),
p.getQuaternionFromEuler([0, 0, 0])
]
for i in range(4):
offset = walls[i] - WALL_THICKNESS / 2
if i > 1:
offset = SCENE_SIZE - offset
pos = np.zeros((3, ))
pos[i % 2] = offset
obj = p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=wall_collision,
baseOrientation=orients[i % 2],
basePosition=pos)
bodies.append(obj)
colorize(obj)
for i in range(np.random.randint(6, 10)):
obj = insert_model(np.random.choice(model_names),
pos=random_pos(walls),
rot=np.random.random() * 2 * math.pi)
colorize(obj)
bodies.append(obj)
# for i in range(8):
# p.addUserDebugLine([0, 0, i / 4], [SCENE_SIZE, 0, i / 4], [0, 0, 1])
# p.addUserDebugLine([SCENE_SIZE, 0, i / 4], [SCENE_SIZE, SCENE_SIZE, i / 4], [0, 0, 1])
# p.addUserDebugLine([SCENE_SIZE, SCENE_SIZE, i / 4], [0, SCENE_SIZE, i / 4], [0, 0, 1])
# p.addUserDebugLine([0, SCENE_SIZE, i / 4], [0, 0, i / 4], [0, 0, 1])
return walls, map_label, bodies
def addCylinder(pos: list,
raidus: float,
length: float,
mass: float = 10000.,
rgba: list = [1., 1., 1., 1.],
physicsClientId: int = 0):
visual_shape = p.createVisualShape(p.GEOM_CYLINDER,
radius=raidus,
length=length,
rgbaColor=rgba,
physicsClientId=physicsClientId)
collision_shape = p.createCollisionShape(p.GEOM_CYLINDER,
radius=raidus,
height=length,
physicsClientId=physicsClientId)
entity_id = p.createMultiBody(
baseMass=mass,
baseCollisionShapeIndex=collision_shape,
baseVisualShapeIndex=visual_shape,
basePosition=[pos[0], pos[1], pos[2] + length / 2.],
physicsClientId=physicsClientId)
return entity_id
import pybullet as p import time p.connect(p.GUI) if (1): box_collision_shape_id = p.createCollisionShape( shapeType=p.GEOM_BOX, halfExtents=[0.01,0.01,0.055]) box_mass=0.1 box_visual_shape_id = -1 box_position=[0,0.1,1] box_orientation=[0,0,0,1] p.createMultiBody( box_mass, box_collision_shape_id, box_visual_shape_id, box_position, box_orientation, useMaximalCoordinates=True) terrain_mass=0 terrain_visual_shape_id=-1 terrain_position=[0,0,0] terrain_orientation=[0,0,0,1] terrain_collision_shape_id = p.createCollisionShape( shapeType=p.GEOM_MESH, fileName="terrain.obj", flags=p.GEOM_FORCE_CONCAVE_TRIMESH|p.GEOM_CONCAVE_INTERNAL_EDGE, meshScale=[0.5, 0.5, 0.5]) p.createMultiBody( terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id, terrain_position, terrain_orientation)
import pybullet as p
import time
useMaximalCoordinates = 0
p.connect(p.GUI)
#p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates)
monastryId = concaveEnv =p.createCollisionShape(p.GEOM_MESH,fileName="samurai_monastry.obj",flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
orn = p.getQuaternionFromEuler([1.5707963,0,0])
p.createMultiBody (0,monastryId, baseOrientation=orn)
sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
mass = 1
visualShapeId = -1
for i in range (5):
for j in range (5):
for k in range (5):
if (k&2):
sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1],useMaximalCoordinates=useMaximalCoordinates)
else:
sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1], useMaximalCoordinates=useMaximalCoordinates)
p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0)
p.setGravity(0,0,-10)
) #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):
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
useMaximalCoordinates = 0
p.connect(p.GUI)
p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates)
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setPhysicsEngineParameter(fixedTimeStep=1./120.)
sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
mass = 1
visualShapeId = -1
for i in range (5):
for j in range (5):
for k in range (5):
if (k&2):
sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1],useMaximalCoordinates=useMaximalCoordinates)
else:
sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1], useMaximalCoordinates=useMaximalCoordinates)
p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(1)
while (1):
time.sleep(0.01)
def createMultiBody(self, basePosition=[0,0,0],baseOrientation=[0,0,0,1], physicsClientId=0):
#assume link[0] is base
if (len(self.urdfLinks)==0):
return -1
#for i in range (len(self.urdfLinks)):
# print("link", i, "=",self.urdfLinks[i].link_name)
base = self.urdfLinks[0]
#v.tmp_collision_shape_ids=[]
baseMass = base.urdf_inertial.mass
baseCollisionShapeIndex = -1
baseShapeTypeArray=[]
baseRadiusArray=[]
baseHalfExtentsArray=[]
lengthsArray=[]
fileNameArray=[]
meshScaleArray=[]
basePositionsArray=[]
baseOrientationsArray=[]
for v in base.urdf_collision_shapes:
shapeType = v.geom_type
baseShapeTypeArray.append(shapeType)
baseHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]])
baseRadiusArray.append(v.geom_radius)
lengthsArray.append(v.geom_length)
fileNameArray.append(v.geom_meshfilename)
meshScaleArray.append(v.geom_meshscale)
basePositionsArray.append(v.origin_xyz)
orn=p.getQuaternionFromEuler(v.origin_rpy)
baseOrientationsArray.append(orn)
if (len(baseShapeTypeArray)):
#print("fileNameArray=",fileNameArray)
baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray,
radii=baseRadiusArray,
halfExtents=baseHalfExtentsArray,
lengths=lengthsArray,
fileNames=fileNameArray,
meshScales=meshScaleArray,
collisionFramePositions=basePositionsArray,
collisionFrameOrientations=baseOrientationsArray,
physicsClientId=physicsClientId)
urdfVisuals = base.urdf_visual_shapes
shapeTypes=[v.geom_type for v in urdfVisuals]
halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals]
radii=[v.geom_radius for v in urdfVisuals]
lengths=[v.geom_length for v in urdfVisuals]
fileNames=[v.geom_meshfilename for v in urdfVisuals]
meshScales=[v.geom_meshscale for v in urdfVisuals]
rgbaColors=[v.material_rgba for v in urdfVisuals]
visualFramePositions=[v.origin_xyz for v in urdfVisuals]
visualFrameOrientations=[p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals]
baseVisualShapeIndex = -1
if (len(shapeTypes)):
#print("len(shapeTypes)=",len(shapeTypes))
#print("len(halfExtents)=",len(halfExtents))
#print("len(radii)=",len(radii))
#print("len(lengths)=",len(lengths))
#print("len(fileNames)=",len(fileNames))
#print("len(meshScales)=",len(meshScales))
#print("len(rgbaColors)=",len(rgbaColors))
#print("len(visualFramePositions)=",len(visualFramePositions))
#print("len(visualFrameOrientations)=",len(visualFrameOrientations))
baseVisualShapeIndex = p.createVisualShapeArray(shapeTypes=shapeTypes,
halfExtents=halfExtents,radii=radii,lengths=lengths,fileNames=fileNames,
meshScales=meshScales,rgbaColors=rgbaColors,visualFramePositions=visualFramePositions,
visualFrameOrientations=visualFrameOrientations,physicsClientId=physicsClientId)
linkMasses=[]
linkCollisionShapeIndices=[]
linkVisualShapeIndices=[]
linkPositions=[]
linkOrientations=[]
linkInertialFramePositions=[]
linkInertialFrameOrientations=[]
linkParentIndices=[]
linkJointTypes=[]
linkJointAxis=[]
for joint in self.urdfJoints:
link = joint.link
linkMass = link.urdf_inertial.mass
linkCollisionShapeIndex=-1
linkVisualShapeIndex=-1
linkPosition=[0,0,0]
linkOrientation=[0,0,0]
linkInertialFramePosition=[0,0,0]
linkInertialFrameOrientation=[0,0,0]
linkParentIndex=self.linkNameToIndex[joint.parent_name]
linkJointType=joint.joint_type
linkJointAx = joint.joint_axis_xyz
linkShapeTypeArray=[]
linkRadiusArray=[]
linkHalfExtentsArray=[]
lengthsArray=[]
fileNameArray=[]
linkMeshScaleArray=[]
linkPositionsArray=[]
linkOrientationsArray=[]
for v in link.urdf_collision_shapes:
shapeType = v.geom_type
linkShapeTypeArray.append(shapeType)
linkHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]])
linkRadiusArray.append(v.geom_radius)
lengthsArray.append(v.geom_length)
fileNameArray.append(v.geom_meshfilename)
linkMeshScaleArray.append(v.geom_meshscale)
linkPositionsArray.append(v.origin_xyz)
linkOrientationsArray.append(p.getQuaternionFromEuler(v.origin_rpy))
if (len(linkShapeTypeArray)):
linkCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=linkShapeTypeArray,
radii=linkRadiusArray,
halfExtents=linkHalfExtentsArray,
lengths=lengthsArray,
fileNames=fileNameArray,
meshScales=linkMeshScaleArray,
collisionFramePositions=linkPositionsArray,
collisionFrameOrientations=linkOrientationsArray,
physicsClientId=physicsClientId)
urdfVisuals = link.urdf_visual_shapes
linkVisualShapeIndex = -1
shapeTypes=[v.geom_type for v in urdfVisuals]
halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals]
radii=[v.geom_radius for v in urdfVisuals]
lengths=[v.geom_length for v in urdfVisuals]
fileNames=[v.geom_meshfilename for v in urdfVisuals]
meshScales=[v.geom_meshscale for v in urdfVisuals]
rgbaColors=[v.material_rgba for v in urdfVisuals]
visualFramePositions=[v.origin_xyz for v in urdfVisuals]
visualFrameOrientations=[p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals]
if (len(shapeTypes)):
print("fileNames=",fileNames)
linkVisualShapeIndex = p.createVisualShapeArray(shapeTypes=shapeTypes,
halfExtents=halfExtents,radii=radii,lengths=lengths,
fileNames=fileNames,meshScales=meshScales,rgbaColors=rgbaColors,
visualFramePositions=visualFramePositions,
visualFrameOrientations=visualFrameOrientations,
physicsClientId=physicsClientId)
linkMasses.append(linkMass)
linkCollisionShapeIndices.append(linkCollisionShapeIndex)
linkVisualShapeIndices.append(linkVisualShapeIndex)
linkPositions.append(joint.joint_origin_xyz)
linkOrientations.append(p.getQuaternionFromEuler(joint.joint_origin_rpy))
linkInertialFramePositions.append(link.urdf_inertial.origin_xyz)
linkInertialFrameOrientations.append(p.getQuaternionFromEuler(link.urdf_inertial.origin_rpy))
linkParentIndices.append(linkParentIndex)
linkJointTypes.append(joint.joint_type)
linkJointAxis.append(joint.joint_axis_xyz)
obUid = p.createMultiBody(baseMass,\
baseCollisionShapeIndex=baseCollisionShapeIndex,
baseVisualShapeIndex=baseVisualShapeIndex,
basePosition=basePosition,
baseOrientation=baseOrientation,
baseInertialFramePosition=base.urdf_inertial.origin_xyz,
baseInertialFrameOrientation=p.getQuaternionFromEuler(base.urdf_inertial.origin_rpy),
linkMasses=linkMasses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=linkParentIndices,
linkJointTypes=linkJointTypes,
linkJointAxis=linkJointAxis,
physicsClientId=physicsClientId)
return obUid
p.connect(p.GUI)
p.setPhysicsEngineParameter(allowedCcdPenetration=0.0)
terrain_mass=0
terrain_visual_shape_id=-1
terrain_position=[0,0,0]
terrain_orientation=[0,0,0,1]
terrain_collision_shape_id = p.createCollisionShape(
shapeType=p.GEOM_MESH,
fileName="terrain.obj",
flags=p.GEOM_FORCE_CONCAVE_TRIMESH|p.GEOM_CONCAVE_INTERNAL_EDGE,
meshScale=[0.5, 0.5, 0.5])
p.createMultiBody(
terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id,
terrain_position, terrain_orientation)
useMaximalCoordinates = True
sphereRadius = 0.005
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
mass = 1
visualShapeId = -1
for i in range (5):
for j in range (5):
for k in range (5):
boxHalfLength = 0.5
boxHalfWidth = 2.5
boxHalfHeight = 0.1
segmentLength = 5
colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight])
mass = 1
visualShapeId = -1
segmentStart = 0
for i in range(segmentLength):
p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
basePosition=[segmentStart, 0, -0.1])
segmentStart = segmentStart - 1
for i in range(segmentLength):
height = 0
if (i % 2):
height = 1
p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
basePosition=[segmentStart, 0, -0.1 + height])
segmentStart = segmentStart - 1
baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.])
for i in range(segmentLength):
def createMultiBody(self, basePosition=[0,0,0],physicsClientId=0):
#assume link[0] is base
if (len(self.urdfLinks)==0):
return -1
base = self.urdfLinks[0]
#v.tmp_collision_shape_ids=[]
baseMass = base.urdf_inertial.mass
print("baseMass=",baseMass)
baseCollisionShapeIndex = -1
baseShapeTypeArray=[]
baseRadiusArray=[]
baseHalfExtentsArray=[]
lengthsArray=[]
fileNameArray=[]
meshScaleArray=[]
basePositionsArray=[]
baseOrientationsArray=[]
for v in base.urdf_collision_shapes:
print("base v.origin_xyz=",v.origin_xyz)
print("base v.origin_rpy=",v.origin_rpy)
shapeType = v.geom_type
baseShapeTypeArray.append(shapeType)
baseHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]])
baseRadiusArray.append(v.geom_radius)
lengthsArray.append(v.geom_length)
fileNameArray.append(v.geom_meshfilename)
meshScaleArray.append(v.geom_meshscale)
basePositionsArray.append(v.origin_xyz)
print("v.origin_rpy=",v.origin_rpy)
orn=p.getQuaternionFromEuler(v.origin_rpy)
baseOrientationsArray.append(orn)
print("baseHalfExtentsArray=",baseHalfExtentsArray)
if (len(baseShapeTypeArray)):
baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray,
radii=baseRadiusArray,
halfExtents=baseHalfExtentsArray,
lengths=lengthsArray,
fileNames=fileNameArray,
meshScales=meshScaleArray,
collisionFramePositions=basePositionsArray,
collisionFrameOrientations=baseOrientationsArray,
physicsClientId=physicsClientId)
print("baseCollisionShapeIndex=",baseCollisionShapeIndex)
linkMasses=[]
linkCollisionShapeIndices=[]
linkVisualShapeIndices=[]
linkPositions=[]
linkOrientations=[]
linkMeshScaleArray=[]
linkInertialFramePositions=[]
linkInertialFrameOrientations=[]
linkParentIndices=[]
linkJointTypes=[]
linkJointAxis=[]
for joint in self.urdfJoints:
link = joint.link
linkMass = link.urdf_inertial.mass
print("linkMass=",linkMass)
linkCollisionShapeIndex=-1
linkVisualShapeIndex=-1
linkPosition=[0,0,0]
linkOrientation=[0,0,0]
linkInertialFramePosition=[0,0,0]
linkInertialFrameOrientation=[0,0,0]
linkParentIndex=self.linkNameToIndex[joint.parent_name]
print("parentId=",linkParentIndex)
linkJointType=joint.joint_type
print("linkJointType=",linkJointType, self.jointTypeToName[linkJointType] )
linkJointAx = joint.joint_axis_xyz
linkShapeTypeArray=[]
linkRadiusArray=[]
linkHalfExtentsArray=[]
lengthsArray=[]
fileNameArray=[]
linkPositionsArray=[]
linkOrientationsArray=[]
for v in link.urdf_collision_shapes:
print("link v.origin_xyz=",v.origin_xyz)
print("link v.origin_rpy=",v.origin_rpy)
shapeType = v.geom_type
linkShapeTypeArray.append(shapeType)
linkHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]])
linkRadiusArray.append(v.geom_radius)
lengthsArray.append(v.geom_length)
fileNameArray.append(v.geom_meshfilename)
linkMeshScaleArray.append(v.geom_meshscale)
linkPositionsArray.append(v.origin_xyz)
linkOrientationsArray.append(p.getQuaternionFromEuler(v.origin_rpy))
print("linkHalfExtentsArray=",linkHalfExtentsArray)
if (len(linkShapeTypeArray)):
linkCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=linkShapeTypeArray,
radii=linkRadiusArray,
halfExtents=linkHalfExtentsArray,
lengths=lengthsArray,
fileNames=fileNameArray,
meshScales=linkMeshScaleArray,
collisionFramePositions=linkPositionsArray,
collisionFrameOrientations=linkOrientationsArray,
physicsClientId=physicsClientId)
linkMasses.append(linkMass)
linkCollisionShapeIndices.append(linkCollisionShapeIndex)
linkVisualShapeIndices.append(linkVisualShapeIndex)
linkPositions.append(joint.joint_origin_xyz)
linkOrientations.append(p.getQuaternionFromEuler(joint.joint_origin_rpy))
linkInertialFramePositions.append(link.urdf_inertial.origin_xyz)
linkInertialFrameOrientations.append(p.getQuaternionFromEuler(link.urdf_inertial.origin_rpy))
linkParentIndices.append(linkParentIndex)
linkJointTypes.append(joint.joint_type)
linkJointAxis.append(joint.joint_axis_xyz)
print("\n\n\nlinkMasses=",linkMasses)
obUid = p.createMultiBody(baseMass,\
baseCollisionShapeIndex= baseCollisionShapeIndex,
basePosition=basePosition,
baseInertialFramePosition=base.urdf_inertial.origin_xyz,
baseInertialFrameOrientation=base.urdf_inertial.origin_rpy,
linkMasses=linkMasses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=linkParentIndices,
linkJointTypes=linkJointTypes,
linkJointAxis=linkJointAxis,
physicsClientId=physicsClientId)
return obUid
count+=1
if ((count % 100)==0):
print(count,"out of ", imgW*imgH/(stepX*stepY))
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
import math
# This simple snake logic is from some 15 year old Havok C++ demo
# Thanks to Michael Ewert!
p.connect(p.GUI)
plane = p.createCollisionShape(p.GEOM_PLANE)
p.createMultiBody(0, plane)
useMaximalCoordinates = True
sphereRadius = 0.25
#colBoxId = p.createCollisionShapeArray([p.GEOM_BOX, p.GEOM_SPHERE],radii=[sphereRadius+0.03,sphereRadius+0.03], halfExtents=[[sphereRadius,sphereRadius,sphereRadius],[sphereRadius,sphereRadius,sphereRadius]])
colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[sphereRadius, sphereRadius, sphereRadius])
mass = 1
visualShapeId = -1
link_Masses = []
linkCollisionShapeIndices = []
linkVisualShapeIndices = []
linkPositions = []
linkOrientations = []
linkInertialFramePositions = []
linkInertialFrameOrientations = []
indices = []
jointTypes = []
axis = []
#visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, fileName="duck.obj")
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
vertices=vertices,
collisionFramePosition=shift,
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()
def createMultiBody(self, basePosition=[0,0,0],physicsClientId=0):
#assume link[0] is base
if (len(self.urdfLinks)==0):
return -1
base = self.urdfLinks[0]
#v.tmp_collision_shape_ids=[]
baseMass = base.urdf_inertial.mass
baseCollisionShapeIndex = -1
baseShapeTypeArray=[]
baseRadiusArray=[]
baseHalfExtentsArray=[]
lengthsArray=[]
fileNameArray=[]
meshScaleArray=[]
basePositionsArray=[]
baseOrientationsArray=[]
for v in base.urdf_collision_shapes:
shapeType = v.geom_type
baseShapeTypeArray.append(shapeType)
baseHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]])
baseRadiusArray.append(v.geom_radius)
lengthsArray.append(v.geom_length)
fileNameArray.append(v.geom_meshfilename)
meshScaleArray.append(v.geom_meshscale)
basePositionsArray.append(v.origin_xyz)
orn=p.getQuaternionFromEuler(v.origin_rpy)
baseOrientationsArray.append(orn)
if (len(baseShapeTypeArray)):
baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray,
radii=baseRadiusArray,
halfExtents=baseHalfExtentsArray,
lengths=lengthsArray,
fileNames=fileNameArray,
meshScales=meshScaleArray,
collisionFramePositions=basePositionsArray,
collisionFrameOrientations=baseOrientationsArray,
physicsClientId=physicsClientId)
urdfVisuals = base.urdf_visual_shapes
baseVisualShapeIndex = p.createVisualShapeArray(shapeTypes=[v.geom_type for v in urdfVisuals],
halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals],
radii=[v.geom_radius for v in urdfVisuals],
lengths=[v.geom_length[0] for v in urdfVisuals],
fileNames=[v.geom_meshfilename for v in urdfVisuals],
meshScales=[v.geom_meshscale for v in urdfVisuals],
rgbaColors=[v.material_rgba for v in urdfVisuals],
visualFramePositions=[v.origin_xyz for v in urdfVisuals],
visualFrameOrientations=[v.origin_rpy for v in urdfVisuals],
physicsClientId=physicsClientId)
# urdfVisual = base.urdf_visual_shapes[0]
# baseVisualShapeIndex = p.createVisualShape(shapeType=urdfVisual.geom_type,
# halfExtents=[ext * 0.5 for ext in urdfVisual.geom_extents],
# radius=urdfVisual.geom_radius,
# length=urdfVisual.geom_length[0],
# fileName=urdfVisual.geom_meshfilename,
# meshScale=urdfVisual.geom_meshscale,
# rgbaColor=urdfVisual.material_rgba,
# visualFramePosition=urdfVisual.origin_xyz,
# visualFrameOrientation=urdfVisual.origin_rpy,
# physicsClientId=physicsClientId)
linkMasses=[]
linkCollisionShapeIndices=[]
linkVisualShapeIndices=[]
linkPositions=[]
linkOrientations=[]
linkMeshScaleArray=[]
linkInertialFramePositions=[]
linkInertialFrameOrientations=[]
linkParentIndices=[]
linkJointTypes=[]
linkJointAxis=[]
for joint in self.urdfJoints:
link = joint.link
linkMass = link.urdf_inertial.mass
linkCollisionShapeIndex=-1
linkVisualShapeIndex=-1
linkPosition=[0,0,0]
linkOrientation=[0,0,0]
linkInertialFramePosition=[0,0,0]
linkInertialFrameOrientation=[0,0,0]
linkParentIndex=self.linkNameToIndex[joint.parent_name]
linkJointType=joint.joint_type
linkJointAx = joint.joint_axis_xyz
linkShapeTypeArray=[]
linkRadiusArray=[]
linkHalfExtentsArray=[]
lengthsArray=[]
fileNameArray=[]
linkPositionsArray=[]
linkOrientationsArray=[]
for v in link.urdf_collision_shapes:
shapeType = v.geom_type
linkShapeTypeArray.append(shapeType)
linkHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]])
linkRadiusArray.append(v.geom_radius)
lengthsArray.append(v.geom_length)
fileNameArray.append(v.geom_meshfilename)
linkMeshScaleArray.append(v.geom_meshscale)
linkPositionsArray.append(v.origin_xyz)
linkOrientationsArray.append(p.getQuaternionFromEuler(v.origin_rpy))
if (len(linkShapeTypeArray)):
linkCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=linkShapeTypeArray,
radii=linkRadiusArray,
halfExtents=linkHalfExtentsArray,
lengths=lengthsArray,
fileNames=fileNameArray,
meshScales=linkMeshScaleArray,
collisionFramePositions=linkPositionsArray,
collisionFrameOrientations=linkOrientationsArray,
physicsClientId=physicsClientId)
urdfVisuals = link.urdf_visual_shapes
linkVisualShapeIndex = p.createVisualShapeArray(shapeTypes=[v.geom_type for v in urdfVisuals],
halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals],
radii=[v.geom_radius for v in urdfVisuals],
lengths=[v.geom_length[0] for v in urdfVisuals],
fileNames=[v.geom_meshfilename for v in urdfVisuals],
meshScales=[v.geom_meshscale for v in urdfVisuals],
rgbaColors=[v.material_rgba for v in urdfVisuals],
visualFramePositions=[v.origin_xyz for v in urdfVisuals],
visualFrameOrientations=[v.origin_rpy for v in urdfVisuals],
physicsClientId=physicsClientId)
linkMasses.append(linkMass)
linkCollisionShapeIndices.append(linkCollisionShapeIndex)
linkVisualShapeIndices.append(linkVisualShapeIndex)
linkPositions.append(joint.joint_origin_xyz)
linkOrientations.append(p.getQuaternionFromEuler(joint.joint_origin_rpy))
linkInertialFramePositions.append(link.urdf_inertial.origin_xyz)
linkInertialFrameOrientations.append(p.getQuaternionFromEuler(link.urdf_inertial.origin_rpy))
linkParentIndices.append(linkParentIndex)
linkJointTypes.append(joint.joint_type)
linkJointAxis.append(joint.joint_axis_xyz)
obUid = p.createMultiBody(baseMass,\
baseCollisionShapeIndex=baseCollisionShapeIndex,
baseVisualShapeIndex=baseVisualShapeIndex,
basePosition=basePosition,
baseInertialFramePosition=base.urdf_inertial.origin_xyz,
baseInertialFrameOrientation=base.urdf_inertial.origin_rpy,
linkMasses=linkMasses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=linkParentIndices,
linkJointTypes=linkJointTypes,
linkJointAxis=linkJointAxis,
physicsClientId=physicsClientId)
return obUid
import pybullet as p
import time
p.connect(p.GUI)
useCollisionShapeQuery = True
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
geom = p.createCollisionShape(p.GEOM_SPHERE, radius=0.1)
geomBox = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2, 0.2, 0.2])
baseOrientationB = p.getQuaternionFromEuler([0, 0.3, 0]) #[0,0.5,0.5,0]
basePositionB = [1.5, 0, 1]
obA = -1
obB = -1
obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom, basePosition=[0.5, 0, 1])
obB = p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=geomBox,
basePosition=basePositionB,
baseOrientation=baseOrientationB)
lineWidth = 3
colorRGB = [1, 0, 0]
lineId = p.addUserDebugLine(lineFromXYZ=[0, 0, 0],
lineToXYZ=[0, 0, 0],
lineColorRGB=colorRGB,
lineWidth=lineWidth,
lifeTime=0)
pitch = 0
yaw = 0
while (p.isConnected()):
pitch += 0.01
if (pitch >= 3.1415 * 2.):
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.)
p.resetSimulation()
meshScale = [.1, .1, .01]
shift = [0, 0, 0]
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
fileName="marble_cube.obj",
rgbaColor=[1, 1, 1, 1],
specularColor=[1, 1, 1],
visualFramePosition=shift,
meshScale=meshScale)
#collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="textures/marble_cube.obj", collisionFramePosition=shift,meshScale=meshScale)
collisionShapeId = -1
uiCube = p.createMultiBody(baseMass=0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[0, 1, 0],
useMaximalCoordinates=True)
textOrn = p.getQuaternionFromEuler([0, 0, -1.5707963])
numLines = 1
lines = [-1] * numLines
p.stepSimulation()
#disable rendering during loading makes it much faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
#objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
kitchenObj = p.loadSDF("kitchens/1.sdf")
#objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [
p.loadURDF("pr2_gripper.urdf", 0.500000, 0.300006, 0.700000, -0.000000, -0.000000, -0.000031,
