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