Location Designator

Location Designator#

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

A location designator describes a region of poses the robot can be sent to, for example every position from which the robot can reach a certain object. In CoraPlex location designators are the locations in coraplex.locations. A Location is iterable: iterating it yields candidate poses, and a plan tries them in turn until one works. CoraPlex provides locations for:

Reachability: ReachabilityLocation
Visibility: VisibilityLocation

Both sample their candidates from costmaps (see Costmaps): 2D grids around the target whose cells are rated above zero wherever the robot meets the criterion. Both also merge in an occupancy costmap, so every candidate is a position where the robot can stand without colliding with its surroundings. The costmaps are built from the world as it is when the candidates are sampled.

We start with a world holding the PR2 in the apartment. The candidates are sampled at random, with the seed of the plan’s context, so this example runs the same way every time.

from coraplex.testing import setup_world
from semantic_digital_twin.robots.pr2 import PR2

world = setup_world()
pr2_view = PR2.from_world(world)
from coraplex.plans.context_extensions import RobotAccess, StatementGrounding
from cramph.context import StatechartContext
from coraplex.plans.executors import KinematicallySimulatedPlanExecutor
from cramph.statechart import Statechart

extensions = [RobotAccess(pr2_view), StatementGrounding()]
context = KinematicallySimulatedPlanExecutor(
    StatechartContext.with_extensions(world, extensions)
).context


def run(plan):
    """
    Run `plan` simulated in `world`, with the robot and settings in `extensions`.
    """
    executor = KinematicallySimulatedPlanExecutor(
        StatechartContext.with_extensions(world, extensions)
    )
    statechart = Statechart(context=executor.context)
    statechart.add_node(plan)
    executor.compile(statechart)
    executor.execute()
    return executor


origin_pose = pr2_view.root.global_pose

Reachable#

A ReachabilityLocation describes where the robot can stand to reach a target pose with one arm, for example an object the robot should pick up. It needs the target and the arm that is to reach it.

We use the milk as the target. The torso of the PR2 is raised first, since otherwise its arms are too low to reach onto the countertop.

from coraplex.robot_plans.actions.core.robot_body import ParkArmsAction, MoveTorsoAction
from cramph.composites import Sequence
from semantic_digital_twin.datastructures.definitions import TorsoState

run(Sequence(nodes=[ParkArmsAction(arms=pr2_view.all_arms),
            MoveTorsoAction(torso_state=TorsoState.HIGH)]))

from coraplex.robot_plans.actions.core.navigation import NavigateAction
from coraplex.locations.locations import ReachabilityLocation
from krrood_spatial_math.spatial_types import Pose

location = ReachabilityLocation(
    Pose(reference_frame=world.get_body_by_name("milk.stl")),
    pr2_view.left_arm,
    context=context,
)

run(NavigateAction(target_location=location.ground()))

pr2_view.root.parent_connection.origin = origin_pose.homogeneous_matrix

ground() returns the first candidate. It is a pose near the countertop where the robot can stand without colliding with it, facing the milk, at about half the arm’s length from it. Candidates farther from the target than the arm is long are left out, since the target cannot be reached from there.

The target is given relative to the milk, so the location follows the milk wherever it is when the candidates are sampled.

Accessing#

How far the robot stands off the target is a fraction of the arm’s length, given as a ReachFraction. The default, GRASPING, suits an object that stays where it is. A container’s handle is pulled towards the robot when it opens, so for a handle the robot stands further back, at ACCESSING.

from coraplex.datastructures.enums import ReachFraction
from semantic_digital_twin.semantic_annotations.semantic_annotations import Drawer, Handle

with world.modify_world():
    world.add_semantic_annotation_recursively(
        drawer := Drawer(
            root=world.get_body_by_name("cabinet10_drawer_middle"),
            handle=Handle(root=world.get_body_by_name("handle_cab10_m")),
        )
    )

location = ReachabilityLocation(
    Pose(reference_frame=drawer.handle.root),
    pr2_view.left_arm,
    ReachFraction.ACCESSING,
    context=context,
)

print(location.ground())

Visible#

A VisibilityLocation describes where the robot can stand to see a target pose with its default camera. It only needs the target.

from coraplex.locations.locations import VisibilityLocation

location = VisibilityLocation(
    Pose(reference_frame=world.get_body_by_name("milk.stl")), context=context
)

run(NavigateAction(target_location=location.ground()))

pr2_view.root.parent_connection.origin = origin_pose.homogeneous_matrix

Iterating the Candidates#

A location is a generator of candidates, which is useful when the first candidate does not work for some reason. A location samples at most number_of_samples candidates. It draws them with the plan’s seed, the sampling_seed of the StatementGrounding in its context, which is 0 unless set; a seed of None draws different candidates each time.

from krrood_spatial_math.spatial_types import Point3

location = VisibilityLocation(
    Pose(Point3.from_iterable([-1, 0, 1.2]), reference_frame=world.root),
    context=context,
    number_of_samples=5,
)

for pose in location:
    print(pose)

Locations in a Plan#

A plan does not have to pick a candidate itself. Given as the domain of a variable, a location leaves the pose of an action open, and the plan tries the candidates in turn when it gets to that action, until the action succeeds with one of them.

from cramph.composites import StatementNode
from krrood.entity_query_language.factories import a, variable

navigate = a(NavigateAction)(
    target_location=variable(
        Pose,
        domain=ReachabilityLocation(
            Pose(reference_frame=world.get_body_by_name("milk.stl")),
            pr2_view.left_arm,
            context=context,
        ),
    )
)

run(StatementNode(statement=navigate))

pr2_view.root.parent_connection.origin = origin_pose.homogeneous_matrix

Tuning the Costmaps#

The costmaps a location samples from are square, map_cells cells along each side, each cell map_resolution meters wide. The defaults cover 4 m by 4 m around the target. A reachability location also takes ring_standard_deviation, how far, in cells, its candidates spread around the stand-off distance.

location = ReachabilityLocation(
    Pose(reference_frame=world.get_body_by_name("milk.stl")),
    pr2_view.left_arm,
    context=context,
    map_resolution=0.04,
    map_cells=100,
    ring_standard_deviation=8,
)

print(location.ground())