# UR10 Sample Applications¶

Below are two examples with step-by-step instructions on how to simulate complex behaviors with Omniverse Isaac Sim on a flattened state machine structure.

## Getting Started¶

Enable extension omni.isaac.samples from the extension manager menu Window -> Extensions to enable the UR10 sample.

To run the sample, first load the UR10 Palletizing scene by selecting the option Isaac Examples -> Scenes -> UR10 Palletizing. A window with the UR10 preview extension will show up as shown below:

## Example 1: Fill Bin¶

This example demonstrates a UR10 robot picking a bin from the ground and filling it with components until the suction gripper fails to support the bin in place.

### How to run¶

1. Enable the Samples extension as explained in Getting Started

2. Select Fill Bin task from drop down at the UR10 Preview screen.

3. Press Create Scenario and it will load the environment.

Note

Please wait for materials to get loaded. You can track progress on the bottom right corner of UI.

4. After the environment loads, press the Play button.

5. Press Perform Task and it will start picking up the bin.

6. Once the bin is picked up, it will lift it close to the dispenser, and objects will start falling into the bin.

7. Eventually the bin will become too heavy, or the impulse generated by the falling objects will be too strong, and the surface gripper will not stand and lose vacuum, making the bin fall to the ground and spill its contents.

8. The robot arm will try to locate and pick up the bin again, but if it falls too far from the arm’s reach, the robot may get stuck.

9. Pressing the Pause Task button in the UR10 Preview window will pause the robot automation, and provide a gizmo on screen where the user can manipulate the end effector target and make the robot move to a desired position.

10. Pressing on the Drop Parts button will drop more objects from the dispenser, at any time during the simulation.

11. Press the Stop button, or Reset Task to reset the demo.

### Code explained¶

This section describes the demo code, located in source/extensions/omni.isaac/samples/python/scripts/ur10_scenarios/fill_bin.py.

#### Class Fill_Bin¶

This is the main class that is executed by the script. It contains handles to the buttons events from the interface. It derives from a base Scenario class, that is used by all UR10 samples.

##### Function create_UR10¶

This function loads the robot environment (line 6) in simulation from USD at the location (0,0,0). Next, a reference transform is created to control the robot arm when the task is paused, and its location is set to (0, 0.75, 0.42) (in m), and the orientation of the target such that the camera is pointing down perpendicular to the ground, represented by the quaternion (-0.3342, 0.3339, 0.6231, 0.6234) (lines 8-12).

Then, a series of objects randomly selected from a list in the member self.objects are loaded on a far location, to be later used in the simulation.

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 self.ur10_table_usd = self.asset_path + "/Samples/Leonardo/Stage/ur10_bin_filling.usd" super().create_UR10() # Load robot environment and set its transform solid_robot = "/physics/scene/solid" self.env_path = "/environments/env" CreateSolidUR10(self._stage, self.env_path, self.ur10_table_usd, solid_robot, Gf.Vec3d(0, 0, 0)) # Set robot end effector Target orig = [0, 0.75, 0.42] self.default_position = _dynamic_control.Transform() self.default_position.p = orig self.default_position.r = [-0.3342, 0.3339, 0.6231, 0.6234] GoalPrim = self._stage.DefinePrim(self.env_path + "/target", "Xform") p = self.default_position.p r = self.default_position.r setTranslate(GoalPrim, Gf.Vec3d(p.x * 100, p.y * 100, p.z * 100)) setRotate(GoalPrim, Gf.Matrix3d(Gf.Quatd(r.w, r.x, r.y, r.z))) num_objs = self.max_objs a = [self.objects[random.randint(0, len(self.objects) - 1)] for i in range(num_objs)] b = [self.env_path + "/objects/object_{}".format(self.current_obj + i) for i in range(num_objs)] c = [Gf.Vec3d(-50000, 0, -50000 + 5 * i) for i in range(num_objs)] CreateObjects(self._stage, a, b, c) self.current_obj = 0 # Setup physics simulation SetupPhysics(self._stage) 
##### Function register_assets¶

This function will fetch the handles and register the obstacles for the various assets used in simulation and motion planning. It is called on the first step after the simulation starts, so it can obtain valid handles for the physics objects.

First, we fetch the rigid body handle for the green bin to be used in the pick and place task (line 4) from its USD path.

 1 2 3 4 # Prim path of the bin and its handle prim = self._stage.GetPrimAtPath(self.env_path) self.bin_path = self.env_path + "/bin/SmallKLT" self.bin_handle = self._dc.get_rigid_body(self.bin_path) 

Then, the world and the robot objects are created. The world object requires handles for the dynamic control (self._dc) and motion planning (self._mp) extensions. The robot requires both extension handles, the world object, and the properties for its surface gripper.

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 ur10_path = str(prim.GetPath()) + "/ur10" self.world = World(self._dc, self._mp) sgp = Surface_Gripper_Properties() sgp.parentPath = ur10_path + "/ee_link" sgp.d6JointPath = sgp.parentPath + "/d6FixedJoint" sgp.gripThreshold = 1 sgp.forceLimit = 5.0e3 sgp.torqueLimit = 5.0e4 sgp.bendAngle = np.pi / 24 # 7.5 degrees sgp.stiffness = 1.0e5 sgp.damping = 1.0e4 sgp.disableGravity = True tr = _dynamic_control.Transform() tr.p.x = 15.509 sgp.offset = tr self.ur10_solid = UR10( self._stage, self._stage.GetPrimAtPath(ur10_path), self._dc, self._mp, sgp, self.world, "/physics/scene/solid", default_config, urdf="/urdf/ur10_robot_robotiq.urdf", ) 

We create the state machine that will lift the box. The state machine needs to receive a handle to the current stage, the robot, the object to be lifted, the robot default pose, and the function handle to make the objects fall on the bin.

 1 2 3 4 5 6 7 8 self.pick_and_place = PickAndPlaceStateMachine( self._stage, self.ur10_solid, self._stage.GetPrimAtPath(self.env_path + "/ur10/ee_link"), self.bin_path, self.default_position, ) self.pick_and_place.add_bin = self.add_new_objects 

Finally, we disable the simulation for every object that is out of the active scene, so they won’t burden the simulation until they are necessary.

 1 2 3 4 5 6 7 8 self.objects_handles = [] for prim in self._stage.GetPrimAtPath(self.env_path + "/objects").GetChildren(): for o in prim.GetChildren(): if "Looks" not in o.GetPath().pathString: obj = self._dc.get_rigid_body(o.GetPath().pathString) self.objects_handles.append(obj) self._dc.set_rigid_body_disable_simulation(obj, True) break 
##### Function step¶

The scene is simulated with a discrete-time step, and the state machine is updated at every time step. Initially we check if the simulation is running, and if so, update the RMP references and robot references (lines 5-6). If the task is not paused we step the state machine (line 14), and check if the flag _reset was set. if so, reset the time, and set the robot target prim to the default pose, otherwise we update the prim target to the current end effector pose. However, if the flag _paused is set, it goes the other way around, with the end effector target being set to the prim target pose (lines 46-60).

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 def step(self, step): if self._editor.is_playing(): # Updates current references and locations for the robot. self.world.update() self.ur10_solid.update() target = self._stage.GetPrimAtPath("/environments/env/target") xform_attr = target.GetAttribute("xformOp:transform") if self._reset: self._paused = False if not self._paused: self._time += 1.0 / 60.0 self.pick_and_place.step(self._time, self._start, self._reset) if self._reset: self._paused = (self._time - self._start_time) < self.timeout_max self._time = 0 self._start_time = 0 p = self.default_position.p r = self.default_position.r setTranslate(target, Gf.Vec3d(p.x * 100, p.y * 100, p.z * 100)) setRotate(target, Gf.Matrix3d(Gf.Quatd(r.w, r.x, r.y, r.z))) else: state = self.ur10_solid.end_effector.status.current_target state_1 = self.pick_and_place.target_position tr = state["orig"] * 100.0 setTranslate(target, Gf.Vec3d(tr[0], tr[1], tr[2])) setRotate(target, Gf.Matrix3d(Gf.Quatd(state_1.r.w, state_1.r.x, state_1.r.y, state_1.r.z))) self._start = False self._reset = False if self.add_objects_timeout > 0: self.add_objects_timeout -= 1 if self.add_objects_timeout == 0: self.create_new_objects() if ( self.pick_and_place.current_state == self.current_state and self.current_state in [SM_states.PICKING, SM_states.ATTACH] and (self._time - self._start_time) > self.timeout_max ): self.stop_tasks() elif self.pick_and_place.current_state != self.current_state: self._start_time = self._time print(self._time) self.current_state = self.pick_and_place.current_state if self._paused: translate_attr = xform_attr.Get().GetRow3(3) rotate_x = xform_attr.Get().GetRow3(0) rotate_y = xform_attr.Get().GetRow3(1) rotate_z = xform_attr.Get().GetRow3(2) orig = np.array(translate_attr) / 100.0 axis_x = np.array(rotate_x) axis_y = np.array(rotate_y) axis_z = np.array(rotate_z) self.ur10_solid.end_effector.go_local( orig=orig, axis_x=axis_x, axis_y=axis_y, axis_z=axis_z, use_default_config=True, wait_for_target=False, wait_time=5.0, ) 

#### Class PickAndPlaceStateMachine¶

This class is a self-contained class for the a pick and place task. every state and event are defined by an enumerated list defined as SM_states and SM_events

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 class SM_events(Enum): START = 0 WAYPOINT_REACHED = 1 GOAL_REACHED = 2 ATTACHED = 3 DETACHED = 4 TIMEOUT = 5 STOP = 6 NONE = 7 # no event ocurred, just clocks class SM_states(Enum): STANDBY = 0 # Default state, does nothing unless enters with event START PICKING = 1 ATTACH = 2 HOLDING = 3 

Then, the transition functions are stored in a 2D matrix where the first position is the state, and the second position is the event. During Initialization, the state machine transition matrix is defined (line 12), and an empty event handler function is assigned for each state-event pair.Each state-event pair that requires handling and may transition the state machine to a next state is assigned a function to handle such event, and the appropriate threshold level is given for the waypoint clearing for every state.

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 # Threshold to clear waypoints/goal # (any waypoint that is not final will be cleared with the least precision) self.precision_thresh = [ [0.0005, 0.0025, 0.0025, 0.0025], [0.0005, 0.005, 0.005, 0.005], [0.05, 0.2, 0.2, 0.2], [0.08, 0.4, 0.4, 0.4], [0.18, 0.6, 0.6, 0.6], ] # Define the state machine handling functions self.sm = {} # Make empty state machine for all events and states for s in SM_states: self.sm[s] = {} for e in SM_events: self.sm[s][e] = self._empty self.thresh[s] = 0 # Fill in the functions to handle each event for each status self.sm[SM_states.STANDBY][SM_events.START] = self._standby_start self.sm[SM_states.STANDBY][SM_events.GOAL_REACHED] = self._standby_goal_reached self.thresh[SM_states.STANDBY] = 3 self.sm[SM_states.PICKING][SM_events.GOAL_REACHED] = self._picking_goal_reached self.sm[SM_states.PICKING][SM_events.NONE] = self._picking_no_event self.thresh[SM_states.PICKING] = 1 self.sm[SM_states.ATTACH][SM_events.GOAL_REACHED] = self._attach_goal_reached self.sm[SM_states.ATTACH][SM_events.ATTACHED] = self._attach_attached self.sm[SM_states.HOLDING][SM_events.GOAL_REACHED] = self._holding_goal_reached self.thresh[SM_states.HOLDING] = 3 for s in SM_states: self.sm[s][SM_events.DETACHED] = self._all_detached self.current_state = SM_states.STANDBY self.previous_state = -1 
##### Function step¶

The step function handles the different events that may happen at the scene, and make the appropriate calls for the event handlers

First, it checks for external events (start and reset), and checks if the gripper suction has been broken. then, it goes over each event flag in the precedence order, and if the event has occurred, calls the handler and quits the step.

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 def step(self, timestamp, start=False, reset=False): """ Steps the State machine, handling which event to call """ if self.current_state != self.previous_state: self.previous_state = self.current_state if not self.start: self.start = start if self.is_closed and not self.robot.end_effector.gripper.is_closed(): self._detached = True self.is_closed = False # Process events if reset: self.current_state = SM_states.STANDBY self.robot.end_effector.gripper.open() self.start = False self._upright = False self.waypoints.clear() self.target_position = self.default_position self.move_to_target() elif self._detached: self._detached = False self.sm[self.current_state][SM_events.DETACHED]() elif self.goalReached(): if len(self.waypoints) == 0: self.sm[self.current_state][SM_events.GOAL_REACHED]() else: self.target_position = self.waypoints.popleft() self.move_to_target() self.start_time = self._time elif self.current_state == SM_states.STANDBY and self.start: self.sm[self.current_state][SM_events.START]() elif self._attached: self._attached = False self.sm[self.current_state][SM_events.ATTACHED]() elif self._time - self.start_time > self.default_timeout: self.sm[self.current_state][SM_events.TIMEOUT]() else: self.sm[self.current_state][SM_events.NONE]() 

### Things to try¶

1. Move the robotic arm with task paused
1. Press the play button

3. Once robot starts to move, press Pause Task

4. click in one of the arrows in the target gizmo that showed on screen, and drag it with the mouse
• If you want to rotate the orientation, select the rotate tool on the toolbar, and click on the circles corresponding to the rotation axis you want to change.

Note

A few scenarios to avoid while running the sample application:

1. When the bin is moved to an unreachable location for the robot to pick, press Stop/Reset Task button to reset the environment.

2. When the bin falls off at an angle that would cause the robot to be stuck and not grab it again, press Stop/Reset Task button to reset the environment.

## Example 2: Bin Stacking¶

This example shows the UR10 performing an elaborate behavior of stacking the bins on top of a pallet

### How to run¶

1. Enable the Samples extension as explained in Getting Started

2. Select Stack Bins task from drop down at the UR10 Preview screen.

3. Press Create Scenario and it will load the environment.

Note

Please wait for materials to get loaded. You can track progress on the bottom right corner of UI.

4. After the environment loads, press the Play button.

5. Press Perform Task. A bin will start rolling in the conveyor, and once it reaches the bottom, the robot will start picking it up.

6. Once the bin is picked up, if it’s upside down, it will be placed on the stack, otherwise it will first be placed on the flipping base so the robot can re-grip from the bottom.

7. After the bin is placed on the pile, the arm will return to its rest pose, and if another bin is already waiting on the conveyor, it will be picked up and the process will continue.

8. The robot will stop once all 36 bins have been stacked.

9. Pressing the Pause Task button will stop the robot automation, and provide a gizmo on screen where the user can manipulate the end effector target and make the robot move to a desired position.

10. Pressing on the Add bin button will make another bin appear on the conveyor.

11. Press the Stop button, or Reset Task to reset the demo.

### Code explained¶

This section describes the demo code, located in source/extensions/omni.isaac/samples/python/scripts/ur10_scenarios/bin_stack.py.

#### Class BinStack¶

This is the main class that is executed by the script. It contains handles to the buttons events from the interface. It derives from a base Scenario class, that is used by all UR10 samples.

##### Function create_UR10¶

This function loads the robot environment (line 6) in simulation from USD at the location (0,0,0).

A reference transform is created to control the robot arm when the task is paused, and its location is set to (-0.645, 0.7214, 0.495) (in m), and the orientation of the target is pointing such that the camera is pointing down perpendicular to the ground, represented by the quaternion (-0.3342, 0.3339, 0.6231, 0.6234) (lines 14-18).

Next, a set of 32 bins are created off the stage so they can be used later in the simulation (lines 8-12).

After loading the warehouse background and hiding any portion of the scenario that was loaded by default from a new stage (lines 24-29), it sets up the physics scene for the simulation (line 32).

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 self.ur10_table_usd = self.asset_path + "/Samples/Leonardo/Stage/ur10_bin_stacking_short_suction.usd" super().create_UR10() # Load robot environment and set its transform solid_robot = "/physics/scene/solid" self.env_path = "/environments/env" CreateSolidUR10(self._stage, self.env_path, self.ur10_table_usd, solid_robot, Gf.Vec3d(0, 0, 0)) # Set robot end effector orig = [-0.0645, 0.7214, 0.495] self.default_position = _dynamic_control.Transform() self.default_position.p = orig self.default_position.r = [-0.3342, 0.3339, 0.6231, 0.6234] GoalPrim = self._stage.DefinePrim(self.env_path + "/target", "Xform") p = self.default_position.p r = self.default_position.r setTranslate(GoalPrim, Gf.Vec3d(p.x * 100, p.y * 100, p.z * 100)) setRotate(GoalPrim, Gf.Matrix3d(Gf.Quatd(r.w, r.x, r.y, r.z))) a = [self.small_klt_usd for i in range(self.max_bins)] b = [self.env_path + "/bins/bin_{}".format(i) for i in range(self.max_bins)] c = [Gf.Vec3d(-50000 - 50 * i, 150, 0) for i in range(self.max_bins)] CreateObjects(self._stage, a, b, c) CreateBackground( self._stage, self.background_usd, [5747.25, 1826.020, -118.180], Gf.Quatd(0.7071, 0, 0, 0.7071) ) # Setup physics simulation SetupPhysics(self._stage) 
##### Function register_assets¶

This function will register various assets used in simulation and motion planning. It is called on the first step after the simulation starts, so it can obtain valid handles for the physics objects.

First, we fetch the rigid body handles for the bins to be used in the pick and place task (line 4) from their USD path.

 1 2 3 4 # Prim path of two blocks and their handles prim = self._stage.GetPrimAtPath(self.env_path) self.bin_paths = [self.env_path + "/bins/bin_{}/SmallKLT".format(i) for i in range(self.max_bins)] self.bin_handles = [self._dc.get_rigid_body(i) for i in self.bin_paths] 

Then, we create the world handle, which requires handles for the Dynamic Control (self._dc) and Motion Planning (self._mp) Libraries.

 1 2 # Create world and robot object self.world = World(self._dc, self._mp) 

Next, the UR10 robot handle is created, using the robot prim, and surface gripper properties for the gripper joint, as well as handles for the Dynamic Control and Motion Planning Libraries, and the handle to the World.

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 ur10_path = str(prim.GetPath()) + "/ur10" self.world = World(self._dc, self._mp) mjp = Surface_Gripper_Properties() mjp.parentPath = ur10_path + "/ee_link" mjp.d6JointPath = mjp.parentPath + "/d6FixedJoint" mjp.gripThreshold = 1 mjp.forceLimit = 5.0e3 mjp.torqueLimit = 5.0e4 mjp.bendAngle = np.pi / 24 # 7.5 degrees mjp.stiffness = 1.0e5 mjp.damping = 1.0e4 mjp.disableGravity = True tr = _dynamic_control.Transform() tr.p.x = 15.509 mjp.offset = tr self.ur10_solid = UR10( self._stage, self._stage.GetPrimAtPath(ur10_path), self._dc, self._mp, mjp, self.world, "/physics/scene/solid", default_config, urdf="/urdf/ur10_robot_robotiq.urdf", ) 

Then, for every element that the robot interacts with, we register the obstacles in the motion planner.

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24  i = 0 for p in self._stage.GetPrimAtPath(str(prim.GetPath()) + "/sortbot_housing/RMPObstacle").GetChildren(): self.world.register_object(0, p.GetPath().pathString, "housing_" + str(i)) self.world.make_obstacle("housing_" + str(i), 3, p.GetAttribute("xformOp:scale").Get()) i += 1 i = 0 for p in self._stage.GetPrimAtPath(str(prim.GetPath()) + "/pallet_holder/RMPObstacle").GetChildren(): self.world.register_object(0, p.GetPath().pathString, "holder_" + str(i)) self.world.make_obstacle("holder_" + str(i), 3, p.GetAttribute("xformOp:scale").Get()) self.world.get_object_from_name("holder_" + str(i)).suppress() i += 1 self._bin_holder_obstacle = self.world.get_object_from_name("holder_0") self._bin_objects = {} for i, (bin_handle, bin_path) in enumerate(zip(self.bin_handles, self.bin_paths)): self.world.register_object(bin_handle, bin_path, "{}_bin".format(i)) self.world.make_obstacle("{}_bin".format(i), 3, np.asarray(self.small_bin_scale)) obj = self.world.get_object_from_name("{}_bin".format(i)) self._dc.set_rigid_body_disable_simulation(bin_handle, True) obj.suppress() self._bin_objects[bin_path] = obj self._obstacles.append(obj) i = 0 

Finally, we create the robot state machine, which will make the robot stack the bins as they come on the conveyor.

 1 2 3 4 5 6 7 8 9 self.pick_and_place = PickAndPlaceStateMachine( self._stage, self.ur10_solid, self._stage.GetPrimAtPath(self.env_path + "/ur10/ee_link"), self._bin_objects, self.default_position, self._bin_holder_obstacle, ) self.pick_and_place.add_bin = self.add_new_bin 
##### Function step¶

The scene is simulated with a discrete-time step, and the state machine is updated at every time step. Initially we check if the simulation is running, and if so, update the motion planning references and robot references (lines 9-10). If the task is not paused (lines 14-35), we step the state machine (line 18), and check if the flag _reset was set. if so, reset the time, and set the robot target prim to the default pose, otherwise we update the prim target to the current end effector pose. However, if the flag _paused is set, it goes the other way around, with the end effector target being set to the prim target pose (lines 24-29). We initially clear all the pending waypoints from the state machine, and then convert the target prim in the input format for the motion planning call.

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 def step(self, step): if self._editor.is_playing(): # Disable requires a one simulation step after they have been moved # from their previous location to work. if self._pending_disable: self.disable_bins() # Updates current references and locations for the robot. self.world.update() self.ur10_solid.update() target = self._stage.GetPrimAtPath("/environments/env/target") xform_attr = target.GetAttribute("xformOp:transform") if self._reset: self._paused = False if not self._paused: self._time += 1.0 / 60.0 if self._start and self.current_bin == 0: self.create_new_bin() self.pick_and_place.step(self._time, self._start, self._reset) if self._reset: self._paused = True self._time = 0 p = self.default_position.p r = self.default_position.r setTranslate(target, Gf.Vec3d(p.x * 100, p.y * 100, p.z * 100)) setRotate(target, Gf.Matrix3d(Gf.Quatd(r.w, r.x, r.y, r.z))) else: state = self.ur10_solid.end_effector.status.current_target state_1 = self.pick_and_place.target_position tr = state["orig"] * 100.0 setTranslate(target, Gf.Vec3d(tr[0], tr[1], tr[2])) setRotate(target, Gf.Matrix3d(Gf.Quatd(state_1.r.w, state_1.r.x, state_1.r.y, state_1.r.z))) self._start = False self._reset = False if self.add_bin_timeout > 0: self.add_bin_timeout -= 1 if self.add_bin_timeout == 0: self.create_new_bin() if self._paused: translate_attr = xform_attr.Get().GetRow3(3) rotate_x = xform_attr.Get().GetRow3(0) rotate_y = xform_attr.Get().GetRow3(1) rotate_z = xform_attr.Get().GetRow3(2) orig = np.array(translate_attr) / 100.0 axis_x = np.array(rotate_x) axis_y = np.array(rotate_y) axis_z = np.array(rotate_z) self.ur10_solid.end_effector.go_local( orig=orig, axis_x=axis_x, axis_y=axis_y, axis_z=axis_z, use_default_config=True, wait_for_target=False, wait_time=5.0, ) 

#### Class PickAndPlaceStateMachine¶

This class is a self-contained class for the a pick and place task. every state and event are defined by an enumerated list defined as SM_states and SM_events. The events are handled using flags and processed during the step function.

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 class SM_events(Enum): START = 0 WAYPOINT_REACHED = 1 GOAL_REACHED = 2 ATTACHED = 3 DETACHED = 4 TIMEOUT = 5 STOP = 6 BROKENGRIP = 7 NONE = 8 class SM_states(Enum): STANDBY = 0 PICKING = 1 ATTACH = 2 PLACING = 3 DETACH = 4 FLIPPING = 5 

Then, the transition functions are stored in a 2D matrix where the first position is the state, and the second position is the event. During Initialization, the state machine transition matrix is defined (line 24), and an empty event handler function is assigned for each state-event pair. Each state-event pair that requires handling and may transition the state machine to a next state is assigned a function to handle such event, and the appropriate threshold level is given for the waypoint clearing for every state.

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 # Threshold to clear waypoints/goal # (any waypoint that is not final will be cleared with the least precision) self.precision_thresh = [ [0.0005, 0.0025, 0.0025, 0.0025], [0.0005, 0.005, 0.005, 0.005], [0.05, 0.2, 0.2, 0.2], [0.08, 0.4, 0.4, 0.4], [0.18, 0.6, 0.6, 0.6], ] self.add_bin = None # Event management variables # Used to verify if the goal was reached due to robot moving or it had never left previous target self._is_moving = False self._attached = False # Used to flag the Attached/Detached events on a change of state from the end effector self._detached = False self._upright = False # Used to indicate if the bin is being picked facing up, so the proper state is called self._flipped = False self._closed = False self.pick_count = 0 # Define the state machine handling functions self.sm = {} # Make empty state machine for all events and states for s in SM_states: self.sm[s] = {} for e in SM_events: self.sm[s][e] = self._empty self.thresh[s] = 0 # Use a same event handler for broken grip on all events for s in SM_states: self.sm[s][SM_events.BROKENGRIP] = self._all_broken_grip # Fill in the functions to handle each event for each status self.sm[SM_states.STANDBY][SM_events.START] = self._standby_start self.sm[SM_states.STANDBY][SM_events.GOAL_REACHED] = self._standby_goal_reached self.thresh[SM_states.STANDBY] = 3 self.sm[SM_states.PICKING][SM_events.GOAL_REACHED] = self._picking_goal_reached self.sm[SM_states.PICKING][SM_events.NONE] = self._picking_no_event self.thresh[SM_states.PICKING] = 1 self.sm[SM_states.FLIPPING][SM_events.GOAL_REACHED] = self._flipping_goal_reached self.thresh[SM_states.FLIPPING] = 2 self.sm[SM_states.PLACING][SM_events.GOAL_REACHED] = self._placing_goal_reached self.thresh[SM_states.PLACING] = 0 self.sm[SM_states.ATTACH][SM_events.GOAL_REACHED] = self._attach_goal_reached self.sm[SM_states.ATTACH][SM_events.ATTACHED] = self._attach_attached self.thresh[SM_states.ATTACH] = 0 self.sm[SM_states.DETACH][SM_events.GOAL_REACHED] = self._detach_goal_reached self.sm[SM_states.DETACH][SM_events.DETACHED] = self._detach_detached self.thresh[SM_states.DETACH] = 0 self.current_state = SM_states.STANDBY self.previous_state = -1 

### Things to try¶

1. Manually stack a bin
1. Press the play button

3. Once robot starts to move, press Pause Task

4. Click in one of the arrows in the target gizmo that showed on screen, and drag it with the mouse
• If you want to rotate the orientation, select the rotate tool on the toolbar, and click on the circles corresponding to the rotation axis you want to change.

5. Pick a bin in the conveyor belt.
• Once the gripper is touching the surface of the bin, click on Close/Open Gripper

6. Move it either to the stack or to the flipping station to pick it up from the bottom

7. Drop the bin on the desired pose by clicking on Close/Open Gripper.

Note

A few scenarios to avoid while running the sample application:

1. When the bin is moved to an unreachable location for the robot to pick, press Stop/Reset Task button to reset the environment.

2. In Manual override, avoid rotating the arm too much to either side, as the robot’s joints have a limit of one full turn to each side, making the robot get stuck.