World Tutorial

Objectives

Through this tutorial, you will learn how to:

1. Create a world

2. Create a world view

3. Add obstacles to the world

4. Create a world inspector

5. Update a world view

6. Update an obstacle after it has been added to the world

What are a World and World View?

When a robot must avoid collision with or interact with its environment, we need a way to represent that environment. This helps us answer questions like:

• Is the robot in collision with the environment?

• How far is the tool frame from a given obstacle?

A World is the current “true” representation of the environment, which includes the most up-to-date location and description of all the obstacles in their environment.

A World View is a snapshot of the world. It is intended to be a lightweight object that can be cheaply copied and passed between algorithms, and guarantees the world seen through that world view will only be updated when it is specifically requested, and will simultaneously update all usages of that view. This is critical when the world representation is being constantly updated by external processes, such as a vision system running NVIDIA’s nvblox.

Creating a World

A world is easily created using cumotion::CreateWorld():

C++

std::shared_ptr<cumotion::World> world = cumotion::CreateWorld();

PYTHON

world: cumotion.World = cumotion.create_world()

which creates an empty world with no obstacles.

Tip

It is recommended that cumotion::CreateWorld() only be called a single time for each application. Most use-cases for creating another world should be solvable by using cumotion::WorldViewHandle.

Creating a World View

To create a world view, we simply call cumotion::World::addWorldView():

C++

cumotion::WorldViewHandle world_view = world->addWorldView();

PYTHON

world_view: cumotion.WorldViewHandle = world.add_world_view()

Creating a World Inspector

The cumotion::WorldViewHandle does not provide any capabilities for getting information about the world, such as the number of obstacles, distances to obstacles, etc. To do that we need to create a cumotion::WorldInspector, which provides an interface for querying information about the current state of the world:

C++

std::unique_ptr<cumotion::WorldInspector> world_inspector =
    cumotion::CreateWorldInspector(world_view);

PYTHON

world_inspector = cumotion.create_world_inspector(world_view)

We can use this to, for example, query the number of active obstacles in the environment:

C++

int num_enabled_obstacles = world_inspector->numEnabledObstacles();
std::cout << "Number of enabled obstacles (before update): " << num_enabled_obstacles << "\n";

PYTHON

num_enabled_obstacles = world_inspector.num_enabled_obstacles()
print(f"Number of enabled obstacles (before update): {num_enabled_obstacles}")

Output:

Number of enabled obstacles (before update): 0

Creating an Obstacle

In cuMotion, an obstacle is specified by a Type and a set of attributes specifications for the given type, such as height, radius, side lengths, etc.

Here we add a single cuboid obstacle to the world, with dimensions \([1, 2, 3]\):

C++

std::unique_ptr<cumotion::Obstacle> cuboid =
    cumotion::CreateObstacle(cumotion::Obstacle::Type::CUBOID);
cuboid->setAttribute(cumotion::Obstacle::Attribute::SIDE_LENGTHS, Eigen::Vector3d(1.0, 2.0, 3.0));

PYTHON

cuboid: cumotion.Obstacle = cumotion.create_obstacle(cumotion.Obstacle.Type.CUBOID)
cuboid.set_attribute(
    cumotion.Obstacle.Attribute.SIDE_LENGTHS, np.array([1.0, 2.0, 3.0]))

Adding an Obstacle to the World

After we’ve created an Obstacle, we add it to the world using cumotion::World::addObstacle():

C++

cumotion::World::ObstacleHandle obstacle_handle = world->addObstacle(*cuboid);

PYTHON

obstacle_handle: cumotion.World.ObstacleHandle = world.add_obstacle(cuboid)

The return type is an cumotion::World::ObstacleHandle, which is an opaque handle to the copy of the obstacle in the world. The cuMotion library currently does not provide any way to query data about an obstacle given its cumotion::World::ObstacleHandle, so it is the responsibility of the user the keep track of its type, attributes, enabled state, and pose. We will see how to set these values below.

Note that currently this obstacle is NOT seen by the world view we created earlier. If we query the number of enabled obstacles using cumotion::WorldInspector::numEnabledObstacles(), we see it returns zero:

C++

int num_enabled_obstacles = world_inspector->numEnabledObstacles();
std::cout << "Number of enabled obstacles (before update): " << num_enabled_obstacles << "\n";

PYTHON

num_enabled_obstacles = world_inspector.num_enabled_obstacles()
print(f"Number of enabled obstacles (before update): {num_enabled_obstacles}")

Output:

Number of enabled obstacles (before update): 0

Whereas if we create a new world view, the new world view shows that we have indeed added an obstacle:

C++

cumotion::WorldViewHandle world_view2 = world->addWorldView();
auto world_inspector2 = cumotion::CreateWorldInspector(world_view2);
int num_enabled_obstacles2 = world_inspector2->numEnabledObstacles();
std::cout << "Number of enabled obstacles (new world view): " << num_enabled_obstacles2 << "\n";

PYTHON

world_view2: cumotion.WorldViewHandle = world.add_world_view()
world_inspector2 = cumotion.create_world_inspector(world_view2)
num_enabled_obstacles2 = world_inspector2.num_enabled_obstacles()
print(f"Number of enabled obstacles (new world view): {num_enabled_obstacles2}")

Output:

Number of enabled obstacles (new world view): 1

Updating a World View

Like we mentioned above, a world view is a snapshot of the world, and must be explicitly updated. All copies of a world view will be updated at the same time. We demonstrate this below:

C++

// Create a copy of the original world view.
cumotion::WorldViewHandle world_view_copy = world_view;
auto world_inspector_copy = cumotion::CreateWorldInspector(world_view_copy);

// Check the number of enabled obstacles in the copy.
int num_enabled_obstacles_copy = world_inspector_copy->numEnabledObstacles();
std::cout << "Number of enabled obstacles (copy, before update): " << num_enabled_obstacles_copy
          << "\n";

// Update the original world view.
world_view.update();

// Check the number of enabled obstacles after update.
num_enabled_obstacles = world_inspector->numEnabledObstacles();
std::cout << "Number of enabled obstacles (after update): " << num_enabled_obstacles << "\n";

// Check the copy - it should also be updated.
num_enabled_obstacles_copy = world_inspector_copy->numEnabledObstacles();
std::cout << "Number of enabled obstacles (copy, after update): " << num_enabled_obstacles_copy
          << "\n";

PYTHON

# Create a copy of the original world view.
world_view_copy = world_view
world_inspector_copy = cumotion.create_world_inspector(world_view_copy)

# Check the number of enabled obstacles in the copy.
num_enabled_obstacles_copy = world_inspector_copy.num_enabled_obstacles()
print(f"Number of enabled obstacles (copy, before update): {num_enabled_obstacles_copy}")

# Update the original world view.
world_view.update()

# Check the number of enabled obstacles after update.
num_enabled_obstacles = world_inspector.num_enabled_obstacles()
print(f"Number of enabled obstacles (after update): {num_enabled_obstacles}")

# Check the copy - it should also be updated.
num_enabled_obstacles_copy = world_inspector_copy.num_enabled_obstacles()
print(
    f"Number of enabled obstacles (copy, after update): {num_enabled_obstacles_copy}")

Output:

Number of enabled obstacles (copy, before update): 0
Number of enabled obstacles (after update): 1
Number of enabled obstacles (copy, after update): 1

Tip

A world view behaves nearly identically to that of a std::shared_ptr in C++. Copies are cheap, and all “point” to the same data. Updating the underlying data will affect all copies of the pointer.

Modifying an Obstacle

After adding an obstacle to the world, we are only allowed to do the following:

• Modify the pose of the obstacle in the world

• Set whether the obstacle is enabled or disabled

• Remove the obstacle from the world

To do any of these operations, we must use the cumotion::World::ObstacleHandle returned by cumotion::World::addObstacle().

Warning

Calling cumotion::Obstacle::setAttribute() on an cumotion::Obstacle after adding it to the world will NOT update the copy of the obstacle in the world.

To change the pose of an obstacle, use cumotion::World::setPose(). Here we move the cuboid so that it is centered on \([0.5, 0.5, 0.7]\), and rotated 30 degrees about the x-axis:

C++

// Move the cuboid to [0.5, 0.5, 0.7] and rotate 30 degrees about the x-axis.
cumotion::Rotation3 rotation =
    cumotion::Rotation3::FromAxisAngle(Eigen::Vector3d::UnitX(), 30.0 * M_PI / 180.0);
world->setPose(obstacle_handle, {rotation, {0.5, 0.5, 0.7}});

PYTHON

# Move the cuboid to [0.5, 0.5, 0.7] and rotate 30 degrees about the x-axis.
rotation = cumotion.Rotation3.from_axis_angle(
    np.array([1.0, 0.0, 0.0]), 30.0 * np.pi / 180.0)
world.set_pose(obstacle_handle, cumotion.Pose3(rotation, np.array([0.5, 0.5, 0.7])))

We can enable and disable the obstacle using cumotion::World::enableObstacle() and cumotion::World::disableObstacle():

C++

world->disableObstacle(obstacle_handle);

// Update the world view and check the number of enabled obstacles.
world_view.update();
num_enabled_obstacles = world_inspector->numEnabledObstacles();
std::cout << "Number of enabled obstacles (after disabling): " << num_enabled_obstacles << "\n";

// Re-enable the obstacle.
world->enableObstacle(obstacle_handle);

// Update the world view and check the number of enabled obstacles.
world_view.update();
num_enabled_obstacles = world_inspector->numEnabledObstacles();
std::cout << "Number of enabled obstacles (after re-enabling): " << num_enabled_obstacles << "\n";

PYTHON

# Move the cuboid to [0.5, 0.5, 0.7] and rotate 30 degrees about the x-axis.
rotation = cumotion.Rotation3.from_axis_angle(
    np.array([1.0, 0.0, 0.0]), 30.0 * np.pi / 180.0)
world.set_pose(obstacle_handle, cumotion.Pose3(rotation, np.array([0.5, 0.5, 0.7])))
# *snippet-end* #

# Disable the obstacle.
# *snippet-begin* #
world.disable_obstacle(obstacle_handle)

# Update the world view and check the number of enabled obstacles.
world_view.update()
num_enabled_obstacles = world_inspector.num_enabled_obstacles()
print(f"Number of enabled obstacles (after disabling): {num_enabled_obstacles}")

# Re-enable the obstacle.
world.enable_obstacle(obstacle_handle)

# Update the world view and check the number of enabled obstacles.
world_view.update()
num_enabled_obstacles = world_inspector.num_enabled_obstacles()
print(f"Number of enabled obstacles (after re-enabling): {num_enabled_obstacles}")

Output:

Number of enabled obstacles (after disabling): 0
Number of enabled obstacles (after re-enabling): 1