Python pybullet.rayTestBatch() Examples

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 

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 

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)] 

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