Location Designator

Location Designator#

This example will show you what location designators are, how to use them and what they are capable of.

Location Designators are used to semantically describe locations in the world. You could, for example, create a location designator that describes every position where a robot can be placed without colliding with the environment. Location designator can describe locations for:

  • Visibility

  • Reachability

  • Occupancy

  • URDF Links (for example a table)

To find locations that fit the given constrains, location designator create Costmaps. Costmaps are a 2D distribution that have a value greater than 0 for every position that fits the costmap criteria.

Location designators work similar to other designators, meaning you have to create a location designator description which describes the location. This description can then be resolved to the actual 6D pose on runtime.

Occupancy#

We will start with a simple location designator that describes a location where the robot can be placed without colliding with the environment. To do this we need a BulletWorld since the costmaps are mostly created from the current state of the BulletWorld.

from pycram.testing import setup_world
from pycram.datastructures.dataclasses import Context
from semantic_digital_twin.robots.pr2 import PR2


world = setup_world()
pr2_view = PR2.from_world(world)
context = Context(world, pr2_view)

Next up we will create the location designator description, the CostmapLocation() that we will be using needs a target as a parameter. This target describes what the location designator is for, this could either be a pose or object that the robot should be able to see or reach.

In this case we only want poses where the robot can be placed, this is the default behaviour of the location designator which we will be extending later.

Since every designator in PyCRAM needs to be part of a plan we create a simple plan which contains our Location Designator.

# from pycram.designators.location_designator import CostmapLocation
# from pycram.language import SequentialPlan
# from pycram.robot_plans import NavigateActionDescription
# 
# location_description = CostmapLocation(world.root)
# 
# location_description = SequentialPlan((world, None), pr2_view, NavigateActionDescription(location_description))
# 
# pose = location_description.resolve()
# 
# print(pose)

Reachable#

Next we want to have locations from where the robot can reach a specific point, like an object the robot should pick up. This can also be done with the CostmapLocation() description, but this time we need to provide an additional argument. The additional argument is the robot which should be able to reach the pose.

Since a robot is needed we will use the PR2 and use a milk as a target point for the robot to reach. The torso of the PR2 will be set to 0.2 since otherwise the arms of the robot will be too low to reach on the countertop.

from pycram.process_module import simulated_robot
from pycram.language import SequentialPlan
from pycram.robot_plans.actions import ParkArmsActionDescription, MoveTorsoActionDescription
from pycram.datastructures.enums import Arms, TorsoState

with simulated_robot:
    SequentialPlan(context, 
                   ParkArmsActionDescription(Arms.BOTH),
                   MoveTorsoActionDescription(TorsoState.HIGH)).perform()


from pycram.designators.location_designator import CostmapLocation
from pycram.designators.object_designator import BelieveObject
from pycram.language import SequentialPlan

location_description = CostmapLocation(target=world.get_body_by_name("milk.stl"), reachable_for=pr2_view)

SequentialPlan(context, location_description)

print(location_description.resolve())

As you can see we get a pose near the countertop where the robot can be placed without colliding with it. Furthermore, we get a list of arms with which the robot can reach the given object.

Visible#

The CostmapLocation() can also find position from which the robot can see a given object or location. This is very similar to how reachable locations are described, meaning we provide a object designator or a pose and a robot designator but this time we use the visible_for parameter.

For this example we need the milk as well as the PR2, so if you did not spawn them during the previous location designator you can spawn them with the following cell.

from pycram.designators.location_designator import CostmapLocation
from pycram.designators.object_designator import BelieveObject
from pycram.language import SequentialPlan

location_description = CostmapLocation(target=world.get_body_by_name("milk.stl"), visible_for=pr2_view)

plan = SequentialPlan(context, location_description)

print(location_description.resolve())

Semantic#

Semantic location designator are used to create location descriptions for semantic entities, like a table. An example of this is: You have a robot that picked up an object and should place it on a table. Semantic location designator then allows to find poses that are on this table.

Semantic location designator need an object from which the target entity is a part and the URDF link representing the entity. In this case we want a position on the kitchen island, so we have to provide the kitchen object designator since the island is a part of the kitchen and the link name of the island surface.

For this example we need the kitchen as well as the milk. If you spawned them in one of the previous examples you don’t need to execute the following cell.

from pycram.designators.location_designator import SemanticCostmapLocation
from pycram.designators.object_designator import BelieveObject

location_description = SemanticCostmapLocation(world.get_body_by_name("island_countertop"),
                                               for_object=world.get_body_by_name("milk.stl"))

plan = SequentialPlan(context, location_description)

print(location_description.resolve())

ProbabilisticSemanticLocation fulfills te same purpose as the SemanticCostmapLocation, but it uses probabilistic circuits and random events to build the costmap and sample from it. This allows more control over the sample space as well as overall better quality of samples. One difference to the SemanticCostmapLocation is that it takes a list of link names instead of a single link name, which allows us to sample from multiple links at once.

from pycram.designators.location_designator import ProbabilisticSemanticLocation

location_description = ProbabilisticSemanticLocation(world.get_body_by_name("island_countertop"),
                                                    for_object=world.get_body_by_name("milk.stl"))

plan = SequentialPlan(context, location_description)

print(location_description.resolve())

Location Designator as Generator#

Location designator descriptions implement an iter method, so they can be used as generators which generate valid poses for the location described in the description. This can be useful if the first pose does not work for some reason.

We will see this at the example of a location designator for visibility. For this example we need the milk, if you already have a milk spawned in you world you can ignore the following cell.

from pycram.designators.location_designator import CostmapLocation
from pycram.designators.object_designator import BelieveObject
from pycram.language import SequentialPlan

location_description = CostmapLocation(target=world.get_body_by_name("milk.stl"), visible_for=pr2_view)
plan = SequentialPlan(context, location_description)


for i, pose in enumerate(location_description):
    print(pose)
    if i > 3:
        break

Similar to the ProbabilisticSemanticLocation, the ProbabilisticCostmapLocation can be used as an alternative to the CostmapLocation. It uses probabilistic circuits and random events to build the costmap and sample from it, but has the same interface as the CostmapLocation.

from pycram.designators.location_designator import ProbabilisticCostmapLocation
from pycram.designators.object_designator import BelieveObject
from pycram.language import SequentialPlan

location_description = ProbabilisticCostmapLocation(target=world.get_body_by_name("milk.stl"), visible_for=pr2_view)
plan = SequentialPlan(context, location_description)

for i, pose in enumerate(location_description):
    print(pose)
    if i > 3:
        break

Accessing Locations#

Accessing describes a location from which the robot can open a drawer. The drawer is specified by a ObjetcPart designator which describes the handle of the drawer.

At the moment this location designator only works in the apartment environment, so please remove the kitchen if you spawned it in a previous example. Furthermore, we need a robot, so we also spawn the PR2 if it isn’t spawned already.

from pycram.designators.object_designator import *
from pycram.designators.location_designator import *
from pycram.language import SequentialPlan

access_location = AccessingLocation(world.get_body_by_name("handle_cab10_t"), pr2_view)
plan = SequentialPlan(context, access_location)

print(access_location.resolve())
Contents