Pick & Place App Tutorial

This chapter is a hands-on introduction to writing Apps. At the end, the App you create will be able to pick an object and place it in a user-taught position. While placing the object, the robot will monitor contact forces, and open the gripper once it touches the surface. This will compensate for inaccuracies in place position and allow for easy stacking.

The App will be created step by step, and we will be adding the functionality in the following order:

  • Print messages to the log

  • Controlling the gripper

  • Transforming a state machine into an App

  • Moving the robot arm

  • Monitoring if the grasped object was lost

  • Checking for expected collisions

Hello World

To familiarize ourselves with the ride-cli tool, we will start by installing a simple state machine that prints a message.

All state machines are installed as part of bundles. The bundle in this chapter will be called tutorial:

ride bundle new bundles/tutorial

This will create the following folder structure and two text files:

.
└── bundles
  └── tutorial
      ├── manifest.json
      ├── resources
      └── sources
         └── Tutorial.lf

The manifest.json file indicates the name and the version of a bundle. When installing a bundle on a robot which already has a bundle with the same name, it will only be updated if the version is the same or newer.

The Tutorial.lf file will contain the code we create in this tutorial. Let us start by creating a single state which will print a message to the log.

Tutorial {
  port Success true
  entry @{
    printf("Hello World!")
  }@
}

This creates a root state Tutorial. The root must always have the same name as the file it is contained in. Our state has one exit port called Success. After the port's name we put a condition, which defines when the execution should exit through that port. In this case, the condition is simply true, so it will immediately exit.

Finally, we attach a Lua script block (between @{...}@) onto the entry action. Whenever the Tutorial state is entered, a "Hello World!" message will be printed onto the log.

Compiling our Bundle

Next we have to compile our bundle. We will specify a custom build folder where ride should put the built bundle.

ride bundle compile bundles/tutorial

This will produce our Tutorial bundle which we can install on the robot.

Installing the Bundle

To install our tutorial bundle, you must first log-in using ride

ride login SERVER

SERVER is the same address you would use to connect to Desk. The command uses robot.franka.de by default.

ride bundle install bundles/tutorial.bundle

You should be able now to see the tutorial bundle listed when using:

ride bundle list

Running our State Machine

New state machines cannot be started when others are currently running. Right now, the robot is most likely running the idle_statemachine state machine. It is started by the App server at startup and whenever a Task from Desk stops. Stop it using ride cli stop. Run ride cli log in a separate terminal to see the log. Then, start our newly installed state machine with:

ride execution start -t Tutorial

Where -t stands for tracing. You should obtain an output similar to this one:

ride execution start -t Tutorial
Selected revision: 77
Filtered tracing of path Tutorial

Execution (155) [FINISHED]

You should see "Hello World!" printed in the log.

Opening and Closing the Gripper

Now we will add states responsible for opening and closing the gripper. Instead of writing the functionality for controlling the gripper from scratch, we will reuse states contained in the gripper bundle. To add two new states to Tutorial.lf, modify it as follows:

Tutorial {
  port Success child("gripper_close").port("success")
  port Error child("gripper_open").port("error") or
                  child("gripper_close").port("error")

  entry @{
    printf("Hello World!")
  }@

  --> gripper_open <- gripper_move {
    port success -> gripper_close
  } where {
    width: 0.1;
    speed: 0.2;
  }

  gripper_close <- grasp {
  } where {
    width: 0.05;
    speed: 0.1;
    force: 20.0;
  }
}

We nest gripper_open and gripper_close as child states of Tutorial. By using the <- arrow we declare those two as link states, inheriting all the elements of the gripper_move and grasp states. The library will take care of calling the necessary operations to operate the gripper. The --> arrow declares gripper_open as the first child, which will be activated whenever the root state is activated. The where { ... } clause is used to parameterize the states, here simply using hardcoded values. And finally, the port statements were modified as well:

port Success child("gripper_close").port("success")

This line specifies that the Tutorial state should exit through the Success port whenever the nested state's port success is reached. Note that we use the success port which gripper_close inherits from grasp.

Before we can successfully build our tutorial bundle again, we have to specify any extra dependency our bundle uses as a build dependency (see Handling Dependencies) However since gripper is a builtin dependency, we don't need to add the bundle to our manifest.

ride bundle compile bundles/tutorial --ignore-dependencies -o build/tutorial.bundle
ride bundle install build/tutorial.bundle

Warning

TODO: Check builtin dependencies (such as gripper) against the robot, when connected. This way you must not opt out of the dependency check entirely with the --ignore-dependencies flag when using builtin bundles.

Then start the modified state machine using the commands from above. The gripper should now open and close. The trace indicates how execution transfers from gripper_open to gripper_close. Notice that the parent state Tutorial remains active:

Execution (5) of Tutorial [FINISHED]

Execution (6) of Tutorial [RUNNING]
Tutorial.gripper_open [ACTIVE]

Execution (6) of Tutorial [RUNNING]
Tutorial.gripper_close [ACTIVE]

Execution (6) of Tutorial [FINISHED]

These states interact with the gripper service. You can see which operations and events are made available using the ride service list command. You can call ride service echo gripper gripper_state in a separate terminal to observe how the gripper_state is modified during the execution of Tutorial state machine.

Wrapping State Machines in Apps

To be used in Franka Desk, the state machine needs to be defined as an app. Apps are state machines which match a specific interface. Apps must have at least one port named Success and one port named Error. In addition, the resultType must contain at least one string field named error_cause, which provides an indication of possible problems for the user in case of an error. Additional fields are possible though. The following code snippet shows the required content of an app.

name {
  port Success true
  port Error false

  clientData {
    type : "app";
    name : "Name";
  }

  resultType {
    string error_cause;
  }
}

In addition to the introduced parts of a state machine, an app contains client data, which describes the visualization of the app in Franka Desk.

  • type [mandatory] (String): "app"

  • name [mandatory] (String): Displayed name of the App

  • color (String): The display color of the app

  • image (String): A string containing an SVG image to be used as the app's icon

  • requiredParameters (Array[Object]): An array of objects which contain source parameters defined by

    • source: The name of the parameter in the source

    • localParameter: The name of the parameter as it is used in the state machine

  • contextMenu (Script): The definition of steps to configure the app, refer to Creating Context Menus for Apps for a detailed explanation

  • components (Script): Custom components used in the app, refer to Custom Components for more information

To use the Tutorial state machine as an app, we add the clientData, resultType and some actions to pass the error_cause of the child states to the root state.

clientData {
  type : "app";
  name : "Pick Contact";
  color : "#B6E591";
  requiredParameters: [{source: speed; localParameter: speed;}];
  image : @{
    <svg><use href="bundles/tutorial/Tutorial_logo.svg#icon"></use></svg>
  }@;
}

resultType {
  string error_cause;
}

action child("gripper_open").port("error") @{
  setResult({error_cause = child("gripper_open").result.error_cause})
}@
action child("gripper_close").port("error") @{
  setResult({error_cause = child("gripper_close").result.error_cause})
}@

The following image depicts how an app with a certain color and image appears in the library in Franka Desk. The name is shown below and should be kept short.

_images/pick.png

To access the Tutorial_logo.svg file, create a folder named resources inside the tutorial folder and place the image there (see Guidelines for SVG Images for more detailed information).

.
└── bundles
  └── tutorial
      ├── manifest.json
      ├── resources
      │  └── Tutorial_logo.svg
      └── sources
         └── Tutorial.lf

Compile and install as before. You should now see a block similar to the one depicted above appear in Desk. Drag it onto a new Task and press start. The gripper should open and close as before.

When you trace the execution using ride execution trace, notice how your Tutorial state now contains a nested state called app. Since tasks are also state machines, you can find the name of your task in the list obtained from ride node list, and similarly start it using ride execution start.

User Parameterization

We want to make the gripper width parameterizable by the user of the app. We therefore introduce parameters, i.e. gripper_open_width and gripper_closed_width. We will later create a context menu for the app, which contains components to manipulate those parameters in Desk (see Creating Context Menus for Apps). We replace the hard coded values for width with the new parameters and set default values in the where { ... } clause.

  parameterType {
    float gripper_open_width;
    float gripper_closed_width;
  }

  --> gripper_open <- gripper_move {
    port success -> gripper_close
  } where {
    width: parameter.gripper_open_width;
    speed: 0.1;
  }

  gripper_close <- grasp {
  } where {
    width: parameter.gripper_closed_width;
    speed: 0.1;
    force: 20.0;
  }
} where {
  gripper_open_width: nil;
  gripper_closed_width: nil;
}

If the parameter is initialized with nil, the user is forced to parameterize it, because an app can only be executed if all parameters are defined.

In order to parameterize this from Desk, the context menu entry needs now be added to the clientData:

clientData {
  contextMenu : @{
    <step id="pick-motion" name="Gripper Width" class="flex-column">
      <step id="approach-width" name="Open fingers">
        <gripper-control params="
          width: parameter('gripper_open_width'),
          step: step
        "></gripper-control>
      </step>
      <step id="pick-width" name="Close fingers">
          <gripper-control params="
            width: parameter('gripper_closed_width'),
            step: step
          "></gripper-control>
      </step>
    </step>
  }@;
  ...
}

You can also download the .lf file with a basic context menu here. After installing the App, you should be able to set the gripper width from Desk.

Moving the Robot

Now that we can control the gripper, we need to move to the object we want to grasp.

The overall strategy is as follows:

  1. Open the gripper.

  2. Move to the object.

  3. Close the gripper.

  4. Retract.

Apart from these three new states to handle the motion, we need one more to calculate the trajectory parameters.

We will introduce the necessary changes step by step, but you can also see the full .lf file here first, and follow along.

First, the parameters of the two robot poses we want the user to set are added. The pick_approach pose will also be used for retract, i.e. the robot will move from the approach pose to the pick pose and back to the approach pose during the execution. The speed parameter is inherited from the source, as defined in the requiredParameters in the clientData. The velocity parameter will be used to control the overall speed of the movement.

parameterType {
  {
    [16]float pose;
    []float joint_angles;
  } pick_approach;
  {
    [16]float pose;
    []float joint_angles;
  } pick_pose;
  float speed;
  float velocity;
  float gripper_open_width;
  float gripper_closed_width;
}

Note that you can use structs and array types for your parameters as is the case for the poses.

We will use move_via_with_move_configuration state to move the robot, but first we need to merge the taught poses into a trajectory. This will be done with the help of the merge_trajectory_and_kinematic_parameters. Add a new start state:

--> compute_parameters <- merge_trajectory_and_kinematic_parameters {
  port done -> gripper_open
} where {
  add_to_approach: true;
  add_to_retract: false;
  approach: [parameter.pick_approach];
  approach_end: parameter.pick_pose;
  retract_start: parameter.pick_pose;
  retract: [parameter.pick_approach];
  cartesian_velocity_factor_approach: parameter.velocity*parameter.speed;
  cartesian_acceleration_factor_approach: 0.8*parameter.velocity*parameter.speed;
  cartesian_deceleration_factor_approach: 0.8*parameter.velocity*parameter.speed;
  cartesian_velocity_factor_ptp: parameter.velocity*parameter.speed;
  cartesian_acceleration_factor_ptp: 0.8*parameter.velocity*parameter.speed;
  cartesian_deceleration_factor_ptp: 0.8*parameter.velocity*parameter.speed;
  cartesian_velocity_factor_retract: parameter.velocity*parameter.speed;
  cartesian_acceleration_factor_retract: 0.8*parameter.velocity*parameter.speed;
  cartesian_deceleration_factor_retract: 0.8*parameter.velocity*parameter.speed;
}

Note that the parameters approach and retract expect trajectories. As we only teach one approach pose, we wrap the parameter with square brackets. The overall speed of the movement is influenced by two parameters: The speed parameter which is task-specific and the velocity parameter which is app-specific.

Next, we can introduce the movement states:

gripper_open <- gripper_move {
  port success -> approach
} where {
  width: parameter.gripper_open_width;
  speed: 0.1;
}

approach <- move_via_with_move_configuration {
  port success -> gripper_close
} where {
  poses: child("compute_parameters").result.approach;
}

gripper_close <- grasp {
  port success -> retract
} where {
  width: parameter.gripper_closed_width;
  speed: 0.1;
  force: 20.0;
}

retract <- move_via_with_move_configuration {
} where {
  poses: child("compute_parameters").result.retract;
}

Notice that gripper_open is no longer the start state, and how we use child("compute_parameters").result to access the results of compute_parameters in its siblings.

Having defined our states, we can now go back to update our ports:

port Success child("retract").port("success")
port Error child("approach").port("error") or
           child("gripper_open").port("error") or
           child("gripper_close").port("error") or
           child("retract").port("error")

And update the error messages and parameter initialization:

  action child("gripper_open").port("error") @{
    setResult({error_cause = child("gripper_open").result.error_cause})
  }@
  action child("approach").port("error") @{
    setResult({error_cause = child("approach").result.error_cause})
  }@
  action child("retract").port("error") @{
    setResult({error_cause = child("retract").result.error_cause})
  }@
  action child("gripper_close").port("error") @{
    setResult({error_cause = child("gripper_close").result.error_cause})
  }@
} where {
  pick_approach: nil;
  pick_pose: nil;
  speed: nil;
  velocity: nil;
  gripper_open_width: nil;
  gripper_closed_width: nil;
}

You can download the .lf file with a basic context menu here. To have a proper visualization, place gripper_opened.svg, gripper_holding.svg and object.svg inside the resources folder.

Install your state machine on the robot. If you have Tasks utilizing a previous versions of your Tutorial App already, you first might have to delete the instances of Tutorial to prevent conflicts. If the App was installed correctly, you should now be able to teach the poses. Run the App and watch the robot grasp an object!

Barriers

Next, we want to transport our object. While we move the robot, we would like to monitor the gripper service and exit with an error if the grasped object is lost during transit. In such cases, we can use barriers for parallel execution.

For better modularization, we will implement that functionality in a separate file. Create a move_monitored.lf file next to Tutorial.lf, and enter the following:

move_monitored {
  port success child("move").port("success")
  port error child("move").port("error") or
             child("observe_part_lost").port("part_lost")

  parameterType {
    []{
      {[16]float pose;} pose;
      bool point2point;
      float cartesian_velocity_factor;
      float cartesian_acceleration_factor;
      float cartesian_deceleration_factor;
      float q3;
    } poses;
  }

  resultType {
    string error_cause;
  }

  --> barrier move_and_check {
    -> move
    -> observe_part_lost
  }

  move <- move_via_with_move_configuration {
  } where {
    poses: parameter.poses;
  }

  observe_part_lost <- gripper_observer {
  }

  action child("observe_part_lost").port("part_lost") @{
    setResult({ error_cause = "Part was lost during motion." })
  }@

  action child("move").port("error") @{
    setResult({ error_cause = child("move").result.error_cause })
  }@
}

The barrier keyword indicates that move_and_check node is a barrier. It activates two states in parallel, move and observe_part_lost:

--> barrier move_and_check {
  -> move
  -> observe_part_lost
}

The move_observe_gripper ports are connected to move's ports as before, but the error port can also be reached when observe_part_lost exits through part_lost. If that happens, the execution of move will be preempted.

To compute the pose for our new state, let's add a compute_parameters_place state to Tutorial.lf:

compute_parameters_place <- merge_trajectory_and_kinematic_parameters {
  port done -> gripper_open
} where {
  add_to_approach: true;
  add_to_retract: false;
  approach: [parameter.place_approach];
  approach_end: parameter.place_pose;
  retract_start: parameter.place_pose;
  retract: [parameter.place_approach];
  cartesian_velocity_factor_approach: parameter.velocity*parameter.speed;
  cartesian_acceleration_factor_approach: 0.8*parameter.velocity*parameter.speed;
  cartesian_deceleration_factor_approach: 0.8*parameter.velocity*parameter.speed;
  cartesian_velocity_factor_ptp: parameter.velocity*parameter.speed;
  cartesian_acceleration_factor_ptp: 0.8*parameter.velocity*parameter.speed;
  cartesian_deceleration_factor_ptp: 0.8*parameter.velocity*parameter.speed;
  cartesian_velocity_factor_retract: parameter.velocity*parameter.speed;
  cartesian_acceleration_factor_retract: 0.8*parameter.velocity*parameter.speed;
  cartesian_deceleration_factor_retract: 0.8*parameter.velocity*parameter.speed;
}

Now we can use our move_monitored state! After using it to touch the surface, we want to open the gripper. Let's add two new states: transport and gripper_open_place:

retract <- move_via_with_move_configuration {
  port success -> transport
} where {
  poses: child("compute_parameters").result.retract;
}

transport <- move_monitored {
  port success -> gripper_open_place
} where {
  poses: child("compute_parameters_place").result.approach;
}

gripper_open_place <- gripper_move {
} where {
  width: parameter.gripper_place_open_width;
  speed: 0.1;
}

Another step was added to the context menu, allowing the user to teach the placing motion. Similarly as before, the ports, parameters, context menu and resulting string assignments have to be updated. See here for the full changes.

Execute your App and try to remove the object from the gripper during motion. The App should stop with an error message: "Part was lost during motion".

Moving until Contact

At this stage, we have a functioning pick and place App. Let's extend it by moving to contact for the place motion. When the robot collides, instead of stopping with an error, we will check if the collision took place within a defined area. If it did, we will recover from the error, and continue the execution instead.

Add a check_error.lf state:

check_error {
  port success result ~= nil and result.expected -> reset
  port error result ~= nil and result.expected == false
  parameterType {
      [16]float expected_pose;
      string error_cause;
      {
        [7]float tau_ext;
        [6]float K_F_ext_hat_K;
        {
          [16]float pose;
        } O_T_EE;
        [2]float elbow;
        int cartesian_goal_id;
        [7]float q;
        [6]float cartesian_collision;
        [7]float joint_collision;
        []string robot_errors;
      } move_error;
      {
        float x;
        float y;
        float z;
      } contact;
  }
  resultType {
    bool expected;
    string error_cause;
  }
  entry @{
    setResult(nil)
  }@
  action result == nil @{
    if parameter.move_error == nil or not utils.contains(parameter.move_error.robot_errors, {"cartesian_reflex_flag", "joint_reflex_flag"}) then
      setResult({expected = false, error_cause = "Unexpected error. " .. parameter.error_cause })
    else
      local current_ee = parameter.move_error.O_T_EE.pose
      if (abs(current_ee[13] - parameter.expected_pose[13]) > parameter.contact.x or
          abs(current_ee[14] - parameter.expected_pose[14]) > parameter.contact.y or
          abs(current_ee[15] - parameter.expected_pose[15]) > parameter.contact.z) then
        setResult({expected = false, error_cause = "Error outside of defined radius. " .. parameter.error_cause})
      else
        setResult({expected = true})
      end
    end
  }@
}

The new state is used in move_monitored.lf:

move <- move_via_with_move_configuration {
  port error -> check
} where {
  poses: parameter.poses;
}

check <- check_error {
  port success -> reset
} where {
  expected_pose: parameter.poses[#parameter.poses].pose.pose;
  contact: parameter.contact;
  error_cause: child("move").result.error_cause;
  move_error: child("move").result.move_error;
}

reset <- wait_until_reset_collision {
}

The error port leads to check state, where its move_error result is inspected. The # operator in #parameter.poses returns the length of its operand. Here it's used to obtain the last pose passed to the move state. The collision area check will be computed with respect to that position.

Port definitions have to be updated, see here for full changes.

The new parameter, contact, has to be added in Tutorial.lf:

{
  float x;
  float y;
  float z;
} contact;

The values should be passed into move_monitored state:

transport <- move_monitored {
  port success -> gripper_open_place
} where {
  poses: child("compute_parameters_place").result.approach;
  contact: parameter.contact;
}

And, finally, we can add some default values for the contact area:

} where {
  pick_approach: nil;
  pick_pose: nil;
  speed: nil;
  velocity: nil;
  gripper_open_width: nil;
  gripper_closed_width: nil;
  place_approach: nil;
  place_pose: nil;
  contact: {
    x: 0.01;
    y: 0.01;
    z: 0.01;
  };
}

See the full version of the file here.

If you'd like to learn more by improving the App further, here are possible next steps:

  • The place motion could be divided into two parts: approach, and sensitive place. The first part could be made faster, while the second could increase the compliance values.

  • Parameters passed to the states can be integrated with the Task-wide settings, accessible when the Task name is pressed in Desk.