5. Example: State Machines¶
5.1. Learning Objectives¶
We’ll see here how to design state machines and an example of where subtle reactivity bugs can arise as a result of their structure.
Completed Overview and Fundamentals for an overview of cortex and its core concepts.
Completed Scripting Behaviors for an overview of how to setup and run your own behaviors.
Scripts throughout this tutorial are generally referenced relative to
5.2. Running the example¶
This tutorial is based on the example
To run the example:
./cortex launch # Launches the default franka blocks belief world. ./cortex activate franka/peck_state_machine.py # Run the example.
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.
5.3. An error case in reactivity¶
Now try moving a block directly into the path of a current peck. Since the state machine chooses the target on entry and keeps it fixed throughout the behavior, it will get stuck trying to reach the target since the obstacle is in the way.
State machines, by themselves, aren’t great at modeling reactive behavior. We use decider networks, in conjunction with state machines, to solve this problem in Example: Reactivity Using Deciders.
But first, let’s take a look at the simple state machine based implementation.
5.4. The Code¶
import numpy as np from omni.isaac.cortex.df import DfNetwork, DfBindableState, DfStateSequence, DfTimedDeciderState, DfStateMachineDecider from omni.isaac.cortex.dfb import DfToolsContext, DfLift, DfCloseGripper import omni.isaac.cortex.math_util as math_util from omni.isaac.cortex.motion_commander import MotionCommand, ApproachParams, PosePq def sample_target_p(): min_x = .3 max_x = .7 min_y = -.4 max_y = .4 pt = np.zeros(3) pt = (max_x-min_x) * np.random.random_sample() + min_x pt = (max_y-min_y) * np.random.random_sample() + min_y pt = .01 return pt def make_target_rotation(target_p): return math_util.matrix_to_quat(math_util.make_rotation_matrix( az_dominant=np.array([0., 0., -1.]), ax_suggestion=-target_p)) class PeckState(DfBindableState): def is_near_obs(self, p): for _,obs in self.context.tools.obstacles.items(): obs_p,_ = obs.get_world_pose() if np.linalg.norm(obs_p - p) < .2: return True return False def sample_target_p_away_from_obs(self): target_p = sample_target_p() while self.is_near_obs(target_p): target_p = sample_target_p() return target_p 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) self.approach_params = ApproachParams(direction=np.array([0.,0.,-.1]), std_dev=.04) def step(self): # Send the command each cycle so exponential smoothing will converge. self.context.tools.commander.set_command( MotionCommand(self.target, approach_params=self.approach_params)) target_dist = np.linalg.norm(self.context.tools.commander.get_fk_p() - self.target.p) if target_dist < .01: return None # Exit return self # Keep going def build_behavior(tools): tools.enable_obstacles() tools.commander.set_target_full_pose() # Build a state machine decider from a sequencial state machine. The sequence will be 1. close # gripper, 2. peck at target, 3. lift the end-effector. It's set to loop, so it will simply peck # repeatedly until the behavior is replaced. Note that PeckState chooses its target on entry. root = DfStateMachineDecider(DfStateSequence([ DfCloseGripper(width=.0), PeckState(), DfTimedDeciderState(DfLift(height=.05), activity_duration=.25)], loop=True)) return DfNetwork(decider=root, context=DfToolsContext(tools))
build_behavior(tools) method constructs the behavior as a
represents internally a sequential state machine defined by
DfStateSequence. On construction, we
setup that state sequence to be:
Send a close gripper command.
Peck. On entry, the peck state chooses its next target, then steps the state machine sending that command until it arrives that that target.
Lift away from its peck point just briefly before looping again.
loop=True in the
DfStateSequence so the state machine is looped indefinitely.