本文整理汇总了Python中pybullet.rayTestBatch方法的典型用法代码示例。如果您正苦于以下问题：Python pybullet.rayTestBatch方法的具体用法？Python pybullet.rayTestBatch怎么用？Python pybullet.rayTestBatch使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类pybullet的用法示例。
在下文中一共展示了pybullet.rayTestBatch方法的3个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: find_best_k_views
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import rayTestBatch [as 别名]
def find_best_k_views(self, eye_pos, all_dist, all_pos, avoid_block=False):
least_order = (np.argsort(all_dist))
top_k = []
num_to_test = self.k * 2
curr_num = 0
all_pos = np.array(all_pos)
if not avoid_block:
return least_order[:self.k]
while len(top_k) < self.k:
curr_order = least_order[curr_num: curr_num + num_to_test]
curr_pos = all_pos[curr_order]
print("Curr num", curr_num, "top k", len(top_k), self.k)
if len(curr_pos) <= p.MAX_RAY_INTERSECTION_BATCH_SIZE:
collisions = list(p.rayTestBatch([eye_pos] * len(curr_pos), curr_pos))
else:
collisions = []
curr_i = 0
while (curr_i < len(curr_pos)):
curr_n = min(len(curr_pos), curr_i + p.MAX_RAY_INTERSECTION_BATCH_SIZE - 1)
collisions = collisions + list(p.rayTestBatch([eye_pos] * (curr_n - curr_i), curr_pos[curr_i: curr_n]))
curr_i = curr_n
has_collision = [c[0] > 0 for c in collisions]
## (hzyjerry): ray_casting-based view selection occasionally gives unstable behaviour. Will keep watching on this
for i, x in enumerate(curr_order):
if not has_collision[i]:
top_k.append(x)
return top_k 示例2: compute
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import rayTestBatch [as 别名]
def compute(self, world, state):
import pybullet as pb
object_translation_rotation = []
# camera.py ImageData namedtuple
camera = world.cameras[0]
image_data = camera.capture()
image_data_arrays = [np.array(value) for value in image_data] + \
[camera.matrix, camera.projection_matrix]
# 'camera_view_matrix' namedtuple index is 4
# TODO(ahundt) ensure camera matrix translation component is from world origin to camera origin
# camera ray is from the origin of the camera
camera_transform_array = np.transpose(image_data[4]).reshape((4, 4))
# use the more conveniently formatted transform for writing out
image_data_arrays[4] = camera_transform_array
camera_translation = camera_transform_array[0:3, 3].tolist()
# print("TEST IMAGEDATA named tuple img matrix: \n", )
# print("TEST IMAGEDATA named tuple img matrix translation: ", camera_translation)
camera_ray_from = [camera_translation] * len(world.id_by_object.items())
# camera_ray_to is the center of each object
camera_ray_to = []
for name, oid in world.id_by_object.items():
# print("oid type:", str(type(oid)))
# print("actor type:", str(type(world.actors[oid])))
obj = world.actors[oid].getState()
p = obj.T.p
q = obj.T.M.GetQuaternion()
one_t_r = np.array([p[0], p[1], p[2], q[0], q[1], q[2], q[3]], dtype=np.float32)
object_translation_rotation.append(one_t_r)
camera_ray_to.append(list(obj.T.p))
# print("lengths: ", len(camera_ray_from), len(camera_ray_to))
object_surface_points = []
# TODO(ahundt) allow multiple simulations to run
raylist = pb.rayTestBatch(camera_ray_from, camera_ray_to)
for i, (uid, linkidx, hitfrac, hitpos, hitnormal) in enumerate(raylist):
if uid is -1:
# if the object wasn't hit, use its origin
name, oid = world.id_by_object.items()[i]
obj = world.actors[oid].getState()
object_surface_points += [obj.T.p]
else:
object_surface_points += hitpos
return [np.array(object_translation_rotation),
np.array(state.arm),
np.array(state.gripper)] + image_data_arrays[1:] + [np.array(object_surface_points)] 示例3: _laserScan
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import rayTestBatch [as 别名]
def _laserScan(self):
"""
INTERNAL METHOD, a loop that simulate the laser and update the distance
value of each laser
"""
lastLidarTime = time.time()
while not self._module_termination:
nowLidarTime = time.time()
if (nowLidarTime-lastLidarTime > 1/LASER_FRAMERATE):
results = pybullet.rayTestBatch(
self.ray_from,
self.ray_to,
parentObjectUniqueId=self.robot_model,
parentLinkIndex=self.laser_id,
physicsClientId=self.physics_client)
for i in range(NUM_RAY*len(ANGLE_LIST_POSITION)):
hitObjectUid = results[i][0]
hitFraction = results[i][2]
hitPosition = results[i][3]
self.laser_value[i] = hitFraction * RAY_LENGTH
if self.display:
if not self.ray_ids:
self._createDebugLine()
if (hitFraction == 1.):
pybullet.addUserDebugLine(
self.ray_from[i],
self.ray_to[i],
RAY_MISS_COLOR,
replaceItemUniqueId=self.ray_ids[i],
parentObjectUniqueId=self.robot_model,
parentLinkIndex=self.laser_id,
physicsClientId=self.physics_client)
else: # pragma: no cover
localHitTo = [self.ray_from[i][0]+hitFraction*(
self.ray_to[i][0]-self.ray_from[i][0]),
self.ray_from[i][1]+hitFraction*(
self.ray_to[i][1]-self.ray_from[i][1]),
self.ray_from[i][2]+hitFraction*(
self.ray_to[i][2]-self.ray_from[i][2])]
pybullet.addUserDebugLine(
self.ray_from[i],
localHitTo,
RAY_HIT_COLOR,
replaceItemUniqueId=self.ray_ids[i],
parentObjectUniqueId=self.robot_model,
parentLinkIndex=self.laser_id,
physicsClientId=self.physics_client)
else:
if self.ray_ids:
self._resetDebugLine()
lastLidarTime = nowLidarTime