Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adding dynamic obstacles in environment #257

Open
Astik-2002 opened this issue Dec 29, 2024 · 2 comments
Open

Adding dynamic obstacles in environment #257

Astik-2002 opened this issue Dec 29, 2024 · 2 comments
Labels
question Further information is requested

Comments

@Astik-2002
Copy link

Hello. I'm working on a vision based collision avoidance scheme for UAVs. I've been adding obstacles into the environment using the _addObstacles function in baseaviary. It looks like this
`
def _addObstacles(self):
"""Add a 'forest' of trees as obstacles to the environment.

    Parameters
    ----------
    num_trees : int, optional
        The number of trees to add to the environment.
    x_bounds : tuple, optional
        The x-axis bounds within which trees will be randomly placed.
    y_bounds : tuple, optional
        The y-axis bounds within which trees will be randomly placed.
    """
    # Call the parent class _addObstacles (if needed)
    # super()._addObstacles()
    less = False
    if not less:
        num_trees= 30
        x_bounds=(0.5, 55.5)
        y_bounds=(-10, 10)
        
        base_path = pkg_resources.resource_filename('gym_pybullet_drones', 'assets')
        tree_urdf = os.path.join(base_path, "simple_tree.urdf")

        # Add trees randomly within the specified bounds
        for _ in range(num_trees):
            # Generate random x and y coordinates within the specified bounds
            x_pos = random.uniform(x_bounds[0], x_bounds[1])
            y_pos = random.uniform(y_bounds[0], y_bounds[1])

            # Randomly place the tree at this location, z is fixed (0 for ground level)
            pos = (x_pos, y_pos, 0.0)

            # Load the tree URDF at the generated position
            if os.path.exists(tree_urdf):
                p.loadURDF(tree_urdf,
                        pos,
                        p.getQuaternionFromEuler([0, 0, 0]),  # No rotation
                        useFixedBase=True,
                        physicsClientId=self.CLIENT)
            else:
                print(f"File not found: {tree_urdf}")
    
    else:
        base_path = pkg_resources.resource_filename('gym_pybullet_drones', 'assets')
        tree_urdf = os.path.join(base_path, "simple_tree.urdf")
        pos = (0.5, 0, 0)
        if os.path.exists(tree_urdf):
                p.loadURDF(tree_urdf,
                        pos,
                        p.getQuaternionFromEuler([0, 0, 0]),  # No rotation
                        useFixedBase=True,
                        physicsClientId=self.CLIENT)
        else:
            print(f"File not found: {tree_urdf}")

`
The resulting environment can be seen in the image pinned below. Now, I need to introduce dynamic objects in the environment which follow some trajectory (constant velocity, random velocity, constant acceleration trajectories etc.). Can someone kindly share how to do this? One simple approach would be to spawn other UAVs, but it will be difficult to segment them in depth image due to small size, so I would like to spawn urdfs with larger size and have them move in simulation.
image

@JacopoPan JacopoPan added the question Further information is requested label Jan 4, 2025
@JacopoPan
Copy link
Member

p.loadURDF returns a body unique id, you could use p.resetBaseVelocity() and p.applyExternalForce() on that id before calling p.stepSimulation() in BaseAviary.step() to make them move (check PyBullet Quick Guide)

@Astik-2002
Copy link
Author

Its working now, thanks @JacopoPan

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
question Further information is requested
Projects
None yet
Development

No branches or pull requests

2 participants