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.bullet_world import BulletWorld, Object
from pycram.enums import ObjectType
from pycram.pose import Pose

world = BulletWorld()
kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf")

Scalar element defined multiple times: limit
Scalar element defined multiple times: limit

world.exit()

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.

from pycram.designators.location_designator import CostmapLocation

target = kitchen.get_pose()

location_description = CostmapLocation(target)

pose = location_description.resolve()

print(pose)

CostmapLocation.Location(pose=header:
  seq: 0
  stamp:
    secs: 1699625777
    nsecs: 318477869
  frame_id: "map"
pose:
  position:
    x: 0.32
    y: 0.46
    z: 0.0
  orientation:
    x: -0.0
    y: 0.0
    z: 0.8863025691598214
    w: -0.46310663556107684, reachable_arms=None)

Reachable

Next we want to 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 robo 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.

pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf")
pr2.set_joint_state("torso_lift_joint", 0.2)
milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9]))

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

target = BelieveObject(names=["milk"]).resolve()
robot_desig = BelieveObject(names=["pr2"]).resolve()

location_description = CostmapLocation(target=target, reachable_for=robot_desig)

print(location_description.resolve())

CostmapLocation.Location(pose=header:
  seq: 0
  stamp:
    secs: 1699448386
    nsecs: 194609165
  frame_id: "map"
pose:
  position:
    x: 0.52
    y: 0.94
    z: 0.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0.03837651950358723
    w: 0.9992633500488202, reachable_arms=['left', 'right'])

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.

Visibile

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.

pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf")
milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9]))

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

target = BelieveObject(names=["milk"]).resolve()
robot_desig = BelieveObject(names=["pr2"]).resolve()

location_description = CostmapLocation(target=target, visible_for=robot_desig)

print(location_description.resolve())

CostmapLocation.Location(pose=header:
  seq: 0
  stamp:
    secs: 1699448390
    nsecs: 664081811
  frame_id: "map"
pose:
  position:
    x: 0.7000000000000001
    y: 0.040000000000000036
    z: 0.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0.4847685323929452
    w: 0.8746424812468178, reachable_arms=None)

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.

kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf")
milk = Object("milk", ObjectType.MILK, "milk.stl")

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

kitchen_desig = BelieveObject(names=["kitchen"]).resolve()
milk_desig = BelieveObject(names=["milk"]).resolve()

location_description = SemanticCostmapLocation(urdf_link_name="kitchen_island_surface",
                                               part_of=kitchen_desig,
                                              for_object=milk_desig)

print(location_description.resolve())

SemanticCostmapLocation.Location(pose=header:
  seq: 0
  stamp:
    secs: 1699448395
    nsecs: 710832595
  frame_id: "map"
pose:
  position:
    x: -1.2074999952316285
    y: 1.019200015068054
    z: 0.9398907270729542
  orientation:
    x: 0.0
    y: 0.0
    z: 0.6339889056055381
    w: 0.7733421413379024)

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.

milk = Object("milk", ObjectType.MILK, "milk.stl")

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

target = BelieveObject(names=["milk"]).resolve()
robot_desig = BelieveObject(names=["pr2"]).resolve()

location_description = CostmapLocation(target=target, visible_for=robot_desig)

for pose in location_description:
    print(pose.pose)

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.

kitchen.remove()

apartment = Object("apartment", ObjectType.ENVIRONMENT, "apartment.urdf")

Unknown tag "material" in /robot[@name='apartment']/link[@name='coffe_machine']/collision[1]
Unknown tag "material" in /robot[@name='apartment']/link[@name='coffe_machine']/collision[1]

pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf")
pr2.set_joint_state("torso_lift_joint", 0.25)

Unknown attribute "type" in /robot[@name='pr2']/link[@name='base_laser_link']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='laser_tilt_link']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='base_laser_link']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='laser_tilt_link']

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

apartment_desig = BelieveObject(names=["apartment"])
handle_desig = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig.resolve())
robot_desig = BelieveObject(names=["pr2"])

access_location = AccessingLocation(handle_desig.resolve(), robot_desig.resolve()).resolve()
print(access_location.pose)

header:
  seq: 0
  stamp:
    secs: 1699448422
    nsecs: 547766208
  frame_id: "map"
pose:
  position:
    x: 1.8074915790557862
    y: 2.7473597526550293
    z: 0.0
  orientation:
    x: -0.0
    y: 0.0
    z: 0.5893608715092853
    w: -0.8078698924541103