3. Behavior Examples: Peck Games

This tutorial shows how to design simple behaviors and explores the tradeoffs between state machines and decider networks. It steps through two implementations of a simple ground-pecking behavior with the Franka robot where the robot must peck around the blocks. The first implementation uses a state machine and is unable to react to blocks moved in front of its path. We fix that issue in the second implementation with a simple decider network that internally leverages parts of the original state machine. The state machine is effectively the same, but the higher-level decider can pre-empt the state machine as needed for reactivity. Finally, we implement a reactive pick game using a pure decider network. In that final example, we demonstrate the utility of the custom context object and its monitors.

For brevity, we use the abbreviation isaac_python for the Isaac Sim python script (../../../python.sh on Linux and ..\..\..\python.bat on Windows), and assume the working directory is standalone_examples/api/omni.isaac.cortex.

Each example will launch Isaac Sim without playing the simulation. Press play to run the simulation and behavior.

3.1. Designing reactivity using decider networks

We start with a simple behavior that has the robot peck at the ground avoiding regions occupied by blocks.

3.1.1. State machine implementation

The behaviors/franka/peck_state_machine.py module implements this simple peck behavior as a state machine. Run the behavior using:

isaac_python franka_examples_main.py --behavior=behaviors/franka/peck_state_machine.py

The Franka robot will peck at the ground avoiding the blocks. You can move the blocks around to see how that affects where the robot chooses to peck.

The implementation is straightforward:

class PeckState(DfState):
    ...
    def enter(self):
        # On entry, sample a target.
        target_p = self.sample_target_p_away_from_obs()
        target_q = make_target_rotation(target_p)
        self.target = PosePq(target_p, target_q)
        approach_params = ApproachParams(direction=np.array([0.0, 0.0, -0.1]), std_dev=0.04)
        self.context.robot.arm.send_end_effector(self.target, approach_params=approach_params)

    def step(self):
        target_dist = np.linalg.norm(self.context.robot.arm.get_fk_p() - self.target.p)
        if target_dist < 0.01:
            return None  # Exit
        return self  # Keep going

def make_decider_network(robot):
    root = DfStateMachineDecider(
        DfStateSequence(
            [
                DfCloseGripper(width=0.0),
                PeckState(),
                DfTimedDeciderState(DfLift(height=0.05), activity_duration=0.25)
            ],
            loop=True,
        )
    )
    return DfNetwork(root, context=PeckContext(robot))

With the simple state machine implementation, however, there’s an error case in reactivity. Try moving a block directly into the path of a current peck. The behavior will hang trying unsuccessfully to get the end-effector to the target.

The state machine chooses the target on entry and keeps it fixed throughout the behavior. It, therefore, doesn’t react to the changing environment. State machines, by themselves, aren’t great at modeling reactive behavior. We’ll use decider networks to fix this problem.

3.1.2. Decider network implementation

The behaviors/franka/peck_decider_network.py module augments this simple peck behavior by adding a reactive Dispatch decider node. Run the behavior using:

isaac_python franka_examples_main.py --behavior=behaviors/franka/peck_decider_network.py

The decider network uses a logical state monitor to monitor whether there’s a block that would prevent the end-effector from reaching the current peck target. If there is, it triggers the system to re-choose the target.

class PeckContext(DfContext):
def __init__(self, robot):
    super().__init__(robot)
    self.robot = robot
    self.reset()
    self.add_monitors([PeckContext.monitor_active_target_p])

    def reset(self):
        self.is_done = True
        self.active_target_p = None

    # Monitor whether a block is too close to the active target.
    def monitor_active_target_p(self):
        if self.active_target_p is not None and self.is_near_obs(self.active_target_p):
            self.is_done = True

    # Called by a special state at the end of the peck behavior.
    def set_is_done(self):
        self.is_done = True

    ...

class PeckState(DfState):
    def enter(self):
        target_p = self.context.active_target_p
        target_q = make_target_rotation(target_p)
        self.target = PosePq(target_p, target_q)
        approach_params = ApproachParams(direction=np.array([0.0, 0.0, -0.1]), std_dev=0.04)
        self.context.robot.arm.send_end_effector(self.target, approach_params=approach_params)

    def step(self):
        # Send the command each cycle so exponential smoothing will converge.
        target_dist = np.linalg.norm(self.context.robot.arm.get_fk_p() - self.target.p)
        if target_dist < 0.01:
            return None  # Exit
        return self  # Keep going

class Dispatch(DfDecider):
    def __init__(self):
        super().__init__()

        self.add_child("choose_target", ChooseTarget())
        self.add_child(
            "peck",
            DfStateMachineDecider(
                DfStateSequence(
                    [
                        CloseGripper(),
                        PeckState(),
                        DfTimedDeciderState(DfLift(height=0.05), activity_duration=0.25),
                        DfWriteContextState(lambda context: context.set_is_done()),
                    ]
                )
            ),
        )

    def decide(self):
        if self.context.is_done:
            return DfDecision("choose_target")
        else:
            return DfDecision("peck")


def make_decider_network(robot):
    return DfNetwork(Dispatch(), context=PeckContext(robot))

Note that the top-level Dispatch() decider can immediately pre-empt the sequential “peck” state machine if the monitor monitor_active_target_p() detects a block to close to the target.

Try moving the block under the end-effector. This time, every time the block gets too close to the end-effector’s target, it immediately chooses a different target.

3.2. Designing logical state contexts

Now let’s implement a simple game where the robot pecks the block that’s most recently been moved. We use decider networks to make it simple to program reactivity to the block movements. This example demonstrates how simple behaviors can be when the logical state is sufficiently modeled by the context object.

The behavior is implemented in behaviors/franka/peck_game.py. Run the behavior using:

isaac_python franka_examples_main.py --behavior=behaviors/franka/peck_game.py

The PeckContext class handles monitoring block movement and setting the latest active target accordingly. It also monitors whether the end-effector is close to the block which is useful in deciding whether the robot needs to lift away from the block before moving to it’s next target.

class PeckContext(DfLogicalState):
    def __init__(self, robot):
        super().__init__()
        self.robot = robot

        self.monitors = [
            PeckContext.monitor_block_movement,
            PeckContext.monitor_active_target_p,
            PeckContext.monitor_active_block,
            PeckContext.monitor_eff_block_proximity,
            PeckContext.monitor_diagnostics,
        ]

Given the logical state monitored by the context object, the main logic can be concisely written as the following Dispatch decider node:

class Dispatch(DfDecider):
    def enter(self):
        self.add_child("peck", PeckAction())
        self.add_child("lift", DfLift(height=0.1))
        self.add_child("go_home", make_go_home())

    def decide(self):
        if self.context.is_eff_close_to_inactive_block:
            return DfDecision("lift")

        if self.context.has_active_block:
            return DfDecision("peck")

        # If we aren't doing anything else, always just go home.
        return DfDecision("go_home")

In words, it reasons according to the following rules: If the end-effector is close to an inactive block, we need to just lift away from it (it’s too close). Otherwise, if there’s an active block, move to peck it. If no block is active, go home. This decide() method is ticked every cycle so immediately once the active block monitor notices a block has moved, it acts on it.