def step_3d_simulation_shooter(player_body,
bullet_body,
goals,
bodies,
nsteps=1,
physicsClientId=0):
done = False
hit = False
reward = 0
for i in range(nsteps):
p.stepSimulation(physicsClientId=physicsClientId)
bullet_aabb = p.getAABB(bullet_body, physicsClientId=physicsClientId)
obs = p.getOverlappingObjects(*bullet_aabb,
physicsClientId=physicsClientId)
if obs is not None:
obs = [ob[0] for ob in obs]
for ob in obs:
if ob in goals:
x, y = np.random.uniform(0, 10, size=(2))
vx, vy = np.random.uniform(-20.0, 20.0, size=[2])
p.resetBasePositionAndOrientation(
ob, [x, y, 3.0], [0, 0, 0, 1],
physicsClientId=physicsClientId)
p.resetBaseVelocity(ob, [vx, vy, 0],
physicsClientId=physicsClientId)
reward += 1
hit = True
elif ob in bodies:
x, y = np.random.uniform(0, 10, size=(2))
vx, vy = np.random.uniform(-20.0, 20.0, size=[2])
p.resetBasePositionAndOrientation(
ob, [x, y, 3.0], [0, 0, 0, 1],
physicsClientId=physicsClientId)
p.resetBaseVelocity(ob, [vx, vy, 0],
physicsClientId=physicsClientId)
reward -= 1
hit = True
if hit:
p.resetBasePositionAndOrientation(bullet_body, [30, 30, 0.5],
[0, 0, 0, 1],
physicsClientId=physicsClientId)
vel, _ = p.getBaseVelocity(player_body, physicsClientId=physicsClientId)
vel = vel[:2]
tot = sum([abs(v) for v in list(vel)])
if tot > 1:
done = True
else:
done = False
return done, reward
def check_collisions(self):
"""Проверяет объект робота на пересечения с объектами внешней
среды и самопересечения
Возвращает true, если существует хотя бы одно пересечение
Возвращает false, если пересечений нет"""
# множество объектов, для которых необходима обработка
# в узкой фазе
obj_set = set()
# широкая фаза
for i in range(-1, pb.getNumJoints(self._id, physicsClientId=self.s)):
# координаты AABB
box_min, box_max = pb.getAABB(self._id, i, physicsClientId=self.s)
# getOverlappingObjects возвращает кортежи,
# первый элемент которых -- id объекта,
# а второй -- id "звеньев" объекта
# нужен только первый элемент
ids = [
obj[0] for obj in pb.getOverlappingObjects(
box_min, box_max, physicsClientId=self.s)
]
obj_set |= set(ids)
# узкая фаза
contacts = []
for o in obj_set:
contacts += pb.getContactPoints(self._id,
o,
physicsClientId=self.s)
return len(contacts) > 0
def test_overlap(self, xmin, ymin, zmin, xmax, ymax, zmax):
""" Return True if the aabb is in contact with an other object
"""
contacts = p.getOverlappingObjects([xmin, ymin, zmin], [xmax, ymax, zmax])
if contacts is not None:
if len(contacts) > 0:
return True
return False
def CheckContact(self, player):
aabbMin, aabbMax = p.getAABB(player.colId, -1)
collision_list = p.getOverlappingObjects(aabbMin, aabbMax)
if collision_list is not None and len(collision_list) != 0:
for objId in collision_list:
colId = objId[0]
if colId != 0 and colId in self.finalScene.activatedBodies:
self.firstFinalContact = True
break
def checkContact(self, obj_list):
aabbMin, aabbMax = p.getAABB(self.colId, -1)
collision_list = p.getOverlappingObjects(aabbMin, aabbMax)
if collision_list is not None and len(collision_list) != 0:
for objId in collision_list:
obj = obj_list.get(str(objId[0]))
if objId[0] != self.colId and objId[
0] != self.lastContactId and obj is not None:
self.lastContactId = objId[0]
self.onContact(obj)
def get_overlapping_objects(body_or_link):
""" Return all the unique ids of objects that have axis aligned
bounding box overlap with a axis aligned bounding box of
given body/link. """
client_id = body_or_link.client_id
kwargs = _compute_args(body_or_link, 'bodyUniqueId', 'linkIndex')
aa, bb = pb.getAABB(physicsClientId=client_id, **kwargs)
obs = pb.getOverlappingObjects(aa, bb, physticsClient=client_id)
if obs is None:
return []
return [link.Link(o[0], o[1]) for o in obs]
def get_overlapping_nodes(self, scene_node, margin=0.0):
if scene_node.id in self.nodes_map:
nodes_overlapping = []
sim_id = self.entity_id_map[scene_node.id]
aabb = p.getAABB(sim_id)
sim_ids_overlapping = p.getOverlappingObjects(aabb[0], aabb[1])
for sim_id in sim_ids_overlapping:
object_id = self.reverse_entity_id_map[sim_id]
nodes_overlapping.append(self.nodes_map[object_id])
return True, nodes_overlapping
return False, []
def _isHitEverything(self, drone_id):
a, b = p.getAABB(drone_id, physicsClientId=self.CLIENT
) # Melihat batas posisi collision drone ke i
list_obj = p.getOverlappingObjects(
a, b, physicsClientId=self.CLIENT
) # Melihat objek2 yang ada di batas posisi collision
if (list_obj != None
and len(list_obj) > 6): # 1 Quadcopter memiliki 6 link/bagian
# print("Drone {}: _isHitEverything".format(drone_id))
return True
return False
def get_closest_objects(self, radius):
pos = self.get_pos()
bboxmin = pos - radius
bboxmax = pos + radius
ids = p.getOverlappingObjects(aabbMin=bboxmin.tolist(),
aabbMax=bboxmax.tolist(),
physicsClientId=self.sim.id)
objects = [self.env.object_dict.get(uid[0], None) for uid in ids]
objects = list(
filter(
lambda obj: self.get_dist(obj, MAX_DIST=radius)['distance'].
shape[0] != 0, objects))
return objects
def ratio_beads_in_cup(self, cup=None):
if cup is None:
cup = self.cupID
aabbMin, aabbMax = p.getAABB(cup)
all_overlapping = p.getOverlappingObjects(aabbMin, aabbMax)
if all_overlapping is None:
return 0
overlapping_objects = [obj for obj in all_overlapping if obj[0] >= self.droplets[0] and obj[0] <= self.droplets[
-1]] # in the range to be a droplet
if overlapping_objects is None:
num_in = 0
else:
num_in = len(overlapping_objects)
total = len(self.droplets)
ratio = num_in / total
assert (ratio <= 1.0)
return ratio
def get_bodies_in_region(aabb):
"""This query will return all the unique ids of objects that have axis aligned bounding box overlap with a given axis aligned bounding box.
Note that the query is conservative and may return additional objects that don't have actual AABB overlap.
This happens because the acceleration structures have some heuristic that enlarges the AABBs a bit
(extra margin and extruded along the velocity vector).
Parameters
----------
aabb : [type]
[description]
Returns
-------
a list of object unique ids.
"""
(lower, upper) = aabb
bodies = p.getOverlappingObjects(lower, upper, physicsClientId=CLIENT)
return [] if bodies is None else bodies
def get_overlapping(self, aabb, filter=set()):
"""Returns all objects overlapping the given bounding box.
:param aabb: Axis aligned bounding box to check against.
:type aabb: AABB
:param filter: All objects in this set get filtered from the results.
:type filter: set
:rtype: list
"""
raw_overlap = pb.getOverlappingObjects(
vec3_to_list(aabb.min),
vec3_to_list(aabb.max),
physicsClientId=self.__client_id)
if raw_overlap is None:
return []
return [
self.__get_obj_link_tuple(bulletId, linkIdx)
for bulletId, linkIdx in raw_overlap
if self.bodies[self.__bId_IdMap[bulletId]] not in filter
]
def get_map(map_label):
for x in range(MAP_SIZE):
for y in range(MAP_SIZE):
if map_label[x, y] == 0:
continue
aabbMin = np.array([PIXEL_FRACTION * x, PIXEL_FRACTION * y, 0])
aabbMax = aabbMin + pixel_extents
objs = p.getOverlappingObjects(aabbMin, aabbMax)
if objs is None or len(objs) == 0:
continue
box = p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=pixel_collision,
basePosition=aabbMin)
for obj in objs:
pts = p.getClosestPoints(box, obj[0], 0.0)
if pts is not None and len(pts) > 0:
map_label[x, y] = 0
break
p.removeBody(box)
# print('Row {} done'.format(x))
return map_label
def estimate(self, track):
if track.id not in self.objects_state:
self.objects_action_state[track.id] = ObjectActionState.UNKNOWN
self.objects_state[track.id] = ObjectState.UNKNOWN
self.simulator.add_shape(track)
else:
# evaluate consistency => is it lying on something ?
sim_id = self.simulator.entity_id_map[track.id]
aabb = p.getAABB(sim_id)
aabb_min = aabb[0]
aabb_max = aabb[1]
if aabb_min[3] > 0.035:
aabb_max[3] = aabb_min[3]
aabb_min[3] -= 0.035
if aabb_min[3] < 0:
aabb_min[3] = 0
overlaping_objects = p.getOverlappingObjects(
aabb_min, aabb_max)
if len(overlaping_objects) == 0:
self.objects_state[track.id] = ObjectState.INCONSISTENT
else:
self.objects_state[track.id] = ObjectState.PLAUSIBLE
else:
self.objects_state[track.id] = ObjectState.PLAUSIBLE
filename = rospkg.RosPack().get_path("pybullet_sandbox") + "/data/mug/mug.urdf"
boxId = p.loadURDF(filename)
box_pose = p.getBasePositionAndOrientation(boxId)
sphere_path = rospkg.RosPack().get_path(
"pybullet_sandbox") + "/data/sphere.urdf"
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
# sphere packing algorithm taken from wiki: https://en.wikipedia.org/wiki/Close-packing_of_equal_spheres
r = 0.002
p1 = 5
d = 3
print 4 * p1 * p1 * (d)
for i in range(-p1, p1):
for j in range(-p1, p1):
for k in range(0, d):
x = r * (2 * i + (j + k) % 2)
y = r * sqrt(3) * (j + (k % 2) / 3.0)
z = r * sqrt(6) * 2 * k / 3.0
p.loadURDF(sphere_path, [x, y, z + 0.01])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.setTimeStep(0.010)
print "starting sim"
while True:
p.stepSimulation(physicsClient)
minAABB, maxAABB = p.getAABB(boxId)
print len(p.getOverlappingObjects(minAABB, maxAABB))
p.disconnect()
p.setGravity(0, 0, 0)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0, 0, 1]
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
boxId = p.loadURDF("r2d2.urdf", cubeStartPos, cubeStartOrientation)
# trigger1 = p.loadURDF("cube.urdf", [2, 2, 5])
shift = [0, -0.02, 0]
meshScale = [0.1, 0.1, 0.1]
triggerCube = p.createVisualShape(shapeType=p.GEOM_MESH,
fileName="cube.urdf",
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=shift,
meshScale=meshScale)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_SPHERE,
radius=5,
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0])
for i in range(10000):
p.stepSimulation()
res = p.getOverlappingObjects(triggerCube, triggerCube)
print(res)
time.sleep(1. / 240.)
cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
print(cubePos, cubeOrn)
p.disconnect()
def generate_cartpole_data(num_samples, torque_control=False):
# physicsClient = pb.connect(pb.GUI)
physicsClient = pb.connect(pb.DIRECT)
pb.setAdditionalSearchPath(pybullet_data.getDataPath())
cartpole_id = pb.loadURDF('hybrid_cartpole.urdf',
flags=pb.URDF_USE_SELF_COLLISION)
pb.setGravity(0, 0, -9.8)
pb.changeDynamics(cartpole_id, 1, restitution=restitution)
pb.changeDynamics(cartpole_id, 2, restitution=restitution)
pb.changeDynamics(cartpole_id, 3, restitution=restitution)
pb.setTimeStep(sim_time_step)
viewMatrix = pb.computeViewMatrix(cameraEyePosition=[0, -3, .5],
cameraTargetPosition=[0, 0, .5],
cameraUpVector=[0, 0, 1])
projectionMatrix = pb.computeProjectionMatrixFOV(fov=45.0,
aspect=1.0,
nearVal=0.1,
farVal=3.1)
num_step = int(data_time_step / sim_time_step)
X = np.zeros((num_samples, 6, width, height), dtype=np.uint8)
X_next = np.zeros((num_samples, 6, width, height), dtype=np.uint8)
U = np.zeros((num_samples, 1), dtype=np.float64)
k = 0
while k < num_samples:
q0 = np.random.rand(2) * (q_up - q_lo) + q_lo
v0 = np.random.rand(2) * (v_up - v_lo) + v_lo
u0 = 2 * (np.random.rand() - 1) * force_mag
for i in range(len(q0)):
pb.resetJointState(cartpole_id, i, q0[i], v0[i])
if torque_control:
pb.setJointMotorControl2(cartpole_id,
1,
pb.TORQUE_CONTROL,
force=u0)
else:
pb.setJointMotorControl2(cartpole_id,
1,
pb.VELOCITY_CONTROL,
force=0)
# check don't start with overlap already
aabb_pole_min, aabb_pole_max = pb.getAABB(cartpole_id, 1)
overlaps = pb.getOverlappingObjects(aabb_pole_min, aabb_pole_max)
if overlaps is not None and len(overlaps) > 0:
if (0, 2) in overlaps or (0, 3) in overlaps:
continue
width0, height0, rgb0, depth0, seg0 = pb.getCameraImage(
width=width,
height=height,
viewMatrix=viewMatrix,
projectionMatrix=projectionMatrix)
for i in range(num_step):
pb.stepSimulation()
width1, height1, rgb1, depth1, seg1 = pb.getCameraImage(
width=width,
height=height,
viewMatrix=viewMatrix,
projectionMatrix=projectionMatrix)
for i in range(num_step):
pb.stepSimulation()
width2, height2, rgb2, depth2, seg2 = pb.getCameraImage(
width=width,
height=height,
viewMatrix=viewMatrix,
projectionMatrix=projectionMatrix)
X[k, :3, :, :] = rgb0[:, :, :3].transpose(2, 0, 1)
X[k, 3:, :, :] = rgb1[:, :, :3].transpose(2, 0, 1)
X_next[k, :3, :, :] = rgb1[:, :, :3].transpose(2, 0, 1)
X_next[k, 3:, :, :] = rgb2[:, :, :3].transpose(2, 0, 1)
U[k, :] = u0
k += 1
update_progress(float(k) / num_samples)
pb.disconnect()
return X, X_next, U
for j in range(ARGS.num_drones):
wp_counters[j] = wp_counters[j] + 1 if wp_counters[j] < (NUM_WP-1) else 0
#### Log the simulation ####################################
for j in range(ARGS.num_drones):
logger.log(drone=j,
timestamp=i/env.SIM_FREQ,
state= obs[str(j)]["state"],
control=np.hstack([TARGET_POS[wp_counters[j], 0:2], INIT_XYZS[j, 2], INIT_RPYS[j, :], np.zeros(6)])
# control=np.hstack([INIT_XYZS[j, :]+TARGET_POS[wp_counters[j], :], INIT_RPYS[j, :], np.zeros(6)])
)
#### Printout ##############################################
if i%env.SIM_FREQ == 0:
a, b = p.getAABB(env.DRONE_IDS[0])
print(p.getOverlappingObjects(a, b))
#### Print matrices with the images captured by each drone #
if ARGS.vision:
for j in range(ARGS.num_drones):
print(obs[str(j)]["rgb"].shape, np.average(obs[str(j)]["rgb"]),
obs[str(j)]["dep"].shape, np.average(obs[str(j)]["dep"]),
obs[str(j)]["seg"].shape, np.average(obs[str(j)]["seg"])
)
#### Sync the simulation ###################################
if ARGS.gui:
sync(i, START, env.TIMESTEP)
#### Close the environment #################################
env.close()
def get_bodies_in_region(aabb):
(lower, upper) = aabb
return p.getOverlappingObjects(lower, upper, physicsClientId=CLIENT)
def place(self, robot_id: int):
# TODO(ycho): Consider exposing this parameter.
EPS = 1e-3
robot_pose = pb.getBasePositionAndOrientation(robot_id,
physicsClientId=self.sim_id)
old_pos = robot_pose[0]
old_rot = robot_pose[1]
old_z = old_pos[2]
floor_aabb = np.asarray(pb.getAABB(
self.env_ids_[0], -1, physicsClientId=self.sim_id), dtype=np.float32)
robot_aabb = debug_get_full_aabb(self.sim_id, robot_id)
robot_size = robot_aabb[1] - robot_aabb[0]
# NOTE(ycho): Shrink the sampled space by the robot radius.
pos_min = floor_aabb[0, :2] + 0.5 * robot_size[:2]
pos_max = floor_aabb[1, :2] - 0.5 * robot_size[:2]
for i in range(self.settings_.max_placement_iter):
logging.debug('Placement {}/{}'.format(i,
self.settings_.max_placement_iter))
# Sample X-Y position from floor AABB.
x, y = np.random.uniform(pos_min, pos_max)
# Cast ray from robot top -> floor.
ray_src = [x, y, floor_aabb[1, 2] +
robot_aabb[1, 2] - robot_aabb[0, 2]]
ray_dst = [x, y, floor_aabb[0, 2] - EPS]
ray_res = pb.rayTest(ray_src, ray_dst, physicsClientId=self.sim_id)
# If by some magic, multiple intersections happened,
# ignore this case.
if len(ray_res) != 1:
continue
ray_res = ray_res[0]
# The ray must hit env + floor.
if (ray_res[0] != self.env_ids_[0]) or (ray_res[1] != -1):
continue
# Complete the desired new position.
# new_z = floor_aabb[1, 2] + (old_z - robot_aabb[0, 2])
new_z = floor_aabb[1, 2] + (old_z - robot_aabb[0, 2])
new_pos = np.asarray([x, y, new_z + EPS], dtype=np.float32)
new_rot = old_rot
# NOTE(ycho): Alternatively, sample from a random SE2 orientation:
# new_rot = pb.getQuaternionFromEuler([0.0, 0.0, np.random.uniform(-np.pi, np.pi)])
# Reject the new position if it collides with existing objects.
if self.settings_.use_fast_aabb_in_placement:
# NOTE(ycho): This query is conservative, so the returned objects
# may not actually overlap with the robot. However,
# perhaps if the object is close enough to the robot that we should
new_aabb = robot_aabb + new_pos - old_pos
o = pb.getOverlappingObjects(new_aabb[0], new_aabb[1],
physicsClientId=self.sim_id)
if o is not None:
continue
pb.resetBasePositionAndOrientation(robot_id,
new_pos,
new_rot,
physicsClientId=self.sim_id)
break
else:
# Actually place the robot where it would go,
# and then check if it results in a collision.
# Try placing the robot here now ...
# NOTE(ycho): Since pybullet uses a default collision margin (0.04 I think?),
# even this may be a little bit more conservative than the actual collision.
pb.resetBasePositionAndOrientation(robot_id,
new_pos,
new_rot,
physicsClientId=self.sim_id)
col = False
for env_id in self.env_ids_:
cpts = pb.getClosestPoints(env_id, robot_id,
np.inf,
physicsClientId=self.sim_id)
for cpt in cpts:
if cpt[8] < 0:
col = True
break
# Early exit if collision found
if col:
break
# Continue searching if collision found
if col:
continue
# All is well! break.
break
