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 Windows->Extension Manager. This will enable the menu Isaac Robotics. select the option Isaac Robotics->Samples->UR10 Preview. A window with the UR10 preview extension will show up, like the below:

Plugin Menu
Plugin Menu

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.

    Material Loading
  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 registedr 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 + "/SmallKLT/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

    2. Press Perform Task

    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 correspoding 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.

    Material Loading
  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
31
32
33
self.ur10_table_usd = self.asset_path + "/Samples/Leonardo/Stage/ur10_bin_stacking_robotiq.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)
)
prim = self._stage.GetPrimAtPath("/World")
imageable = UsdGeom.Imageable(prim)
imageable.MakeInvisible()

# 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. Hoewever, 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

    2. Press Perform Task

    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 correspoding 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.